Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

more bt* to b3*

Browse Source
This commit is contained in:
erwin coumans
2013-04-16 17:08:59 -07:00
parent faabffc23d
commit e646754228
116 changed files with 2466 additions and 3034 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
src/BulletCommon/btAlignedAllocator.cpp → src/BulletCommon/b3AlignedAllocator.cpp
View File

@@ -13,7 +13,7 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
#include "btAlignedAllocator.h"
#include "b3AlignedAllocator.h"
int gNumAlignedAllocs = 0;
int gNumAlignedFree = 0;

18
src/BulletCommon/btAlignedAllocator.h → src/BulletCommon/b3AlignedAllocator.h
View File

@@ -20,7 +20,7 @@ subject to the following restrictions:
///so we replace _aligned_malloc and _aligned_free with our own
///that is better portable and more predictable
#include "btScalar.h"
#include "b3Scalar.h"
//#define BT_DEBUG_MEMORY_ALLOCATIONS 1
#ifdef BT_DEBUG_MEMORY_ALLOCATIONS
@@ -55,23 +55,23 @@ void btAlignedAllocSetCustom(btAllocFunc *allocFunc, btFreeFunc *freeFunc);
void btAlignedAllocSetCustomAligned(btAlignedAllocFunc *allocFunc, btAlignedFreeFunc *freeFunc);
///The btAlignedAllocator is a portable class for aligned memory allocations.
///The b3AlignedAllocator is a portable class for aligned memory allocations.
///Default implementations for unaligned and aligned allocations can be overridden by a custom allocator using btAlignedAllocSetCustom and btAlignedAllocSetCustomAligned.
template < typename T , unsigned Alignment >
class btAlignedAllocator {
class b3AlignedAllocator {
typedef btAlignedAllocator< T , Alignment > self_type;
typedef b3AlignedAllocator< T , Alignment > self_type;
public:
//just going down a list:
btAlignedAllocator() {}
b3AlignedAllocator() {}
/*
btAlignedAllocator( const self_type & ) {}
b3AlignedAllocator( const self_type & ) {}
*/
template < typename Other >
btAlignedAllocator( const btAlignedAllocator< Other , Alignment > & ) {}
b3AlignedAllocator( const b3AlignedAllocator< Other , Alignment > & ) {}
typedef const T* const_pointer;
typedef const T& const_reference;
@@ -93,10 +93,10 @@ public:
template < typename O > struct rebind {
typedef btAlignedAllocator< O , Alignment > other;
typedef b3AlignedAllocator< O , Alignment > other;
};
template < typename O >
self_type & operator=( const btAlignedAllocator< O , Alignment > & ) { return *this; }
self_type & operator=( const b3AlignedAllocator< O , Alignment > & ) { return *this; }
friend bool operator==( const self_type & , const self_type & ) { return true; }
};

26
src/BulletCommon/btAlignedObjectArray.h → src/BulletCommon/b3AlignedObjectArray.h
View File

@@ -17,11 +17,11 @@ subject to the following restrictions:
#ifndef BT_OBJECT_ARRAY__
#define BT_OBJECT_ARRAY__
#include "btScalar.h" // has definitions like SIMD_FORCE_INLINE
#include "btAlignedAllocator.h"
#include "b3Scalar.h" // has definitions like SIMD_FORCE_INLINE
#include "b3AlignedAllocator.h"
///If the platform doesn't support placement new, you can disable BT_USE_PLACEMENT_NEW
///then the btAlignedObjectArray doesn't support objects with virtual methods, and non-trivial constructors/destructors
///then the b3AlignedObjectArray doesn't support objects with virtual methods, and non-trivial constructors/destructors
///You can enable BT_USE_MEMCPY, then swapping elements in the array will use memcpy instead of operator=
///see discussion here: http://continuousphysics.com/Bullet/phpBB2/viewtopic.php?t=1231 and
///http://www.continuousphysics.com/Bullet/phpBB2/viewtopic.php?t=1240
@@ -40,13 +40,13 @@ subject to the following restrictions:
#endif //BT_USE_PLACEMENT_NEW
///The btAlignedObjectArray template class uses a subset of the stl::vector interface for its methods
///The b3AlignedObjectArray template class uses a subset of the stl::vector interface for its methods
///It is developed to replace stl::vector to avoid portability issues, including STL alignment issues to add SIMD/SSE data
template <typename T>
//template <class T>
class btAlignedObjectArray
class b3AlignedObjectArray
{
btAlignedAllocator<T , 16> m_allocator;
b3AlignedAllocator<T , 16> m_allocator;
int m_size;
int m_capacity;
@@ -56,14 +56,14 @@ class btAlignedObjectArray
#ifdef BT_ALLOW_ARRAY_COPY_OPERATOR
public:
SIMD_FORCE_INLINE btAlignedObjectArray<T>& operator=(const btAlignedObjectArray<T> &other)
SIMD_FORCE_INLINE b3AlignedObjectArray<T>& operator=(const b3AlignedObjectArray<T> &other)
{
copyFromArray(other);
return *this;
}
#else//BT_ALLOW_ARRAY_COPY_OPERATOR
private:
SIMD_FORCE_INLINE btAlignedObjectArray<T>& operator=(const btAlignedObjectArray<T> &other);
SIMD_FORCE_INLINE b3AlignedObjectArray<T>& operator=(const b3AlignedObjectArray<T> &other);
#endif//BT_ALLOW_ARRAY_COPY_OPERATOR
protected:
@@ -123,18 +123,18 @@ protected:
public:
btAlignedObjectArray()
b3AlignedObjectArray()
{
init();
}
~btAlignedObjectArray()
~b3AlignedObjectArray()
{
clear();
}
///Generally it is best to avoid using the copy constructor of an btAlignedObjectArray, and use a (const) reference to the array instead.
btAlignedObjectArray(const btAlignedObjectArray& otherArray)
///Generally it is best to avoid using the copy constructor of an b3AlignedObjectArray, and use a (const) reference to the array instead.
b3AlignedObjectArray(const b3AlignedObjectArray& otherArray)
{
init();
@@ -500,7 +500,7 @@ protected:
m_capacity = capacity;
}
void copyFromArray(const btAlignedObjectArray& otherArray)
void copyFromArray(const b3AlignedObjectArray& otherArray)
{
int otherSize = otherArray.size();
resize (otherSize);

8
src/BulletCommon/CommandLineArgs.h → src/BulletCommon/b3CommandLineArgs.h
View File

@@ -9,7 +9,7 @@
#include <string>
#include <cstring>
#include <sstream>
class CommandLineArgs
class b3CommandLineArgs
{
protected:
@@ -18,7 +18,7 @@ protected:
public:
// Constructor
CommandLineArgs(int argc, char **argv)
b3CommandLineArgs(int argc, char **argv)
{
using namespace std;
@@ -63,7 +63,7 @@ public:
};
template <typename T>
void CommandLineArgs::GetCmdLineArgument(const char *arg_name, T &val)
void b3CommandLineArgs::GetCmdLineArgument(const char *arg_name, T &val)
{
using namespace std;
map<string, string>::iterator itr;
@@ -74,7 +74,7 @@ void CommandLineArgs::GetCmdLineArgument(const char *arg_name, T &val)
}
template <>
void CommandLineArgs::GetCmdLineArgument<char*>(const char* arg_name, char* &val)
void b3CommandLineArgs::GetCmdLineArgument<char*>(const char* arg_name, char* &val)
{
using namespace std;
map<string, string>::iterator itr;

18
src/BulletCommon/btHashMap.h → src/BulletCommon/b3HashMap.h
View File

@@ -17,9 +17,9 @@ subject to the following restrictions:
#ifndef BT_HASH_MAP_H
#define BT_HASH_MAP_H
#include "btAlignedObjectArray.h"
#include "b3AlignedObjectArray.h"
///very basic hashable string implementation, compatible with btHashMap
///very basic hashable string implementation, compatible with b3HashMap
struct btHashString
{
const char* m_string;
@@ -214,18 +214,18 @@ public:
};
///The btHashMap template class implements a generic and lightweight hashmap.
///A basic sample of how to use btHashMap is located in Demos\BasicDemo\main.cpp
///The b3HashMap template class implements a generic and lightweight hashmap.
///A basic sample of how to use b3HashMap is located in Demos\BasicDemo\main.cpp
template <class Key, class Value>
class btHashMap
class b3HashMap
{
protected:
btAlignedObjectArray<int> m_hashTable;
btAlignedObjectArray<int> m_next;
b3AlignedObjectArray<int> m_hashTable;
b3AlignedObjectArray<int> m_next;
btAlignedObjectArray<Value> m_valueArray;
btAlignedObjectArray<Key> m_keyArray;
b3AlignedObjectArray<Value> m_valueArray;
b3AlignedObjectArray<Key> m_keyArray;
void growTables(const Key& /*key*/)
{

422
src/BulletCommon/btMatrix3x3.h → src/BulletCommon/b3Matrix3x3.h
View File

@@ -16,8 +16,8 @@ subject to the following restrictions:
#ifndef BT_MATRIX3x3_H
#define BT_MATRIX3x3_H
#include "btVector3.h"
#include "btQuaternion.h"
#include "b3Vector3.h"
#include "b3Quaternion.h"
#include <stdio.h>
#ifdef BT_USE_SSE
@@ -38,32 +38,32 @@ const btSimdFloat4 ATTRIBUTE_ALIGNED16(v0010) = {0.0f, 0.0f, 1.0f, 0.0f};
#endif //BT_USE_DOUBLE_PRECISION
/**@brief The btMatrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with btQuaternion, btTransform and btVector3.
/**@brief The b3Matrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with b3Quaternion, b3Transform and b3Vector3.
* Make sure to only include a pure orthogonal matrix without scaling. */
ATTRIBUTE_ALIGNED16(class) btMatrix3x3 {
ATTRIBUTE_ALIGNED16(class) b3Matrix3x3 {
///Data storage for the matrix, each vector is a row of the matrix
btVector3 m_el[3];
b3Vector3 m_el[3];
public:
/** @brief No initializaion constructor */
btMatrix3x3 () {}
b3Matrix3x3 () {}
// explicit btMatrix3x3(const btScalar *m) { setFromOpenGLSubMatrix(m); }
// explicit b3Matrix3x3(const b3Scalar *m) { setFromOpenGLSubMatrix(m); }
/**@brief Constructor from Quaternion */
explicit btMatrix3x3(const btQuaternion& q) { setRotation(q); }
explicit b3Matrix3x3(const b3Quaternion& q) { setRotation(q); }
/*
template <typename btScalar>
Matrix3x3(const btScalar& yaw, const btScalar& pitch, const btScalar& roll)
template <typename b3Scalar>
Matrix3x3(const b3Scalar& yaw, const b3Scalar& pitch, const b3Scalar& roll)
{
setEulerYPR(yaw, pitch, roll);
}
*/
/** @brief Constructor with row major formatting */
btMatrix3x3(const btScalar& xx, const btScalar& xy, const btScalar& xz,
const btScalar& yx, const btScalar& yy, const btScalar& yz,
const btScalar& zx, const btScalar& zy, const btScalar& zz)
b3Matrix3x3(const b3Scalar& xx, const b3Scalar& xy, const b3Scalar& xz,
const b3Scalar& yx, const b3Scalar& yy, const b3Scalar& yz,
const b3Scalar& zx, const b3Scalar& zy, const b3Scalar& zz)
{
setValue(xx, xy, xz,
yx, yy, yz,
@@ -71,14 +71,14 @@ public:
}
#if (defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE))|| defined (BT_USE_NEON)
SIMD_FORCE_INLINE btMatrix3x3 (const btSimdFloat4 v0, const btSimdFloat4 v1, const btSimdFloat4 v2 )
SIMD_FORCE_INLINE b3Matrix3x3 (const btSimdFloat4 v0, const btSimdFloat4 v1, const btSimdFloat4 v2 )
{
m_el[0].mVec128 = v0;
m_el[1].mVec128 = v1;
m_el[2].mVec128 = v2;
}
SIMD_FORCE_INLINE btMatrix3x3 (const btVector3& v0, const btVector3& v1, const btVector3& v2 )
SIMD_FORCE_INLINE b3Matrix3x3 (const b3Vector3& v0, const b3Vector3& v1, const b3Vector3& v2 )
{
m_el[0] = v0;
m_el[1] = v1;
@@ -86,7 +86,7 @@ public:
}
// Copy constructor
SIMD_FORCE_INLINE btMatrix3x3(const btMatrix3x3& rhs)
SIMD_FORCE_INLINE b3Matrix3x3(const b3Matrix3x3& rhs)
{
m_el[0].mVec128 = rhs.m_el[0].mVec128;
m_el[1].mVec128 = rhs.m_el[1].mVec128;
@@ -94,7 +94,7 @@ public:
}
// Assignment Operator
SIMD_FORCE_INLINE btMatrix3x3& operator=(const btMatrix3x3& m)
SIMD_FORCE_INLINE b3Matrix3x3& operator=(const b3Matrix3x3& m)
{
m_el[0].mVec128 = m.m_el[0].mVec128;
m_el[1].mVec128 = m.m_el[1].mVec128;
@@ -106,7 +106,7 @@ public:
#else
/** @brief Copy constructor */
SIMD_FORCE_INLINE btMatrix3x3 (const btMatrix3x3& other)
SIMD_FORCE_INLINE b3Matrix3x3 (const b3Matrix3x3& other)
{
m_el[0] = other.m_el[0];
m_el[1] = other.m_el[1];
@@ -114,7 +114,7 @@ public:
}
/** @brief Assignment Operator */
SIMD_FORCE_INLINE btMatrix3x3& operator=(const btMatrix3x3& other)
SIMD_FORCE_INLINE b3Matrix3x3& operator=(const b3Matrix3x3& other)
{
m_el[0] = other.m_el[0];
m_el[1] = other.m_el[1];
@@ -126,15 +126,15 @@ public:
/** @brief Get a column of the matrix as a vector
* @param i Column number 0 indexed */
SIMD_FORCE_INLINE btVector3 getColumn(int i) const
SIMD_FORCE_INLINE b3Vector3 getColumn(int i) const
{
return btVector3(m_el[0][i],m_el[1][i],m_el[2][i]);
return b3Vector3(m_el[0][i],m_el[1][i],m_el[2][i]);
}
/** @brief Get a row of the matrix as a vector
* @param i Row number 0 indexed */
SIMD_FORCE_INLINE const btVector3& getRow(int i) const
SIMD_FORCE_INLINE const b3Vector3& getRow(int i) const
{
btFullAssert(0 <= i && i < 3);
return m_el[i];
@@ -142,7 +142,7 @@ public:
/** @brief Get a mutable reference to a row of the matrix as a vector
* @param i Row number 0 indexed */
SIMD_FORCE_INLINE btVector3& operator[](int i)
SIMD_FORCE_INLINE b3Vector3& operator[](int i)
{
btFullAssert(0 <= i && i < 3);
return m_el[i];
@@ -150,7 +150,7 @@ public:
/** @brief Get a const reference to a row of the matrix as a vector
* @param i Row number 0 indexed */
SIMD_FORCE_INLINE const btVector3& operator[](int i) const
SIMD_FORCE_INLINE const b3Vector3& operator[](int i) const
{
btFullAssert(0 <= i && i < 3);
return m_el[i];
@@ -159,21 +159,21 @@ public:
/** @brief Multiply by the target matrix on the right
* @param m Rotation matrix to be applied
* Equivilant to this = this * m */
btMatrix3x3& operator*=(const btMatrix3x3& m);
b3Matrix3x3& operator*=(const b3Matrix3x3& m);
/** @brief Adds by the target matrix on the right
* @param m matrix to be applied
* Equivilant to this = this + m */
btMatrix3x3& operator+=(const btMatrix3x3& m);
b3Matrix3x3& operator+=(const b3Matrix3x3& m);
/** @brief Substractss by the target matrix on the right
* @param m matrix to be applied
* Equivilant to this = this - m */
btMatrix3x3& operator-=(const btMatrix3x3& m);
b3Matrix3x3& operator-=(const b3Matrix3x3& m);
/** @brief Set from the rotational part of a 4x4 OpenGL matrix
* @param m A pointer to the beginning of the array of scalars*/
void setFromOpenGLSubMatrix(const btScalar *m)
void setFromOpenGLSubMatrix(const b3Scalar *m)
{
m_el[0].setValue(m[0],m[4],m[8]);
m_el[1].setValue(m[1],m[5],m[9]);
@@ -190,9 +190,9 @@ public:
* @param zx Bottom Left
* @param zy Bottom Middle
* @param zz Bottom Right*/
void setValue(const btScalar& xx, const btScalar& xy, const btScalar& xz,
const btScalar& yx, const btScalar& yy, const btScalar& yz,
const btScalar& zx, const btScalar& zy, const btScalar& zz)
void setValue(const b3Scalar& xx, const b3Scalar& xy, const b3Scalar& xz,
const b3Scalar& yx, const b3Scalar& yy, const b3Scalar& yz,
const b3Scalar& zx, const b3Scalar& zy, const b3Scalar& zz)
{
m_el[0].setValue(xx,xy,xz);
m_el[1].setValue(yx,yy,yz);
@@ -201,11 +201,11 @@ public:
/** @brief Set the matrix from a quaternion
* @param q The Quaternion to match */
void setRotation(const btQuaternion& q)
void setRotation(const b3Quaternion& q)
{
btScalar d = q.length2();
btFullAssert(d != btScalar(0.0));
btScalar s = btScalar(2.0) / d;
b3Scalar d = q.length2();
btFullAssert(d != b3Scalar(0.0));
b3Scalar s = b3Scalar(2.0) / d;
#if defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
__m128 vs, Q = q.get128();
@@ -259,14 +259,14 @@ public:
m_el[1] = V2;
m_el[2] = V3;
#else
btScalar xs = q.getX() * s, ys = q.getY() * s, zs = q.getZ() * s;
btScalar wx = q.getW() * xs, wy = q.getW() * ys, wz = q.getW() * zs;
btScalar xx = q.getX() * xs, xy = q.getX() * ys, xz = q.getX() * zs;
btScalar yy = q.getY() * ys, yz = q.getY() * zs, zz = q.getZ() * zs;
b3Scalar xs = q.getX() * s, ys = q.getY() * s, zs = q.getZ() * s;
b3Scalar wx = q.getW() * xs, wy = q.getW() * ys, wz = q.getW() * zs;
b3Scalar xx = q.getX() * xs, xy = q.getX() * ys, xz = q.getX() * zs;
b3Scalar yy = q.getY() * ys, yz = q.getY() * zs, zz = q.getZ() * zs;
setValue(
btScalar(1.0) - (yy + zz), xy - wz, xz + wy,
xy + wz, btScalar(1.0) - (xx + zz), yz - wx,
xz - wy, yz + wx, btScalar(1.0) - (xx + yy));
b3Scalar(1.0) - (yy + zz), xy - wz, xz + wy,
xy + wz, b3Scalar(1.0) - (xx + zz), yz - wx,
xz - wy, yz + wx, b3Scalar(1.0) - (xx + yy));
#endif
}
@@ -276,7 +276,7 @@ public:
* @param pitch Pitch about X axis
* @param roll Roll about Z axis
*/
void setEulerYPR(const btScalar& yaw, const btScalar& pitch, const btScalar& roll)
void setEulerYPR(const b3Scalar& yaw, const b3Scalar& pitch, const b3Scalar& roll)
{
setEulerZYX(roll, pitch, yaw);
}
@@ -290,18 +290,18 @@ public:
* angles are applied in ZYX order. I.e a vector is first rotated
* about X then Y and then Z
**/
void setEulerZYX(btScalar eulerX,btScalar eulerY,btScalar eulerZ) {
void setEulerZYX(b3Scalar eulerX,b3Scalar eulerY,b3Scalar eulerZ) {
///@todo proposed to reverse this since it's labeled zyx but takes arguments xyz and it will match all other parts of the code
btScalar ci ( btCos(eulerX));
btScalar cj ( btCos(eulerY));
btScalar ch ( btCos(eulerZ));
btScalar si ( btSin(eulerX));
btScalar sj ( btSin(eulerY));
btScalar sh ( btSin(eulerZ));
btScalar cc = ci * ch;
btScalar cs = ci * sh;
btScalar sc = si * ch;
btScalar ss = si * sh;
b3Scalar ci ( btCos(eulerX));
b3Scalar cj ( btCos(eulerY));
b3Scalar ch ( btCos(eulerZ));
b3Scalar si ( btSin(eulerX));
b3Scalar sj ( btSin(eulerY));
b3Scalar sh ( btSin(eulerZ));
b3Scalar cc = ci * ch;
b3Scalar cs = ci * sh;
b3Scalar sc = si * ch;
b3Scalar ss = si * sh;
setValue(cj * ch, sj * sc - cs, sj * cc + ss,
cj * sh, sj * ss + cc, sj * cs - sc,
@@ -316,30 +316,30 @@ public:
m_el[1] = v0100;
m_el[2] = v0010;
#else
setValue(btScalar(1.0), btScalar(0.0), btScalar(0.0),
btScalar(0.0), btScalar(1.0), btScalar(0.0),
btScalar(0.0), btScalar(0.0), btScalar(1.0));
setValue(b3Scalar(1.0), b3Scalar(0.0), b3Scalar(0.0),
b3Scalar(0.0), b3Scalar(1.0), b3Scalar(0.0),
b3Scalar(0.0), b3Scalar(0.0), b3Scalar(1.0));
#endif
}
static const btMatrix3x3& getIdentity()
static const b3Matrix3x3& getIdentity()
{
#if (defined(BT_USE_SSE_IN_API)&& defined (BT_USE_SSE)) || defined(BT_USE_NEON)
static const btMatrix3x3
static const b3Matrix3x3
identityMatrix(v1000, v0100, v0010);
#else
static const btMatrix3x3
static const b3Matrix3x3
identityMatrix(
btScalar(1.0), btScalar(0.0), btScalar(0.0),
btScalar(0.0), btScalar(1.0), btScalar(0.0),
btScalar(0.0), btScalar(0.0), btScalar(1.0));
b3Scalar(1.0), b3Scalar(0.0), b3Scalar(0.0),
b3Scalar(0.0), b3Scalar(1.0), b3Scalar(0.0),
b3Scalar(0.0), b3Scalar(0.0), b3Scalar(1.0));
#endif
return identityMatrix;
}
/**@brief Fill the rotational part of an OpenGL matrix and clear the shear/perspective
* @param m The array to be filled */
void getOpenGLSubMatrix(btScalar *m) const
void getOpenGLSubMatrix(b3Scalar *m) const
{
#if defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
__m128 v0 = m_el[0].mVec128;
@@ -375,43 +375,43 @@ public:
vm[1] = v1;
vm[2] = v2;
#else
m[0] = btScalar(m_el[0].getX());
m[1] = btScalar(m_el[1].getX());
m[2] = btScalar(m_el[2].getX());
m[3] = btScalar(0.0);
m[4] = btScalar(m_el[0].getY());
m[5] = btScalar(m_el[1].getY());
m[6] = btScalar(m_el[2].getY());
m[7] = btScalar(0.0);
m[8] = btScalar(m_el[0].getZ());
m[9] = btScalar(m_el[1].getZ());
m[10] = btScalar(m_el[2].getZ());
m[11] = btScalar(0.0);
m[0] = b3Scalar(m_el[0].getX());
m[1] = b3Scalar(m_el[1].getX());
m[2] = b3Scalar(m_el[2].getX());
m[3] = b3Scalar(0.0);
m[4] = b3Scalar(m_el[0].getY());
m[5] = b3Scalar(m_el[1].getY());
m[6] = b3Scalar(m_el[2].getY());
m[7] = b3Scalar(0.0);
m[8] = b3Scalar(m_el[0].getZ());
m[9] = b3Scalar(m_el[1].getZ());
m[10] = b3Scalar(m_el[2].getZ());
m[11] = b3Scalar(0.0);
#endif
}
/**@brief Get the matrix represented as a quaternion
* @param q The quaternion which will be set */
void getRotation(btQuaternion& q) const
void getRotation(b3Quaternion& q) const
{
#if (defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE))|| defined (BT_USE_NEON)
btScalar trace = m_el[0].getX() + m_el[1].getY() + m_el[2].getZ();
btScalar s, x;
b3Scalar trace = m_el[0].getX() + m_el[1].getY() + m_el[2].getZ();
b3Scalar s, x;
union {
btSimdFloat4 vec;
btScalar f[4];
b3Scalar f[4];
} temp;
if (trace > btScalar(0.0))
if (trace > b3Scalar(0.0))
{
x = trace + btScalar(1.0);
x = trace + b3Scalar(1.0);
temp.f[0]=m_el[2].getY() - m_el[1].getZ();
temp.f[1]=m_el[0].getZ() - m_el[2].getX();
temp.f[2]=m_el[1].getX() - m_el[0].getY();
temp.f[3]=x;
//temp.f[3]= s * btScalar(0.5);
//temp.f[3]= s * b3Scalar(0.5);
}
else
{
@@ -431,30 +431,30 @@ public:
{ i = 0; j = 1; k = 2; }
}
x = m_el[i][i] - m_el[j][j] - m_el[k][k] + btScalar(1.0);
x = m_el[i][i] - m_el[j][j] - m_el[k][k] + b3Scalar(1.0);
temp.f[3] = (m_el[k][j] - m_el[j][k]);
temp.f[j] = (m_el[j][i] + m_el[i][j]);
temp.f[k] = (m_el[k][i] + m_el[i][k]);
temp.f[i] = x;
//temp.f[i] = s * btScalar(0.5);
//temp.f[i] = s * b3Scalar(0.5);
}
s = btSqrt(x);
q.set128(temp.vec);
s = btScalar(0.5) / s;
s = b3Scalar(0.5) / s;
q *= s;
#else
btScalar trace = m_el[0].getX() + m_el[1].getY() + m_el[2].getZ();
b3Scalar trace = m_el[0].getX() + m_el[1].getY() + m_el[2].getZ();
btScalar temp[4];
b3Scalar temp[4];
if (trace > btScalar(0.0))
if (trace > b3Scalar(0.0))
{
btScalar s = btSqrt(trace + btScalar(1.0));
temp[3]=(s * btScalar(0.5));
s = btScalar(0.5) / s;
b3Scalar s = btSqrt(trace + b3Scalar(1.0));
temp[3]=(s * b3Scalar(0.5));
s = b3Scalar(0.5) / s;
temp[0]=((m_el[2].getY() - m_el[1].getZ()) * s);
temp[1]=((m_el[0].getZ() - m_el[2].getX()) * s);
@@ -468,9 +468,9 @@ public:
int j = (i + 1) % 3;
int k = (i + 2) % 3;
btScalar s = btSqrt(m_el[i][i] - m_el[j][j] - m_el[k][k] + btScalar(1.0));
temp[i] = s * btScalar(0.5);
s = btScalar(0.5) / s;
b3Scalar s = btSqrt(m_el[i][i] - m_el[j][j] - m_el[k][k] + b3Scalar(1.0));
temp[i] = s * b3Scalar(0.5);
s = b3Scalar(0.5) / s;
temp[3] = (m_el[k][j] - m_el[j][k]) * s;
temp[j] = (m_el[j][i] + m_el[i][j]) * s;
@@ -484,13 +484,13 @@ public:
* @param yaw Yaw around Y axis
* @param pitch Pitch around X axis
* @param roll around Z axis */
void getEulerYPR(btScalar& yaw, btScalar& pitch, btScalar& roll) const
void getEulerYPR(b3Scalar& yaw, b3Scalar& pitch, b3Scalar& roll) const
{
// first use the normal calculus
yaw = btScalar(btAtan2(m_el[1].getX(), m_el[0].getX()));
pitch = btScalar(btAsin(-m_el[2].getX()));
roll = btScalar(btAtan2(m_el[2].getY(), m_el[2].getZ()));
yaw = b3Scalar(btAtan2(m_el[1].getX(), m_el[0].getX()));
pitch = b3Scalar(btAsin(-m_el[2].getX()));
roll = b3Scalar(btAtan2(m_el[2].getY(), m_el[2].getZ()));
// on pitch = +/-HalfPI
if (btFabs(pitch)==SIMD_HALF_PI)
@@ -513,13 +513,13 @@ public:
* @param pitch Pitch around Y axis
* @param roll around X axis
* @param solution_number Which solution of two possible solutions ( 1 or 2) are possible values*/
void getEulerZYX(btScalar& yaw, btScalar& pitch, btScalar& roll, unsigned int solution_number = 1) const
void getEulerZYX(b3Scalar& yaw, b3Scalar& pitch, b3Scalar& roll, unsigned int solution_number = 1) const
{
struct Euler
{
btScalar yaw;
btScalar pitch;
btScalar roll;
b3Scalar yaw;
b3Scalar pitch;
b3Scalar roll;
};
Euler euler_out;
@@ -533,18 +533,18 @@ public:
euler_out2.yaw = 0;
// From difference of angles formula
btScalar delta = btAtan2(m_el[0].getX(),m_el[0].getZ());
b3Scalar delta = btAtan2(m_el[0].getX(),m_el[0].getZ());
if (m_el[2].getX() > 0) //gimbal locked up
{
euler_out.pitch = SIMD_PI / btScalar(2.0);
euler_out2.pitch = SIMD_PI / btScalar(2.0);
euler_out.pitch = SIMD_PI / b3Scalar(2.0);
euler_out2.pitch = SIMD_PI / b3Scalar(2.0);
euler_out.roll = euler_out.pitch + delta;
euler_out2.roll = euler_out.pitch + delta;
}
else // gimbal locked down
{
euler_out.pitch = -SIMD_PI / btScalar(2.0);
euler_out2.pitch = -SIMD_PI / btScalar(2.0);
euler_out.pitch = -SIMD_PI / b3Scalar(2.0);
euler_out2.pitch = -SIMD_PI / b3Scalar(2.0);
euler_out.roll = -euler_out.pitch + delta;
euler_out2.roll = -euler_out.pitch + delta;
}
@@ -582,12 +582,12 @@ public:
/**@brief Create a scaled copy of the matrix
* @param s Scaling vector The elements of the vector will scale each column */
btMatrix3x3 scaled(const btVector3& s) const
b3Matrix3x3 scaled(const b3Vector3& s) const
{
#if (defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE))|| defined (BT_USE_NEON)
return btMatrix3x3(m_el[0] * s, m_el[1] * s, m_el[2] * s);
return b3Matrix3x3(m_el[0] * s, m_el[1] * s, m_el[2] * s);
#else
return btMatrix3x3(
return b3Matrix3x3(
m_el[0].getX() * s.getX(), m_el[0].getY() * s.getY(), m_el[0].getZ() * s.getZ(),
m_el[1].getX() * s.getX(), m_el[1].getY() * s.getY(), m_el[1].getZ() * s.getZ(),
m_el[2].getX() * s.getX(), m_el[2].getY() * s.getY(), m_el[2].getZ() * s.getZ());
@@ -595,28 +595,28 @@ public:
}
/**@brief Return the determinant of the matrix */
btScalar determinant() const;
b3Scalar determinant() const;
/**@brief Return the adjoint of the matrix */
btMatrix3x3 adjoint() const;
b3Matrix3x3 adjoint() const;
/**@brief Return the matrix with all values non negative */
btMatrix3x3 absolute() const;
b3Matrix3x3 absolute() const;
/**@brief Return the transpose of the matrix */
btMatrix3x3 transpose() const;
b3Matrix3x3 transpose() const;
/**@brief Return the inverse of the matrix */
btMatrix3x3 inverse() const;
b3Matrix3x3 inverse() const;
btMatrix3x3 transposeTimes(const btMatrix3x3& m) const;
btMatrix3x3 timesTranspose(const btMatrix3x3& m) const;
b3Matrix3x3 transposeTimes(const b3Matrix3x3& m) const;
b3Matrix3x3 timesTranspose(const b3Matrix3x3& m) const;
SIMD_FORCE_INLINE btScalar tdotx(const btVector3& v) const
SIMD_FORCE_INLINE b3Scalar tdotx(const b3Vector3& v) const
{
return m_el[0].getX() * v.getX() + m_el[1].getX() * v.getY() + m_el[2].getX() * v.getZ();
}
SIMD_FORCE_INLINE btScalar tdoty(const btVector3& v) const
SIMD_FORCE_INLINE b3Scalar tdoty(const b3Vector3& v) const
{
return m_el[0].getY() * v.getX() + m_el[1].getY() * v.getY() + m_el[2].getY() * v.getZ();
}
SIMD_FORCE_INLINE btScalar tdotz(const btVector3& v) const
SIMD_FORCE_INLINE b3Scalar tdotz(const b3Vector3& v) const
{
return m_el[0].getZ() * v.getX() + m_el[1].getZ() * v.getY() + m_el[2].getZ() * v.getZ();
}
@@ -631,7 +631,7 @@ public:
*
* Note that this matrix is assumed to be symmetric.
*/
void diagonalize(btMatrix3x3& rot, btScalar threshold, int maxSteps)
void diagonalize(b3Matrix3x3& rot, b3Scalar threshold, int maxSteps)
{
rot.setIdentity();
for (int step = maxSteps; step > 0; step--)
@@ -640,8 +640,8 @@ public:
int p = 0;
int q = 1;
int r = 2;
btScalar max = btFabs(m_el[0][1]);
btScalar v = btFabs(m_el[0][2]);
b3Scalar max = btFabs(m_el[0][1]);
b3Scalar v = btFabs(m_el[0][2]);
if (v > max)
{
q = 2;
@@ -657,7 +657,7 @@ public:
max = v;
}
btScalar t = threshold * (btFabs(m_el[0][0]) + btFabs(m_el[1][1]) + btFabs(m_el[2][2]));
b3Scalar t = threshold * (btFabs(m_el[0][0]) + btFabs(m_el[1][1]) + btFabs(m_el[2][2]));
if (max <= t)
{
if (max <= SIMD_EPSILON * t)
@@ -668,12 +668,12 @@ public:
}
// compute Jacobi rotation J which leads to a zero for element [p][q]
btScalar mpq = m_el[p][q];
btScalar theta = (m_el[q][q] - m_el[p][p]) / (2 * mpq);
btScalar theta2 = theta * theta;
btScalar cos;
btScalar sin;
if (theta2 * theta2 < btScalar(10 / SIMD_EPSILON))
b3Scalar mpq = m_el[p][q];
b3Scalar theta = (m_el[q][q] - m_el[p][p]) / (2 * mpq);
b3Scalar theta2 = theta * theta;
b3Scalar cos;
b3Scalar sin;
if (theta2 * theta2 < b3Scalar(10 / SIMD_EPSILON))
{
t = (theta >= 0) ? 1 / (theta + btSqrt(1 + theta2))
: 1 / (theta - btSqrt(1 + theta2));
@@ -683,8 +683,8 @@ public:
else
{
// approximation for large theta-value, i.e., a nearly diagonal matrix
t = 1 / (theta * (2 + btScalar(0.5) / theta2));
cos = 1 - btScalar(0.5) * t * t;
t = 1 / (theta * (2 + b3Scalar(0.5) / theta2));
cos = 1 - b3Scalar(0.5) * t * t;
sin = cos * t;
}
@@ -692,15 +692,15 @@ public:
m_el[p][q] = m_el[q][p] = 0;
m_el[p][p] -= t * mpq;
m_el[q][q] += t * mpq;
btScalar mrp = m_el[r][p];
btScalar mrq = m_el[r][q];
b3Scalar mrp = m_el[r][p];
b3Scalar mrq = m_el[r][q];
m_el[r][p] = m_el[p][r] = cos * mrp - sin * mrq;
m_el[r][q] = m_el[q][r] = cos * mrq + sin * mrp;
// apply rotation to rot (rot = rot * J)
for (int i = 0; i < 3; i++)
{
btVector3& row = rot[i];
b3Vector3& row = rot[i];
mrp = row[p];
mrq = row[q];
row[p] = cos * mrp - sin * mrq;
@@ -719,7 +719,7 @@ public:
* @param c1 The second column to use for calculating the cofactor
* See http://en.wikipedia.org/wiki/Cofactor_(linear_algebra) for more details
*/
btScalar cofac(int r1, int c1, int r2, int c2) const
b3Scalar cofac(int r1, int c1, int r2, int c2) const
{
return m_el[r1][c1] * m_el[r2][c2] - m_el[r1][c2] * m_el[r2][c1];
}
@@ -737,8 +737,8 @@ public:
};
SIMD_FORCE_INLINE btMatrix3x3&
btMatrix3x3::operator*=(const btMatrix3x3& m)
SIMD_FORCE_INLINE b3Matrix3x3&
b3Matrix3x3::operator*=(const b3Matrix3x3& m)
{
#if defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
__m128 rv00, rv01, rv02;
@@ -827,8 +827,8 @@ btMatrix3x3::operator*=(const btMatrix3x3& m)
return *this;
}
SIMD_FORCE_INLINE btMatrix3x3&
btMatrix3x3::operator+=(const btMatrix3x3& m)
SIMD_FORCE_INLINE b3Matrix3x3&
b3Matrix3x3::operator+=(const b3Matrix3x3& m)
{
#if (defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE))|| defined (BT_USE_NEON)
m_el[0].mVec128 = m_el[0].mVec128 + m.m_el[0].mVec128;
@@ -849,38 +849,38 @@ btMatrix3x3::operator+=(const btMatrix3x3& m)
return *this;
}
SIMD_FORCE_INLINE btMatrix3x3
operator*(const btMatrix3x3& m, const btScalar & k)
SIMD_FORCE_INLINE b3Matrix3x3
operator*(const b3Matrix3x3& m, const b3Scalar & k)
{
#if (defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE))
__m128 vk = bt_splat_ps(_mm_load_ss((float *)&k), 0x80);
return btMatrix3x3(
return b3Matrix3x3(
_mm_mul_ps(m[0].mVec128, vk),
_mm_mul_ps(m[1].mVec128, vk),
_mm_mul_ps(m[2].mVec128, vk));
#elif defined(BT_USE_NEON)
return btMatrix3x3(
return b3Matrix3x3(
vmulq_n_f32(m[0].mVec128, k),
vmulq_n_f32(m[1].mVec128, k),
vmulq_n_f32(m[2].mVec128, k));
#else
return btMatrix3x3(
return b3Matrix3x3(
m[0].getX()*k,m[0].getY()*k,m[0].getZ()*k,
m[1].getX()*k,m[1].getY()*k,m[1].getZ()*k,
m[2].getX()*k,m[2].getY()*k,m[2].getZ()*k);
#endif
}
SIMD_FORCE_INLINE btMatrix3x3
operator+(const btMatrix3x3& m1, const btMatrix3x3& m2)
SIMD_FORCE_INLINE b3Matrix3x3
operator+(const b3Matrix3x3& m1, const b3Matrix3x3& m2)
{
#if (defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE))|| defined (BT_USE_NEON)
return btMatrix3x3(
return b3Matrix3x3(
m1[0].mVec128 + m2[0].mVec128,
m1[1].mVec128 + m2[1].mVec128,
m1[2].mVec128 + m2[2].mVec128);
#else
return btMatrix3x3(
return b3Matrix3x3(
m1[0][0]+m2[0][0],
m1[0][1]+m2[0][1],
m1[0][2]+m2[0][2],
@@ -895,16 +895,16 @@ operator+(const btMatrix3x3& m1, const btMatrix3x3& m2)
#endif
}
SIMD_FORCE_INLINE btMatrix3x3
operator-(const btMatrix3x3& m1, const btMatrix3x3& m2)
SIMD_FORCE_INLINE b3Matrix3x3
operator-(const b3Matrix3x3& m1, const b3Matrix3x3& m2)
{
#if (defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE))|| defined (BT_USE_NEON)
return btMatrix3x3(
return b3Matrix3x3(
m1[0].mVec128 - m2[0].mVec128,
m1[1].mVec128 - m2[1].mVec128,
m1[2].mVec128 - m2[2].mVec128);
#else
return btMatrix3x3(
return b3Matrix3x3(
m1[0][0]-m2[0][0],
m1[0][1]-m2[0][1],
m1[0][2]-m2[0][2],
@@ -920,8 +920,8 @@ operator-(const btMatrix3x3& m1, const btMatrix3x3& m2)
}
SIMD_FORCE_INLINE btMatrix3x3&
btMatrix3x3::operator-=(const btMatrix3x3& m)
SIMD_FORCE_INLINE b3Matrix3x3&
b3Matrix3x3::operator-=(const b3Matrix3x3& m)
{
#if (defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE))|| defined (BT_USE_NEON)
m_el[0].mVec128 = m_el[0].mVec128 - m.m_el[0].mVec128;
@@ -943,36 +943,36 @@ btMatrix3x3::operator-=(const btMatrix3x3& m)
}
SIMD_FORCE_INLINE btScalar
btMatrix3x3::determinant() const
SIMD_FORCE_INLINE b3Scalar
b3Matrix3x3::determinant() const
{
return btTriple((*this)[0], (*this)[1], (*this)[2]);
}
SIMD_FORCE_INLINE btMatrix3x3
btMatrix3x3::absolute() const
SIMD_FORCE_INLINE b3Matrix3x3
b3Matrix3x3::absolute() const
{
#if (defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE))
return btMatrix3x3(
return b3Matrix3x3(
_mm_and_ps(m_el[0].mVec128, btvAbsfMask),
_mm_and_ps(m_el[1].mVec128, btvAbsfMask),
_mm_and_ps(m_el[2].mVec128, btvAbsfMask));
#elif defined(BT_USE_NEON)
return btMatrix3x3(
return b3Matrix3x3(
(float32x4_t)vandq_s32((int32x4_t)m_el[0].mVec128, btv3AbsMask),
(float32x4_t)vandq_s32((int32x4_t)m_el[1].mVec128, btv3AbsMask),
(float32x4_t)vandq_s32((int32x4_t)m_el[2].mVec128, btv3AbsMask));
#else
return btMatrix3x3(
return b3Matrix3x3(
btFabs(m_el[0].getX()), btFabs(m_el[0].getY()), btFabs(m_el[0].getZ()),
btFabs(m_el[1].getX()), btFabs(m_el[1].getY()), btFabs(m_el[1].getZ()),
btFabs(m_el[2].getX()), btFabs(m_el[2].getY()), btFabs(m_el[2].getZ()));
#endif
}
SIMD_FORCE_INLINE btMatrix3x3
btMatrix3x3::transpose() const
SIMD_FORCE_INLINE b3Matrix3x3
b3Matrix3x3::transpose() const
{
#if (defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE))
__m128 v0 = m_el[0].mVec128;
@@ -990,7 +990,7 @@ btMatrix3x3::transpose() const
v2 = btCastdTo128f(_mm_move_sd(btCastfTo128d(v2), btCastfTo128d(vT))); // z0 z1 z2 0
return btMatrix3x3( v0, v1, v2 );
return b3Matrix3x3( v0, v1, v2 );
#elif defined(BT_USE_NEON)
// note: zeros the w channel. We can preserve it at the cost of two more vtrn instructions.
static const uint32x2_t zMask = (const uint32x2_t) {-1, 0 };
@@ -1000,36 +1000,36 @@ btMatrix3x3::transpose() const
float32x4_t v1 = vcombine_f32( vget_low_f32(top.val[1]), bl.val[1] );
float32x2_t q = (float32x2_t) vand_u32( (uint32x2_t) vget_high_f32( m_el[2].mVec128), zMask );
float32x4_t v2 = vcombine_f32( vget_high_f32(top.val[0]), q ); // z0 z1 z2 0
return btMatrix3x3( v0, v1, v2 );
return b3Matrix3x3( v0, v1, v2 );
#else
return btMatrix3x3( m_el[0].getX(), m_el[1].getX(), m_el[2].getX(),
return b3Matrix3x3( m_el[0].getX(), m_el[1].getX(), m_el[2].getX(),
m_el[0].getY(), m_el[1].getY(), m_el[2].getY(),
m_el[0].getZ(), m_el[1].getZ(), m_el[2].getZ());
#endif
}
SIMD_FORCE_INLINE btMatrix3x3
btMatrix3x3::adjoint() const
SIMD_FORCE_INLINE b3Matrix3x3
b3Matrix3x3::adjoint() const
{
return btMatrix3x3(cofac(1, 1, 2, 2), cofac(0, 2, 2, 1), cofac(0, 1, 1, 2),
return b3Matrix3x3(cofac(1, 1, 2, 2), cofac(0, 2, 2, 1), cofac(0, 1, 1, 2),
cofac(1, 2, 2, 0), cofac(0, 0, 2, 2), cofac(0, 2, 1, 0),
cofac(1, 0, 2, 1), cofac(0, 1, 2, 0), cofac(0, 0, 1, 1));
}
SIMD_FORCE_INLINE btMatrix3x3
btMatrix3x3::inverse() const
SIMD_FORCE_INLINE b3Matrix3x3
b3Matrix3x3::inverse() const
{
btVector3 co(cofac(1, 1, 2, 2), cofac(1, 2, 2, 0), cofac(1, 0, 2, 1));
btScalar det = (*this)[0].dot(co);
btFullAssert(det != btScalar(0.0));
btScalar s = btScalar(1.0) / det;
return btMatrix3x3(co.getX() * s, cofac(0, 2, 2, 1) * s, cofac(0, 1, 1, 2) * s,
b3Vector3 co(cofac(1, 1, 2, 2), cofac(1, 2, 2, 0), cofac(1, 0, 2, 1));
b3Scalar det = (*this)[0].dot(co);
btFullAssert(det != b3Scalar(0.0));
b3Scalar s = b3Scalar(1.0) / det;
return b3Matrix3x3(co.getX() * s, cofac(0, 2, 2, 1) * s, cofac(0, 1, 1, 2) * s,
co.getY() * s, cofac(0, 0, 2, 2) * s, cofac(0, 2, 1, 0) * s,
co.getZ() * s, cofac(0, 1, 2, 0) * s, cofac(0, 0, 1, 1) * s);
}
SIMD_FORCE_INLINE btMatrix3x3
btMatrix3x3::transposeTimes(const btMatrix3x3& m) const
SIMD_FORCE_INLINE b3Matrix3x3
b3Matrix3x3::transposeTimes(const b3Matrix3x3& m) const
{
#if (defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE))
// zeros w
@@ -1049,7 +1049,7 @@ btMatrix3x3::transposeTimes(const btMatrix3x3& m) const
r0 = _mm_add_ps( r0, _mm_mul_ps(m2, _mm_shuffle_ps(row, row, 0)));
r1 = _mm_add_ps( r1, _mm_mul_ps(m2, _mm_shuffle_ps(row, row, 0x55)));
r2 = _mm_add_ps( r2, _mm_mul_ps(m2, _mm_shuffle_ps(row, row, 0xaa)));
return btMatrix3x3( r0, r1, r2 );
return b3Matrix3x3( r0, r1, r2 );
#elif defined BT_USE_NEON
// zeros w
@@ -1069,9 +1069,9 @@ btMatrix3x3::transposeTimes(const btMatrix3x3& m) const
r0 = vmlaq_lane_f32( r0, m2, vget_low_f32(row), 0);
r1 = vmlaq_lane_f32( r1, m2, vget_low_f32(row), 1);
r2 = vmlaq_lane_f32( r2, m2, vget_high_f32(row), 0);
return btMatrix3x3( r0, r1, r2 );
return b3Matrix3x3( r0, r1, r2 );
#else
return btMatrix3x3(
return b3Matrix3x3(
m_el[0].getX() * m[0].getX() + m_el[1].getX() * m[1].getX() + m_el[2].getX() * m[2].getX(),
m_el[0].getX() * m[0].getY() + m_el[1].getX() * m[1].getY() + m_el[2].getX() * m[2].getY(),
m_el[0].getX() * m[0].getZ() + m_el[1].getX() * m[1].getZ() + m_el[2].getX() * m[2].getZ(),
@@ -1084,15 +1084,15 @@ btMatrix3x3::transposeTimes(const btMatrix3x3& m) const
#endif
}
SIMD_FORCE_INLINE btMatrix3x3
btMatrix3x3::timesTranspose(const btMatrix3x3& m) const
SIMD_FORCE_INLINE b3Matrix3x3
b3Matrix3x3::timesTranspose(const b3Matrix3x3& m) const
{
#if (defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE))
__m128 a0 = m_el[0].mVec128;
__m128 a1 = m_el[1].mVec128;
__m128 a2 = m_el[2].mVec128;
btMatrix3x3 mT = m.transpose(); // we rely on transpose() zeroing w channel so that we don't have to do it here
b3Matrix3x3 mT = m.transpose(); // we rely on transpose() zeroing w channel so that we don't have to do it here
__m128 mx = mT[0].mVec128;
__m128 my = mT[1].mVec128;
__m128 mz = mT[2].mVec128;
@@ -1106,14 +1106,14 @@ btMatrix3x3::timesTranspose(const btMatrix3x3& m) const
r0 = _mm_add_ps(r0, _mm_mul_ps(mz, _mm_shuffle_ps(a0, a0, 0xaa)));
r1 = _mm_add_ps(r1, _mm_mul_ps(mz, _mm_shuffle_ps(a1, a1, 0xaa)));
r2 = _mm_add_ps(r2, _mm_mul_ps(mz, _mm_shuffle_ps(a2, a2, 0xaa)));
return btMatrix3x3( r0, r1, r2);
return b3Matrix3x3( r0, r1, r2);
#elif defined BT_USE_NEON
float32x4_t a0 = m_el[0].mVec128;
float32x4_t a1 = m_el[1].mVec128;
float32x4_t a2 = m_el[2].mVec128;
btMatrix3x3 mT = m.transpose(); // we rely on transpose() zeroing w channel so that we don't have to do it here
b3Matrix3x3 mT = m.transpose(); // we rely on transpose() zeroing w channel so that we don't have to do it here
float32x4_t mx = mT[0].mVec128;
float32x4_t my = mT[1].mVec128;
float32x4_t mz = mT[2].mVec128;
@@ -1127,29 +1127,29 @@ btMatrix3x3::timesTranspose(const btMatrix3x3& m) const
r0 = vmlaq_lane_f32( r0, mz, vget_high_f32(a0), 0);
r1 = vmlaq_lane_f32( r1, mz, vget_high_f32(a1), 0);
r2 = vmlaq_lane_f32( r2, mz, vget_high_f32(a2), 0);
return btMatrix3x3( r0, r1, r2 );
return b3Matrix3x3( r0, r1, r2 );
#else
return btMatrix3x3(
return b3Matrix3x3(
m_el[0].dot(m[0]), m_el[0].dot(m[1]), m_el[0].dot(m[2]),
m_el[1].dot(m[0]), m_el[1].dot(m[1]), m_el[1].dot(m[2]),
m_el[2].dot(m[0]), m_el[2].dot(m[1]), m_el[2].dot(m[2]));
#endif
}
SIMD_FORCE_INLINE btVector3
operator*(const btMatrix3x3& m, const btVector3& v)
SIMD_FORCE_INLINE b3Vector3
operator*(const b3Matrix3x3& m, const b3Vector3& v)
{
#if (defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE))|| defined (BT_USE_NEON)
return v.dot3(m[0], m[1], m[2]);
#else
return btVector3(m[0].dot(v), m[1].dot(v), m[2].dot(v));
return b3Vector3(m[0].dot(v), m[1].dot(v), m[2].dot(v));
#endif
}
SIMD_FORCE_INLINE btVector3
operator*(const btVector3& v, const btMatrix3x3& m)
SIMD_FORCE_INLINE b3Vector3
operator*(const b3Vector3& v, const b3Matrix3x3& m)
{
#if (defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE))
@@ -1164,7 +1164,7 @@ operator*(const btVector3& v, const btMatrix3x3& m)
c0 = _mm_add_ps(c0, c1);
c2 = _mm_mul_ps(c2, _mm_and_ps(m[2].mVec128, btvFFF0fMask) );
return btVector3(_mm_add_ps(c0, c2));
return b3Vector3(_mm_add_ps(c0, c2));
#elif defined(BT_USE_NEON)
const float32x4_t vv = v.mVec128;
const float32x2_t vlo = vget_low_f32(vv);
@@ -1182,14 +1182,14 @@ operator*(const btVector3& v, const btMatrix3x3& m)
c0 = vaddq_f32(c0, c1);
c0 = vaddq_f32(c0, c2);
return btVector3(c0);
return b3Vector3(c0);
#else
return btVector3(m.tdotx(v), m.tdoty(v), m.tdotz(v));
return b3Vector3(m.tdotx(v), m.tdoty(v), m.tdotz(v));
#endif
}
SIMD_FORCE_INLINE btMatrix3x3
operator*(const btMatrix3x3& m1, const btMatrix3x3& m2)
SIMD_FORCE_INLINE b3Matrix3x3
operator*(const b3Matrix3x3& m1, const b3Matrix3x3& m2)
{
#if (defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE))
@@ -1235,7 +1235,7 @@ operator*(const btMatrix3x3& m1, const btMatrix3x3& m2)
c1 = _mm_add_ps(c1, m11);
c2 = _mm_add_ps(c2, m12);
return btMatrix3x3(c0, c1, c2);
return b3Matrix3x3(c0, c1, c2);
#elif defined(BT_USE_NEON)
@@ -1263,10 +1263,10 @@ operator*(const btMatrix3x3& m1, const btMatrix3x3& m2)
rv1 = vmlaq_lane_f32(rv1, mv2, vget_high_f32(v1), 0);
rv2 = vmlaq_lane_f32(rv2, mv2, vget_high_f32(v2), 0);
return btMatrix3x3(rv0, rv1, rv2);
return b3Matrix3x3(rv0, rv1, rv2);
#else
return btMatrix3x3(
return b3Matrix3x3(
m2.tdotx( m1[0]), m2.tdoty( m1[0]), m2.tdotz( m1[0]),
m2.tdotx( m1[1]), m2.tdoty( m1[1]), m2.tdotz( m1[1]),
m2.tdotx( m1[2]), m2.tdoty( m1[2]), m2.tdotz( m1[2]));
@@ -1274,8 +1274,8 @@ operator*(const btMatrix3x3& m1, const btMatrix3x3& m2)
}
/*
SIMD_FORCE_INLINE btMatrix3x3 btMultTransposeLeft(const btMatrix3x3& m1, const btMatrix3x3& m2) {
return btMatrix3x3(
SIMD_FORCE_INLINE b3Matrix3x3 btMultTransposeLeft(const b3Matrix3x3& m1, const b3Matrix3x3& m2) {
return b3Matrix3x3(
m1[0][0] * m2[0][0] + m1[1][0] * m2[1][0] + m1[2][0] * m2[2][0],
m1[0][0] * m2[0][1] + m1[1][0] * m2[1][1] + m1[2][0] * m2[2][1],
m1[0][0] * m2[0][2] + m1[1][0] * m2[1][2] + m1[2][0] * m2[2][2],
@@ -1290,7 +1290,7 @@ m1[0][2] * m2[0][2] + m1[1][2] * m2[1][2] + m1[2][2] * m2[2][2]);
/**@brief Equality operator between two matrices
* It will test all elements are equal. */
SIMD_FORCE_INLINE bool operator==(const btMatrix3x3& m1, const btMatrix3x3& m2)
SIMD_FORCE_INLINE bool operator==(const b3Matrix3x3& m1, const b3Matrix3x3& m2)
{
#if (defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE))
@@ -1327,32 +1327,32 @@ struct btMatrix3x3DoubleData
SIMD_FORCE_INLINE void btMatrix3x3::serialize(struct btMatrix3x3Data& dataOut) const
SIMD_FORCE_INLINE void b3Matrix3x3::serialize(struct btMatrix3x3Data& dataOut) const
{
for (int i=0;i<3;i++)
m_el[i].serialize(dataOut.m_el[i]);
}
SIMD_FORCE_INLINE void btMatrix3x3::serializeFloat(struct btMatrix3x3FloatData& dataOut) const
SIMD_FORCE_INLINE void b3Matrix3x3::serializeFloat(struct btMatrix3x3FloatData& dataOut) const
{
for (int i=0;i<3;i++)
m_el[i].serializeFloat(dataOut.m_el[i]);
}
SIMD_FORCE_INLINE void btMatrix3x3::deSerialize(const struct btMatrix3x3Data& dataIn)
SIMD_FORCE_INLINE void b3Matrix3x3::deSerialize(const struct btMatrix3x3Data& dataIn)
{
for (int i=0;i<3;i++)
m_el[i].deSerialize(dataIn.m_el[i]);
}
SIMD_FORCE_INLINE void btMatrix3x3::deSerializeFloat(const struct btMatrix3x3FloatData& dataIn)
SIMD_FORCE_INLINE void b3Matrix3x3::deSerializeFloat(const struct btMatrix3x3FloatData& dataIn)
{
for (int i=0;i<3;i++)
m_el[i].deSerializeFloat(dataIn.m_el[i]);
}
SIMD_FORCE_INLINE void btMatrix3x3::deSerializeDouble(const struct btMatrix3x3DoubleData& dataIn)
SIMD_FORCE_INLINE void b3Matrix3x3::deSerializeDouble(const struct btMatrix3x3DoubleData& dataIn)
{
for (int i=0;i<3;i++)
m_el[i].deSerializeDouble(dataIn.m_el[i]);

2
src/BulletCommon/btMinMax.h → src/BulletCommon/b3MinMax.h
View File

@@ -17,7 +17,7 @@ subject to the following restrictions:
#ifndef BT_GEN_MINMAX_H
#define BT_GEN_MINMAX_H
#include "btScalar.h"
#include "b3Scalar.h"
template <class T>
SIMD_FORCE_INLINE const T& btMin(const T& a, const T& b)

12
src/BulletCommon/btPoolAllocator.h → src/BulletCommon/b3PoolAllocator.h
View File

@@ -16,11 +16,11 @@ subject to the following restrictions:
#ifndef _BT_POOL_ALLOCATOR_H
#define _BT_POOL_ALLOCATOR_H
#include "btScalar.h"
#include "btAlignedAllocator.h"
#include "b3Scalar.h"
#include "b3AlignedAllocator.h"
///The btPoolAllocator class allows to efficiently allocate a large pool of objects, instead of dynamically allocating them separately.
class btPoolAllocator
///The b3PoolAllocator class allows to efficiently allocate a large pool of objects, instead of dynamically allocating them separately.
class b3PoolAllocator
{
int m_elemSize;
int m_maxElements;
@@ -30,7 +30,7 @@ class btPoolAllocator
public:
btPoolAllocator(int elemSize, int maxElements)
b3PoolAllocator(int elemSize, int maxElements)
:m_elemSize(elemSize),
m_maxElements(maxElements)
{
@@ -47,7 +47,7 @@ public:
*(void**)p = 0;
}
~btPoolAllocator()
~b3PoolAllocator()
{
btAlignedFree( m_pool);
}

80
src/BulletCommon/btQuadWord.h → src/BulletCommon/b3QuadWord.h
View File

@@ -16,8 +16,8 @@ subject to the following restrictions:
#ifndef BT_SIMD_QUADWORD_H
#define BT_SIMD_QUADWORD_H
#include "btScalar.h"
#include "btMinMax.h"
#include "b3Scalar.h"
#include "b3MinMax.h"
@@ -27,13 +27,13 @@ subject to the following restrictions:
#include <altivec.h>
#endif
/**@brief The btQuadWord class is base class for btVector3 and btQuaternion.
/**@brief The b3QuadWord class is base class for b3Vector3 and b3Quaternion.
* Some issues under PS3 Linux with IBM 2.1 SDK, gcc compiler prevent from using aligned quadword.
*/
#ifndef USE_LIBSPE2
ATTRIBUTE_ALIGNED16(class) btQuadWord
ATTRIBUTE_ALIGNED16(class) b3QuadWord
#else
class btQuadWord
class b3QuadWord
#endif
{
protected:
@@ -41,7 +41,7 @@ protected:
#if defined (__SPU__) && defined (__CELLOS_LV2__)
union {
vec_float4 mVec128;
btScalar m_floats[4];
b3Scalar m_floats[4];
};
public:
vec_float4 get128() const
@@ -54,8 +54,8 @@ public:
#if defined(BT_USE_SSE) || defined(BT_USE_NEON)
union {
btSimdFloat4 mVec128;
btScalar m_floats[4];
struct {btScalar x,y,z,w;};
b3Scalar m_floats[4];
struct {b3Scalar x,y,z,w;};
};
public:
SIMD_FORCE_INLINE btSimdFloat4 get128() const
@@ -67,7 +67,7 @@ public:
mVec128 = v128;
}
#else
btScalar m_floats[4];
b3Scalar m_floats[4];
#endif // BT_USE_SSE
#endif //__CELLOS_LV2__ __SPU__
@@ -77,20 +77,20 @@ public:
#if defined(BT_USE_SSE) || defined(BT_USE_NEON)
// Set Vector
SIMD_FORCE_INLINE btQuadWord(const btSimdFloat4 vec)
SIMD_FORCE_INLINE b3QuadWord(const btSimdFloat4 vec)
{
mVec128 = vec;
}
// Copy constructor
SIMD_FORCE_INLINE btQuadWord(const btQuadWord& rhs)
SIMD_FORCE_INLINE b3QuadWord(const b3QuadWord& rhs)
{
mVec128 = rhs.mVec128;
}
// Assignment Operator
SIMD_FORCE_INLINE btQuadWord&
operator=(const btQuadWord& v)
SIMD_FORCE_INLINE b3QuadWord&
operator=(const b3QuadWord& v)
{
mVec128 = v.mVec128;
@@ -100,29 +100,29 @@ public:
#endif
/**@brief Return the x value */
SIMD_FORCE_INLINE const btScalar& getX() const { return m_floats[0]; }
SIMD_FORCE_INLINE const b3Scalar& getX() const { return m_floats[0]; }
/**@brief Return the y value */
SIMD_FORCE_INLINE const btScalar& getY() const { return m_floats[1]; }
SIMD_FORCE_INLINE const b3Scalar& getY() const { return m_floats[1]; }
/**@brief Return the z value */
SIMD_FORCE_INLINE const btScalar& getZ() const { return m_floats[2]; }
SIMD_FORCE_INLINE const b3Scalar& getZ() const { return m_floats[2]; }
/**@brief Set the x value */
SIMD_FORCE_INLINE void setX(btScalar _x) { m_floats[0] = _x;};
SIMD_FORCE_INLINE void setX(b3Scalar _x) { m_floats[0] = _x;};
/**@brief Set the y value */
SIMD_FORCE_INLINE void setY(btScalar _y) { m_floats[1] = _y;};
SIMD_FORCE_INLINE void setY(b3Scalar _y) { m_floats[1] = _y;};
/**@brief Set the z value */
SIMD_FORCE_INLINE void setZ(btScalar _z) { m_floats[2] = _z;};
SIMD_FORCE_INLINE void setZ(b3Scalar _z) { m_floats[2] = _z;};
/**@brief Set the w value */
SIMD_FORCE_INLINE void setW(btScalar _w) { m_floats[3] = _w;};
SIMD_FORCE_INLINE void setW(b3Scalar _w) { m_floats[3] = _w;};
/**@brief Return the x value */
//SIMD_FORCE_INLINE btScalar& operator[](int i) { return (&m_floats[0])[i]; }
//SIMD_FORCE_INLINE const btScalar& operator[](int i) const { return (&m_floats[0])[i]; }
///operator btScalar*() replaces operator[], using implicit conversion. We added operator != and operator == to avoid pointer comparisons.
SIMD_FORCE_INLINE operator btScalar *() { return &m_floats[0]; }
SIMD_FORCE_INLINE operator const btScalar *() const { return &m_floats[0]; }
//SIMD_FORCE_INLINE b3Scalar& operator[](int i) { return (&m_floats[0])[i]; }
//SIMD_FORCE_INLINE const b3Scalar& operator[](int i) const { return (&m_floats[0])[i]; }
///operator b3Scalar*() replaces operator[], using implicit conversion. We added operator != and operator == to avoid pointer comparisons.
SIMD_FORCE_INLINE operator b3Scalar *() { return &m_floats[0]; }
SIMD_FORCE_INLINE operator const b3Scalar *() const { return &m_floats[0]; }
SIMD_FORCE_INLINE bool operator==(const btQuadWord& other) const
SIMD_FORCE_INLINE bool operator==(const b3QuadWord& other) const
{
#ifdef BT_USE_SSE
return (0xf == _mm_movemask_ps((__m128)_mm_cmpeq_ps(mVec128, other.mVec128)));
@@ -134,7 +134,7 @@ public:
#endif
}
SIMD_FORCE_INLINE bool operator!=(const btQuadWord& other) const
SIMD_FORCE_INLINE bool operator!=(const b3QuadWord& other) const
{
return !(*this == other);
}
@@ -144,7 +144,7 @@ public:
* @param y Value of y
* @param z Value of z
*/
SIMD_FORCE_INLINE void setValue(const btScalar& _x, const btScalar& _y, const btScalar& _z)
SIMD_FORCE_INLINE void setValue(const b3Scalar& _x, const b3Scalar& _y, const b3Scalar& _z)
{
m_floats[0]=_x;
m_floats[1]=_y;
@@ -152,7 +152,7 @@ public:
m_floats[3] = 0.f;
}
/* void getValue(btScalar *m) const
/* void getValue(b3Scalar *m) const
{
m[0] = m_floats[0];
m[1] = m_floats[1];
@@ -165,7 +165,7 @@ public:
* @param z Value of z
* @param w Value of w
*/
SIMD_FORCE_INLINE void setValue(const btScalar& _x, const btScalar& _y, const btScalar& _z,const btScalar& _w)
SIMD_FORCE_INLINE void setValue(const b3Scalar& _x, const b3Scalar& _y, const b3Scalar& _z,const b3Scalar& _w)
{
m_floats[0]=_x;
m_floats[1]=_y;
@@ -173,8 +173,8 @@ public:
m_floats[3]=_w;
}
/**@brief No initialization constructor */
SIMD_FORCE_INLINE btQuadWord()
// :m_floats[0](btScalar(0.)),m_floats[1](btScalar(0.)),m_floats[2](btScalar(0.)),m_floats[3](btScalar(0.))
SIMD_FORCE_INLINE b3QuadWord()
// :m_floats[0](b3Scalar(0.)),m_floats[1](b3Scalar(0.)),m_floats[2](b3Scalar(0.)),m_floats[3](b3Scalar(0.))
{
}
@@ -183,7 +183,7 @@ public:
* @param y Value of y
* @param z Value of z
*/
SIMD_FORCE_INLINE btQuadWord(const btScalar& _x, const btScalar& _y, const btScalar& _z)
SIMD_FORCE_INLINE b3QuadWord(const b3Scalar& _x, const b3Scalar& _y, const b3Scalar& _z)
{
m_floats[0] = _x, m_floats[1] = _y, m_floats[2] = _z, m_floats[3] = 0.0f;
}
@@ -194,15 +194,15 @@ public:
* @param z Value of z
* @param w Value of w
*/
SIMD_FORCE_INLINE btQuadWord(const btScalar& _x, const btScalar& _y, const btScalar& _z,const btScalar& _w)
SIMD_FORCE_INLINE b3QuadWord(const b3Scalar& _x, const b3Scalar& _y, const b3Scalar& _z,const b3Scalar& _w)
{
m_floats[0] = _x, m_floats[1] = _y, m_floats[2] = _z, m_floats[3] = _w;
}
/**@brief Set each element to the max of the current values and the values of another btQuadWord
* @param other The other btQuadWord to compare with
/**@brief Set each element to the max of the current values and the values of another b3QuadWord
* @param other The other b3QuadWord to compare with
*/
SIMD_FORCE_INLINE void setMax(const btQuadWord& other)
SIMD_FORCE_INLINE void setMax(const b3QuadWord& other)
{
#ifdef BT_USE_SSE
mVec128 = _mm_max_ps(mVec128, other.mVec128);
@@ -215,10 +215,10 @@ public:
btSetMax(m_floats[3], other.m_floats[3]);
#endif
}
/**@brief Set each element to the min of the current values and the values of another btQuadWord
* @param other The other btQuadWord to compare with
/**@brief Set each element to the min of the current values and the values of another b3QuadWord
* @param other The other b3QuadWord to compare with
*/
SIMD_FORCE_INLINE void setMin(const btQuadWord& other)
SIMD_FORCE_INLINE void setMin(const b3QuadWord& other)
{
#ifdef BT_USE_SSE
mVec128 = _mm_min_ps(mVec128, other.mVec128);

304
src/BulletCommon/btQuaternion.h → src/BulletCommon/b3Quaternion.h
View File

@@ -18,8 +18,8 @@ subject to the following restrictions:
#define BT_SIMD__QUATERNION_H_
#include "btVector3.h"
#include "btQuadWord.h"
#include "b3Vector3.h"
#include "b3QuadWord.h"
@@ -38,28 +38,28 @@ const btSimdFloat4 ATTRIBUTE_ALIGNED16(vPPPM) = {+0.0f, +0.0f, +0.0f, -0.0f};
#endif
/**@brief The btQuaternion implements quaternion to perform linear algebra rotations in combination with btMatrix3x3, btVector3 and btTransform. */
class btQuaternion : public btQuadWord {
/**@brief The b3Quaternion implements quaternion to perform linear algebra rotations in combination with b3Matrix3x3, b3Vector3 and b3Transform. */
class b3Quaternion : public b3QuadWord {
public:
/**@brief No initialization constructor */
btQuaternion() {}
b3Quaternion() {}
#if (defined(BT_USE_SSE_IN_API) && defined(BT_USE_SSE))|| defined(BT_USE_NEON)
// Set Vector
SIMD_FORCE_INLINE btQuaternion(const btSimdFloat4 vec)
SIMD_FORCE_INLINE b3Quaternion(const btSimdFloat4 vec)
{
mVec128 = vec;
}
// Copy constructor
SIMD_FORCE_INLINE btQuaternion(const btQuaternion& rhs)
SIMD_FORCE_INLINE b3Quaternion(const b3Quaternion& rhs)
{
mVec128 = rhs.mVec128;
}
// Assignment Operator
SIMD_FORCE_INLINE btQuaternion&
operator=(const btQuaternion& v)
SIMD_FORCE_INLINE b3Quaternion&
operator=(const b3Quaternion& v)
{
mVec128 = v.mVec128;
@@ -68,18 +68,18 @@ public:
#endif
// template <typename btScalar>
// explicit Quaternion(const btScalar *v) : Tuple4<btScalar>(v) {}
// template <typename b3Scalar>
// explicit Quaternion(const b3Scalar *v) : Tuple4<b3Scalar>(v) {}
/**@brief Constructor from scalars */
btQuaternion(const btScalar& _x, const btScalar& _y, const btScalar& _z, const btScalar& _w)
: btQuadWord(_x, _y, _z, _w)
b3Quaternion(const b3Scalar& _x, const b3Scalar& _y, const b3Scalar& _z, const b3Scalar& _w)
: b3QuadWord(_x, _y, _z, _w)
{
btAssert(!((_x==1.f) && (_y==0.f) && (_z==0.f) && (_w==0.f)));
}
/**@brief Axis angle Constructor
* @param axis The axis which the rotation is around
* @param angle The magnitude of the rotation around the angle (Radians) */
btQuaternion(const btVector3& _axis, const btScalar& _angle)
b3Quaternion(const b3Vector3& _axis, const b3Scalar& _angle)
{
setRotation(_axis, _angle);
}
@@ -87,7 +87,7 @@ public:
* @param yaw Angle around Y unless BT_EULER_DEFAULT_ZYX defined then Z
* @param pitch Angle around X unless BT_EULER_DEFAULT_ZYX defined then Y
* @param roll Angle around Z unless BT_EULER_DEFAULT_ZYX defined then X */
btQuaternion(const btScalar& yaw, const btScalar& pitch, const btScalar& roll)
b3Quaternion(const b3Scalar& yaw, const b3Scalar& pitch, const b3Scalar& roll)
{
#ifndef BT_EULER_DEFAULT_ZYX
setEuler(yaw, pitch, roll);
@@ -98,29 +98,29 @@ public:
/**@brief Set the rotation using axis angle notation
* @param axis The axis around which to rotate
* @param angle The magnitude of the rotation in Radians */
void setRotation(const btVector3& axis, const btScalar& _angle)
void setRotation(const b3Vector3& axis, const b3Scalar& _angle)
{
btScalar d = axis.length();
btAssert(d != btScalar(0.0));
btScalar s = btSin(_angle * btScalar(0.5)) / d;
b3Scalar d = axis.length();
btAssert(d != b3Scalar(0.0));
b3Scalar s = btSin(_angle * b3Scalar(0.5)) / d;
setValue(axis.getX() * s, axis.getY() * s, axis.getZ() * s,
btCos(_angle * btScalar(0.5)));
btCos(_angle * b3Scalar(0.5)));
}
/**@brief Set the quaternion using Euler angles
* @param yaw Angle around Y
* @param pitch Angle around X
* @param roll Angle around Z */
void setEuler(const btScalar& yaw, const btScalar& pitch, const btScalar& roll)
void setEuler(const b3Scalar& yaw, const b3Scalar& pitch, const b3Scalar& roll)
{
btScalar halfYaw = btScalar(yaw) * btScalar(0.5);
btScalar halfPitch = btScalar(pitch) * btScalar(0.5);
btScalar halfRoll = btScalar(roll) * btScalar(0.5);
btScalar cosYaw = btCos(halfYaw);
btScalar sinYaw = btSin(halfYaw);
btScalar cosPitch = btCos(halfPitch);
btScalar sinPitch = btSin(halfPitch);
btScalar cosRoll = btCos(halfRoll);
btScalar sinRoll = btSin(halfRoll);
b3Scalar halfYaw = b3Scalar(yaw) * b3Scalar(0.5);
b3Scalar halfPitch = b3Scalar(pitch) * b3Scalar(0.5);
b3Scalar halfRoll = b3Scalar(roll) * b3Scalar(0.5);
b3Scalar cosYaw = btCos(halfYaw);
b3Scalar sinYaw = btSin(halfYaw);
b3Scalar cosPitch = btCos(halfPitch);
b3Scalar sinPitch = btSin(halfPitch);
b3Scalar cosRoll = btCos(halfRoll);
b3Scalar sinRoll = btSin(halfRoll);
setValue(cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw,
cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw,
sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw,
@@ -130,17 +130,17 @@ public:
* @param yaw Angle around Z
* @param pitch Angle around Y
* @param roll Angle around X */
void setEulerZYX(const btScalar& yaw, const btScalar& pitch, const btScalar& roll)
void setEulerZYX(const b3Scalar& yaw, const b3Scalar& pitch, const b3Scalar& roll)
{
btScalar halfYaw = btScalar(yaw) * btScalar(0.5);
btScalar halfPitch = btScalar(pitch) * btScalar(0.5);
btScalar halfRoll = btScalar(roll) * btScalar(0.5);
btScalar cosYaw = btCos(halfYaw);
btScalar sinYaw = btSin(halfYaw);
btScalar cosPitch = btCos(halfPitch);
btScalar sinPitch = btSin(halfPitch);
btScalar cosRoll = btCos(halfRoll);
btScalar sinRoll = btSin(halfRoll);
b3Scalar halfYaw = b3Scalar(yaw) * b3Scalar(0.5);
b3Scalar halfPitch = b3Scalar(pitch) * b3Scalar(0.5);
b3Scalar halfRoll = b3Scalar(roll) * b3Scalar(0.5);
b3Scalar cosYaw = btCos(halfYaw);
b3Scalar sinYaw = btSin(halfYaw);
b3Scalar cosPitch = btCos(halfPitch);
b3Scalar sinPitch = btSin(halfPitch);
b3Scalar cosRoll = btCos(halfRoll);
b3Scalar sinRoll = btSin(halfRoll);
setValue(sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw, //x
cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw, //y
cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw, //z
@@ -148,7 +148,7 @@ public:
}
/**@brief Add two quaternions
* @param q The quaternion to add to this one */
SIMD_FORCE_INLINE btQuaternion& operator+=(const btQuaternion& q)
SIMD_FORCE_INLINE b3Quaternion& operator+=(const b3Quaternion& q)
{
#if defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
mVec128 = _mm_add_ps(mVec128, q.mVec128);
@@ -165,7 +165,7 @@ public:
/**@brief Subtract out a quaternion
* @param q The quaternion to subtract from this one */
btQuaternion& operator-=(const btQuaternion& q)
b3Quaternion& operator-=(const b3Quaternion& q)
{
#if defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
mVec128 = _mm_sub_ps(mVec128, q.mVec128);
@@ -182,7 +182,7 @@ public:
/**@brief Scale this quaternion
* @param s The scalar to scale by */
btQuaternion& operator*=(const btScalar& s)
b3Quaternion& operator*=(const b3Scalar& s)
{
#if defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
__m128 vs = _mm_load_ss(&s); // (S 0 0 0)
@@ -202,7 +202,7 @@ public:
/**@brief Multiply this quaternion by q on the right
* @param q The other quaternion
* Equivilant to this = this * q */
btQuaternion& operator*=(const btQuaternion& q)
b3Quaternion& operator*=(const b3Quaternion& q)
{
#if defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
__m128 vQ2 = q.get128();
@@ -285,7 +285,7 @@ public:
}
/**@brief Return the dot product between this quaternion and another
* @param q The other quaternion */
btScalar dot(const btQuaternion& q) const
b3Scalar dot(const b3Quaternion& q) const
{
#if defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
__m128 vd;
@@ -312,20 +312,20 @@ public:
}
/**@brief Return the length squared of the quaternion */
btScalar length2() const
b3Scalar length2() const
{
return dot(*this);
}
/**@brief Return the length of the quaternion */
btScalar length() const
b3Scalar length() const
{
return btSqrt(length2());
}
/**@brief Normalize the quaternion
* Such that x^2 + y^2 + z^2 +w^2 = 1 */
btQuaternion& normalize()
b3Quaternion& normalize()
{
#if defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
__m128 vd;
@@ -350,127 +350,127 @@ public:
/**@brief Return a scaled version of this quaternion
* @param s The scale factor */
SIMD_FORCE_INLINE btQuaternion
operator*(const btScalar& s) const
SIMD_FORCE_INLINE b3Quaternion
operator*(const b3Scalar& s) const
{
#if defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
__m128 vs = _mm_load_ss(&s); // (S 0 0 0)
vs = bt_pshufd_ps(vs, 0x00); // (S S S S)
return btQuaternion(_mm_mul_ps(mVec128, vs));
return b3Quaternion(_mm_mul_ps(mVec128, vs));
#elif defined(BT_USE_NEON)
return btQuaternion(vmulq_n_f32(mVec128, s));
return b3Quaternion(vmulq_n_f32(mVec128, s));
#else
return btQuaternion(getX() * s, getY() * s, getZ() * s, m_floats[3] * s);
return b3Quaternion(getX() * s, getY() * s, getZ() * s, m_floats[3] * s);
#endif
}
/**@brief Return an inversely scaled versionof this quaternion
* @param s The inverse scale factor */
btQuaternion operator/(const btScalar& s) const
b3Quaternion operator/(const b3Scalar& s) const
{
btAssert(s != btScalar(0.0));
return *this * (btScalar(1.0) / s);
btAssert(s != b3Scalar(0.0));
return *this * (b3Scalar(1.0) / s);
}
/**@brief Inversely scale this quaternion
* @param s The scale factor */
btQuaternion& operator/=(const btScalar& s)
b3Quaternion& operator/=(const b3Scalar& s)
{
btAssert(s != btScalar(0.0));
return *this *= btScalar(1.0) / s;
btAssert(s != b3Scalar(0.0));
return *this *= b3Scalar(1.0) / s;
}
/**@brief Return a normalized version of this quaternion */
btQuaternion normalized() const
b3Quaternion normalized() const
{
return *this / length();
}
/**@brief Return the angle between this quaternion and the other
* @param q The other quaternion */
btScalar angle(const btQuaternion& q) const
b3Scalar angle(const b3Quaternion& q) const
{
btScalar s = btSqrt(length2() * q.length2());
btAssert(s != btScalar(0.0));
b3Scalar s = btSqrt(length2() * q.length2());
btAssert(s != b3Scalar(0.0));
return btAcos(dot(q) / s);
}
/**@brief Return the angle of rotation represented by this quaternion */
btScalar getAngle() const
b3Scalar getAngle() const
{
btScalar s = btScalar(2.) * btAcos(m_floats[3]);
b3Scalar s = b3Scalar(2.) * btAcos(m_floats[3]);
return s;
}
/**@brief Return the axis of the rotation represented by this quaternion */
btVector3 getAxis() const
b3Vector3 getAxis() const
{
btScalar s_squared = 1.f-m_floats[3]*m_floats[3];
b3Scalar s_squared = 1.f-m_floats[3]*m_floats[3];
if (s_squared < btScalar(10.) * SIMD_EPSILON) //Check for divide by zero
return btVector3(1.0, 0.0, 0.0); // Arbitrary
btScalar s = 1.f/btSqrt(s_squared);
return btVector3(m_floats[0] * s, m_floats[1] * s, m_floats[2] * s);
if (s_squared < b3Scalar(10.) * SIMD_EPSILON) //Check for divide by zero
return b3Vector3(1.0, 0.0, 0.0); // Arbitrary
b3Scalar s = 1.f/btSqrt(s_squared);
return b3Vector3(m_floats[0] * s, m_floats[1] * s, m_floats[2] * s);
}
/**@brief Return the inverse of this quaternion */
btQuaternion inverse() const
b3Quaternion inverse() const
{
#if defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
return btQuaternion(_mm_xor_ps(mVec128, vQInv));
return b3Quaternion(_mm_xor_ps(mVec128, vQInv));
#elif defined(BT_USE_NEON)
return btQuaternion((btSimdFloat4)veorq_s32((int32x4_t)mVec128, (int32x4_t)vQInv));
return b3Quaternion((btSimdFloat4)veorq_s32((int32x4_t)mVec128, (int32x4_t)vQInv));
#else
return btQuaternion(-m_floats[0], -m_floats[1], -m_floats[2], m_floats[3]);
return b3Quaternion(-m_floats[0], -m_floats[1], -m_floats[2], m_floats[3]);
#endif
}
/**@brief Return the sum of this quaternion and the other
* @param q2 The other quaternion */
SIMD_FORCE_INLINE btQuaternion
operator+(const btQuaternion& q2) const
SIMD_FORCE_INLINE b3Quaternion
operator+(const b3Quaternion& q2) const
{
#if defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
return btQuaternion(_mm_add_ps(mVec128, q2.mVec128));
return b3Quaternion(_mm_add_ps(mVec128, q2.mVec128));
#elif defined(BT_USE_NEON)
return btQuaternion(vaddq_f32(mVec128, q2.mVec128));
return b3Quaternion(vaddq_f32(mVec128, q2.mVec128));
#else
const btQuaternion& q1 = *this;
return btQuaternion(q1.getX() + q2.getX(), q1.getY() + q2.getY(), q1.getZ() + q2.getZ(), q1.m_floats[3] + q2.m_floats[3]);
const b3Quaternion& q1 = *this;
return b3Quaternion(q1.getX() + q2.getX(), q1.getY() + q2.getY(), q1.getZ() + q2.getZ(), q1.m_floats[3] + q2.m_floats[3]);
#endif
}
/**@brief Return the difference between this quaternion and the other
* @param q2 The other quaternion */
SIMD_FORCE_INLINE btQuaternion
operator-(const btQuaternion& q2) const
SIMD_FORCE_INLINE b3Quaternion
operator-(const b3Quaternion& q2) const
{
#if defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
return btQuaternion(_mm_sub_ps(mVec128, q2.mVec128));
return b3Quaternion(_mm_sub_ps(mVec128, q2.mVec128));
#elif defined(BT_USE_NEON)
return btQuaternion(vsubq_f32(mVec128, q2.mVec128));
return b3Quaternion(vsubq_f32(mVec128, q2.mVec128));
#else
const btQuaternion& q1 = *this;
return btQuaternion(q1.getX() - q2.getX(), q1.getY() - q2.getY(), q1.getZ() - q2.getZ(), q1.m_floats[3] - q2.m_floats[3]);
const b3Quaternion& q1 = *this;
return b3Quaternion(q1.getX() - q2.getX(), q1.getY() - q2.getY(), q1.getZ() - q2.getZ(), q1.m_floats[3] - q2.m_floats[3]);
#endif
}
/**@brief Return the negative of this quaternion
* This simply negates each element */
SIMD_FORCE_INLINE btQuaternion operator-() const
SIMD_FORCE_INLINE b3Quaternion operator-() const
{
#if defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
return btQuaternion(_mm_xor_ps(mVec128, btvMzeroMask));
return b3Quaternion(_mm_xor_ps(mVec128, btvMzeroMask));
#elif defined(BT_USE_NEON)
return btQuaternion((btSimdFloat4)veorq_s32((int32x4_t)mVec128, (int32x4_t)btvMzeroMask) );
return b3Quaternion((btSimdFloat4)veorq_s32((int32x4_t)mVec128, (int32x4_t)btvMzeroMask) );
#else
const btQuaternion& q2 = *this;
return btQuaternion( - q2.getX(), - q2.getY(), - q2.getZ(), - q2.m_floats[3]);
const b3Quaternion& q2 = *this;
return b3Quaternion( - q2.getX(), - q2.getY(), - q2.getZ(), - q2.m_floats[3]);
#endif
}
/**@todo document this and it's use */
SIMD_FORCE_INLINE btQuaternion farthest( const btQuaternion& qd) const
SIMD_FORCE_INLINE b3Quaternion farthest( const b3Quaternion& qd) const
{
btQuaternion diff,sum;
b3Quaternion diff,sum;
diff = *this - qd;
sum = *this + qd;
if( diff.dot(diff) > sum.dot(sum) )
@@ -479,9 +479,9 @@ public:
}
/**@todo document this and it's use */
SIMD_FORCE_INLINE btQuaternion nearest( const btQuaternion& qd) const
SIMD_FORCE_INLINE b3Quaternion nearest( const b3Quaternion& qd) const
{
btQuaternion diff,sum;
b3Quaternion diff,sum;
diff = *this - qd;
sum = *this + qd;
if( diff.dot(diff) < sum.dot(sum) )
@@ -494,23 +494,23 @@ public:
* @param q The other quaternion to interpolate with
* @param t The ratio between this and q to interpolate. If t = 0 the result is this, if t=1 the result is q.
* Slerp interpolates assuming constant velocity. */
btQuaternion slerp(const btQuaternion& q, const btScalar& t) const
b3Quaternion slerp(const b3Quaternion& q, const b3Scalar& t) const
{
btScalar magnitude = btSqrt(length2() * q.length2());
btAssert(magnitude > btScalar(0));
b3Scalar magnitude = btSqrt(length2() * q.length2());
btAssert(magnitude > b3Scalar(0));
btScalar product = dot(q) / magnitude;
if (btFabs(product) < btScalar(1))
b3Scalar product = dot(q) / magnitude;
if (btFabs(product) < b3Scalar(1))
{
// Take care of long angle case see http://en.wikipedia.org/wiki/Slerp
const btScalar sign = (product < 0) ? btScalar(-1) : btScalar(1);
const b3Scalar sign = (product < 0) ? b3Scalar(-1) : b3Scalar(1);
const btScalar theta = btAcos(sign * product);
const btScalar s1 = btSin(sign * t * theta);
const btScalar d = btScalar(1.0) / btSin(theta);
const btScalar s0 = btSin((btScalar(1.0) - t) * theta);
const b3Scalar theta = btAcos(sign * product);
const b3Scalar s1 = btSin(sign * t * theta);
const b3Scalar d = b3Scalar(1.0) / btSin(theta);
const b3Scalar s0 = btSin((b3Scalar(1.0) - t) * theta);
return btQuaternion(
return b3Quaternion(
(m_floats[0] * s0 + q.getX() * s1) * d,
(m_floats[1] * s0 + q.getY() * s1) * d,
(m_floats[2] * s0 + q.getZ() * s1) * d,
@@ -522,13 +522,13 @@ public:
}
}
static const btQuaternion& getIdentity()
static const b3Quaternion& getIdentity()
{
static const btQuaternion identityQuat(btScalar(0.),btScalar(0.),btScalar(0.),btScalar(1.));
static const b3Quaternion identityQuat(b3Scalar(0.),b3Scalar(0.),b3Scalar(0.),b3Scalar(1.));
return identityQuat;
}
SIMD_FORCE_INLINE const btScalar& getW() const { return m_floats[3]; }
SIMD_FORCE_INLINE const b3Scalar& getW() const { return m_floats[3]; }
};
@@ -538,8 +538,8 @@ public:
/**@brief Return the product of two quaternions */
SIMD_FORCE_INLINE btQuaternion
operator*(const btQuaternion& q1, const btQuaternion& q2)
SIMD_FORCE_INLINE b3Quaternion
operator*(const b3Quaternion& q1, const b3Quaternion& q2)
{
#if defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
__m128 vQ1 = q1.get128();
@@ -570,7 +570,7 @@ operator*(const btQuaternion& q1, const btQuaternion& q2)
A1 = _mm_xor_ps(A1, vPPPM); // change sign of the last element
A0 = A0 + A1; // AB03 + AB12
return btQuaternion(A0);
return b3Quaternion(A0);
#elif defined(BT_USE_NEON)
@@ -615,10 +615,10 @@ operator*(const btQuaternion& q1, const btQuaternion& q2)
A1 = (btSimdFloat4)veorq_s32((int32x4_t)A1, (int32x4_t)vPPPM);
A0 = vaddq_f32(A0, A1); // AB03 + AB12
return btQuaternion(A0);
return b3Quaternion(A0);
#else
return btQuaternion(
return b3Quaternion(
q1.getW() * q2.getX() + q1.getX() * q2.getW() + q1.getY() * q2.getZ() - q1.getZ() * q2.getY(),
q1.getW() * q2.getY() + q1.getY() * q2.getW() + q1.getZ() * q2.getX() - q1.getX() * q2.getZ(),
q1.getW() * q2.getZ() + q1.getZ() * q2.getW() + q1.getX() * q2.getY() - q1.getY() * q2.getX(),
@@ -626,8 +626,8 @@ operator*(const btQuaternion& q1, const btQuaternion& q2)
#endif
}
SIMD_FORCE_INLINE btQuaternion
operator*(const btQuaternion& q, const btVector3& w)
SIMD_FORCE_INLINE b3Quaternion
operator*(const b3Quaternion& q, const b3Vector3& w)
{
#if defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
__m128 vQ1 = q.get128();
@@ -653,7 +653,7 @@ operator*(const btQuaternion& q, const btVector3& w)
A1 = _mm_xor_ps(A1, vPPPM); // change sign of the last element
A1 = A1 - A3; // AB123 = AB12 - AB3
return btQuaternion(A1);
return b3Quaternion(A1);
#elif defined(BT_USE_NEON)
@@ -698,10 +698,10 @@ operator*(const btQuaternion& q, const btVector3& w)
A1 = vsubq_f32(A1, A3); // AB123 = AB12 - AB3
return btQuaternion(A1);
return b3Quaternion(A1);
#else
return btQuaternion(
return b3Quaternion(
q.getW() * w.getX() + q.getY() * w.getZ() - q.getZ() * w.getY(),
q.getW() * w.getY() + q.getZ() * w.getX() - q.getX() * w.getZ(),
q.getW() * w.getZ() + q.getX() * w.getY() - q.getY() * w.getX(),
@@ -709,8 +709,8 @@ operator*(const btQuaternion& q, const btVector3& w)
#endif
}
SIMD_FORCE_INLINE btQuaternion
operator*(const btVector3& w, const btQuaternion& q)
SIMD_FORCE_INLINE b3Quaternion
operator*(const b3Vector3& w, const b3Quaternion& q)
{
#if defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
__m128 vQ1 = w.get128();
@@ -736,7 +736,7 @@ operator*(const btVector3& w, const btQuaternion& q)
A1 = _mm_xor_ps(A1, vPPPM); // change sign of the last element
A1 = A1 - A3; // AB123 = AB12 - AB3
return btQuaternion(A1);
return b3Quaternion(A1);
#elif defined(BT_USE_NEON)
@@ -781,10 +781,10 @@ operator*(const btVector3& w, const btQuaternion& q)
A1 = vsubq_f32(A1, A3); // AB123 = AB12 - AB3
return btQuaternion(A1);
return b3Quaternion(A1);
#else
return btQuaternion(
return b3Quaternion(
+w.getX() * q.getW() + w.getY() * q.getZ() - w.getZ() * q.getY(),
+w.getY() * q.getW() + w.getZ() * q.getX() - w.getX() * q.getZ(),
+w.getZ() * q.getW() + w.getX() * q.getY() - w.getY() * q.getX(),
@@ -793,30 +793,30 @@ operator*(const btVector3& w, const btQuaternion& q)
}
/**@brief Calculate the dot product between two quaternions */
SIMD_FORCE_INLINE btScalar
dot(const btQuaternion& q1, const btQuaternion& q2)
SIMD_FORCE_INLINE b3Scalar
dot(const b3Quaternion& q1, const b3Quaternion& q2)
{
return q1.dot(q2);
}
/**@brief Return the length of a quaternion */
SIMD_FORCE_INLINE btScalar
length(const btQuaternion& q)
SIMD_FORCE_INLINE b3Scalar
length(const b3Quaternion& q)
{
return q.length();
}
/**@brief Return the angle between two quaternions*/
SIMD_FORCE_INLINE btScalar
btAngle(const btQuaternion& q1, const btQuaternion& q2)
SIMD_FORCE_INLINE b3Scalar
btAngle(const b3Quaternion& q1, const b3Quaternion& q2)
{
return q1.angle(q2);
}
/**@brief Return the inverse of a quaternion*/
SIMD_FORCE_INLINE btQuaternion
inverse(const btQuaternion& q)
SIMD_FORCE_INLINE b3Quaternion
inverse(const b3Quaternion& q)
{
return q.inverse();
}
@@ -826,47 +826,47 @@ inverse(const btQuaternion& q)
* @param q2 The second quaternion
* @param t The ration between q1 and q2. t = 0 return q1, t=1 returns q2
* Slerp assumes constant velocity between positions. */
SIMD_FORCE_INLINE btQuaternion
slerp(const btQuaternion& q1, const btQuaternion& q2, const btScalar& t)
SIMD_FORCE_INLINE b3Quaternion
slerp(const b3Quaternion& q1, const b3Quaternion& q2, const b3Scalar& t)
{
return q1.slerp(q2, t);
}
SIMD_FORCE_INLINE btVector3
quatRotate(const btQuaternion& rotation, const btVector3& v)
SIMD_FORCE_INLINE b3Vector3
quatRotate(const b3Quaternion& rotation, const b3Vector3& v)
{
btQuaternion q = rotation * v;
b3Quaternion q = rotation * v;
q *= rotation.inverse();
#if defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
return btVector3(_mm_and_ps(q.get128(), btvFFF0fMask));
return b3Vector3(_mm_and_ps(q.get128(), btvFFF0fMask));
#elif defined(BT_USE_NEON)
return btVector3((float32x4_t)vandq_s32((int32x4_t)q.get128(), btvFFF0Mask));
return b3Vector3((float32x4_t)vandq_s32((int32x4_t)q.get128(), btvFFF0Mask));
#else
return btVector3(q.getX(),q.getY(),q.getZ());
return b3Vector3(q.getX(),q.getY(),q.getZ());
#endif
}
SIMD_FORCE_INLINE btQuaternion
shortestArcQuat(const btVector3& v0, const btVector3& v1) // Game Programming Gems 2.10. make sure v0,v1 are normalized
SIMD_FORCE_INLINE b3Quaternion
shortestArcQuat(const b3Vector3& v0, const b3Vector3& v1) // Game Programming Gems 2.10. make sure v0,v1 are normalized
{
btVector3 c = v0.cross(v1);
btScalar d = v0.dot(v1);
b3Vector3 c = v0.cross(v1);
b3Scalar d = v0.dot(v1);
if (d < -1.0 + SIMD_EPSILON)
{
btVector3 n,unused;
b3Vector3 n,unused;
btPlaneSpace1(v0,n,unused);
return btQuaternion(n.getX(),n.getY(),n.getZ(),0.0f); // just pick any vector that is orthogonal to v0
return b3Quaternion(n.getX(),n.getY(),n.getZ(),0.0f); // just pick any vector that is orthogonal to v0
}
btScalar s = btSqrt((1.0f + d) * 2.0f);
btScalar rs = 1.0f / s;
b3Scalar s = btSqrt((1.0f + d) * 2.0f);
b3Scalar rs = 1.0f / s;
return btQuaternion(c.getX()*rs,c.getY()*rs,c.getZ()*rs,s * 0.5f);
return b3Quaternion(c.getX()*rs,c.getY()*rs,c.getZ()*rs,s * 0.5f);
}
SIMD_FORCE_INLINE btQuaternion
shortestArcQuatNormalize2(btVector3& v0,btVector3& v1)
SIMD_FORCE_INLINE b3Quaternion
shortestArcQuatNormalize2(b3Vector3& v0,b3Vector3& v1)
{
v0.normalize();
v1.normalize();

2
src/BulletCommon/btQuickprof.cpp → src/BulletCommon/b3Quickprof.cpp
View File

@@ -13,7 +13,7 @@
// Credits: The Clock class was inspired by the Timer classes in
// Ogre (www.ogre3d.org).
#include "btQuickprof.h"
#include "b3Quickprof.h"
#ifndef BT_NO_PROFILE

4
src/BulletCommon/btQuickprof.h → src/BulletCommon/b3Quickprof.h
View File

@@ -19,8 +19,8 @@
//#define BT_NO_PROFILE 1
#ifndef BT_NO_PROFILE
#include <stdio.h>//@todo remove this, backwards compatibility
#include "btScalar.h"
#include "btAlignedAllocator.h"
#include "b3Scalar.h"
#include "b3AlignedAllocator.h"
#include <new>

0
src/BulletCommon/btRandom.h → src/BulletCommon/b3Random.h
View File

136
src/BulletCommon/btScalar.h → src/BulletCommon/b3Scalar.h
View File

@@ -249,13 +249,13 @@ inline int btGetVersion()
#endif
///The btScalar type abstracts floating point numbers, to easily switch between double and single floating point precision.
///The b3Scalar type abstracts floating point numbers, to easily switch between double and single floating point precision.
#if defined(BT_USE_DOUBLE_PRECISION)
typedef double btScalar;
typedef double b3Scalar;
//this number could be bigger in double precision
#define BT_LARGE_FLOAT 1e30
#else
typedef float btScalar;
typedef float b3Scalar;
//keep BT_LARGE_FLOAT*BT_LARGE_FLOAT < FLT_MAX
#define BT_LARGE_FLOAT 1e18f
#endif
@@ -339,23 +339,23 @@ typedef float32x4_t btSimdFloat4;
#if defined(BT_USE_DOUBLE_PRECISION) || defined(BT_FORCE_DOUBLE_FUNCTIONS)
SIMD_FORCE_INLINE btScalar btSqrt(btScalar x) { return sqrt(x); }
SIMD_FORCE_INLINE btScalar btFabs(btScalar x) { return fabs(x); }
SIMD_FORCE_INLINE btScalar btCos(btScalar x) { return cos(x); }
SIMD_FORCE_INLINE btScalar btSin(btScalar x) { return sin(x); }
SIMD_FORCE_INLINE btScalar btTan(btScalar x) { return tan(x); }
SIMD_FORCE_INLINE btScalar btAcos(btScalar x) { if (x<btScalar(-1)) x=btScalar(-1); if (x>btScalar(1)) x=btScalar(1); return acos(x); }
SIMD_FORCE_INLINE btScalar btAsin(btScalar x) { if (x<btScalar(-1)) x=btScalar(-1); if (x>btScalar(1)) x=btScalar(1); return asin(x); }
SIMD_FORCE_INLINE btScalar btAtan(btScalar x) { return atan(x); }
SIMD_FORCE_INLINE btScalar btAtan2(btScalar x, btScalar y) { return atan2(x, y); }
SIMD_FORCE_INLINE btScalar btExp(btScalar x) { return exp(x); }
SIMD_FORCE_INLINE btScalar btLog(btScalar x) { return log(x); }
SIMD_FORCE_INLINE btScalar btPow(btScalar x,btScalar y) { return pow(x,y); }
SIMD_FORCE_INLINE btScalar btFmod(btScalar x,btScalar y) { return fmod(x,y); }
SIMD_FORCE_INLINE b3Scalar btSqrt(b3Scalar x) { return sqrt(x); }
SIMD_FORCE_INLINE b3Scalar btFabs(b3Scalar x) { return fabs(x); }
SIMD_FORCE_INLINE b3Scalar btCos(b3Scalar x) { return cos(x); }
SIMD_FORCE_INLINE b3Scalar btSin(b3Scalar x) { return sin(x); }
SIMD_FORCE_INLINE b3Scalar btTan(b3Scalar x) { return tan(x); }
SIMD_FORCE_INLINE b3Scalar btAcos(b3Scalar x) { if (x<b3Scalar(-1)) x=b3Scalar(-1); if (x>b3Scalar(1)) x=b3Scalar(1); return acos(x); }
SIMD_FORCE_INLINE b3Scalar btAsin(b3Scalar x) { if (x<b3Scalar(-1)) x=b3Scalar(-1); if (x>b3Scalar(1)) x=b3Scalar(1); return asin(x); }
SIMD_FORCE_INLINE b3Scalar btAtan(b3Scalar x) { return atan(x); }
SIMD_FORCE_INLINE b3Scalar btAtan2(b3Scalar x, b3Scalar y) { return atan2(x, y); }
SIMD_FORCE_INLINE b3Scalar btExp(b3Scalar x) { return exp(x); }
SIMD_FORCE_INLINE b3Scalar btLog(b3Scalar x) { return log(x); }
SIMD_FORCE_INLINE b3Scalar btPow(b3Scalar x,b3Scalar y) { return pow(x,y); }
SIMD_FORCE_INLINE b3Scalar btFmod(b3Scalar x,b3Scalar y) { return fmod(x,y); }
#else
SIMD_FORCE_INLINE btScalar btSqrt(btScalar y)
SIMD_FORCE_INLINE b3Scalar btSqrt(b3Scalar y)
{
#ifdef USE_APPROXIMATION
double x, z, tempf;
@@ -364,52 +364,52 @@ SIMD_FORCE_INLINE btScalar btSqrt(btScalar y)
tempf = y;
*tfptr = (0xbfcdd90a - *tfptr)>>1; /* estimate of 1/sqrt(y) */
x = tempf;
z = y*btScalar(0.5);
x = (btScalar(1.5)*x)-(x*x)*(x*z); /* iteration formula */
x = (btScalar(1.5)*x)-(x*x)*(x*z);
x = (btScalar(1.5)*x)-(x*x)*(x*z);
x = (btScalar(1.5)*x)-(x*x)*(x*z);
x = (btScalar(1.5)*x)-(x*x)*(x*z);
z = y*b3Scalar(0.5);
x = (b3Scalar(1.5)*x)-(x*x)*(x*z); /* iteration formula */
x = (b3Scalar(1.5)*x)-(x*x)*(x*z);
x = (b3Scalar(1.5)*x)-(x*x)*(x*z);
x = (b3Scalar(1.5)*x)-(x*x)*(x*z);
x = (b3Scalar(1.5)*x)-(x*x)*(x*z);
return x*y;
#else
return sqrtf(y);
#endif
}
SIMD_FORCE_INLINE btScalar btFabs(btScalar x) { return fabsf(x); }
SIMD_FORCE_INLINE btScalar btCos(btScalar x) { return cosf(x); }
SIMD_FORCE_INLINE btScalar btSin(btScalar x) { return sinf(x); }
SIMD_FORCE_INLINE btScalar btTan(btScalar x) { return tanf(x); }
SIMD_FORCE_INLINE btScalar btAcos(btScalar x) {
if (x<btScalar(-1))
x=btScalar(-1);
if (x>btScalar(1))
x=btScalar(1);
SIMD_FORCE_INLINE b3Scalar btFabs(b3Scalar x) { return fabsf(x); }
SIMD_FORCE_INLINE b3Scalar btCos(b3Scalar x) { return cosf(x); }
SIMD_FORCE_INLINE b3Scalar btSin(b3Scalar x) { return sinf(x); }
SIMD_FORCE_INLINE b3Scalar btTan(b3Scalar x) { return tanf(x); }
SIMD_FORCE_INLINE b3Scalar btAcos(b3Scalar x) {
if (x<b3Scalar(-1))
x=b3Scalar(-1);
if (x>b3Scalar(1))
x=b3Scalar(1);
return acosf(x);
}
SIMD_FORCE_INLINE btScalar btAsin(btScalar x) {
if (x<btScalar(-1))
x=btScalar(-1);
if (x>btScalar(1))
x=btScalar(1);
SIMD_FORCE_INLINE b3Scalar btAsin(b3Scalar x) {
if (x<b3Scalar(-1))
x=b3Scalar(-1);
if (x>b3Scalar(1))
x=b3Scalar(1);
return asinf(x);
}
SIMD_FORCE_INLINE btScalar btAtan(btScalar x) { return atanf(x); }
SIMD_FORCE_INLINE btScalar btAtan2(btScalar x, btScalar y) { return atan2f(x, y); }
SIMD_FORCE_INLINE btScalar btExp(btScalar x) { return expf(x); }
SIMD_FORCE_INLINE btScalar btLog(btScalar x) { return logf(x); }
SIMD_FORCE_INLINE btScalar btPow(btScalar x,btScalar y) { return powf(x,y); }
SIMD_FORCE_INLINE btScalar btFmod(btScalar x,btScalar y) { return fmodf(x,y); }
SIMD_FORCE_INLINE b3Scalar btAtan(b3Scalar x) { return atanf(x); }
SIMD_FORCE_INLINE b3Scalar btAtan2(b3Scalar x, b3Scalar y) { return atan2f(x, y); }
SIMD_FORCE_INLINE b3Scalar btExp(b3Scalar x) { return expf(x); }
SIMD_FORCE_INLINE b3Scalar btLog(b3Scalar x) { return logf(x); }
SIMD_FORCE_INLINE b3Scalar btPow(b3Scalar x,b3Scalar y) { return powf(x,y); }
SIMD_FORCE_INLINE b3Scalar btFmod(b3Scalar x,b3Scalar y) { return fmodf(x,y); }
#endif
#define SIMD_2_PI btScalar(6.283185307179586232)
#define SIMD_PI (SIMD_2_PI * btScalar(0.5))
#define SIMD_HALF_PI (SIMD_2_PI * btScalar(0.25))
#define SIMD_RADS_PER_DEG (SIMD_2_PI / btScalar(360.0))
#define SIMD_DEGS_PER_RAD (btScalar(360.0) / SIMD_2_PI)
#define SIMDSQRT12 btScalar(0.7071067811865475244008443621048490)
#define SIMD_2_PI b3Scalar(6.283185307179586232)
#define SIMD_PI (SIMD_2_PI * b3Scalar(0.5))
#define SIMD_HALF_PI (SIMD_2_PI * b3Scalar(0.25))
#define SIMD_RADS_PER_DEG (SIMD_2_PI / b3Scalar(360.0))
#define SIMD_DEGS_PER_RAD (b3Scalar(360.0) / SIMD_2_PI)
#define SIMDSQRT12 b3Scalar(0.7071067811865475244008443621048490)
#define btRecipSqrt(x) ((btScalar)(btScalar(1.0)/btSqrt(btScalar(x)))) /* reciprocal square root */
#define btRecipSqrt(x) ((b3Scalar)(b3Scalar(1.0)/btSqrt(b3Scalar(x)))) /* reciprocal square root */
#ifdef BT_USE_DOUBLE_PRECISION
@@ -420,48 +420,48 @@ SIMD_FORCE_INLINE btScalar btFmod(btScalar x,btScalar y) { return fmodf(x,y); }
#define SIMD_INFINITY FLT_MAX
#endif
SIMD_FORCE_INLINE btScalar btAtan2Fast(btScalar y, btScalar x)
SIMD_FORCE_INLINE b3Scalar btAtan2Fast(b3Scalar y, b3Scalar x)
{
btScalar coeff_1 = SIMD_PI / 4.0f;
btScalar coeff_2 = 3.0f * coeff_1;
btScalar abs_y = btFabs(y);
btScalar angle;
b3Scalar coeff_1 = SIMD_PI / 4.0f;
b3Scalar coeff_2 = 3.0f * coeff_1;
b3Scalar abs_y = btFabs(y);
b3Scalar angle;
if (x >= 0.0f) {
btScalar r = (x - abs_y) / (x + abs_y);
b3Scalar r = (x - abs_y) / (x + abs_y);
angle = coeff_1 - coeff_1 * r;
} else {
btScalar r = (x + abs_y) / (abs_y - x);
b3Scalar r = (x + abs_y) / (abs_y - x);
angle = coeff_2 - coeff_1 * r;
}
return (y < 0.0f) ? -angle : angle;
}
SIMD_FORCE_INLINE bool btFuzzyZero(btScalar x) { return btFabs(x) < SIMD_EPSILON; }
SIMD_FORCE_INLINE bool btFuzzyZero(b3Scalar x) { return btFabs(x) < SIMD_EPSILON; }
SIMD_FORCE_INLINE bool btEqual(btScalar a, btScalar eps) {
SIMD_FORCE_INLINE bool btEqual(b3Scalar a, b3Scalar eps) {
return (((a) <= eps) && !((a) < -eps));
}
SIMD_FORCE_INLINE bool btGreaterEqual (btScalar a, btScalar eps) {
SIMD_FORCE_INLINE bool btGreaterEqual (b3Scalar a, b3Scalar eps) {
return (!((a) <= eps));
}
SIMD_FORCE_INLINE int btIsNegative(btScalar x) {
return x < btScalar(0.0) ? 1 : 0;
SIMD_FORCE_INLINE int btIsNegative(b3Scalar x) {
return x < b3Scalar(0.0) ? 1 : 0;
}
SIMD_FORCE_INLINE btScalar btRadians(btScalar x) { return x * SIMD_RADS_PER_DEG; }
SIMD_FORCE_INLINE btScalar btDegrees(btScalar x) { return x * SIMD_DEGS_PER_RAD; }
SIMD_FORCE_INLINE b3Scalar btRadians(b3Scalar x) { return x * SIMD_RADS_PER_DEG; }
SIMD_FORCE_INLINE b3Scalar btDegrees(b3Scalar x) { return x * SIMD_DEGS_PER_RAD; }
#define BT_DECLARE_HANDLE(name) typedef struct name##__ { int unused; } *name
#ifndef btFsel
SIMD_FORCE_INLINE btScalar btFsel(btScalar a, btScalar b, btScalar c)
SIMD_FORCE_INLINE b3Scalar btFsel(b3Scalar a, b3Scalar b, b3Scalar c)
{
return a >= 0 ? b : c;
}
#endif
#define btFsels(a,b,c) (btScalar)btFsel(a,b,c)
#define btFsels(a,b,c) (b3Scalar)btFsel(a,b,c)
SIMD_FORCE_INLINE bool btMachineIsLittleEndian()
@@ -497,7 +497,7 @@ SIMD_FORCE_INLINE int btSelect(unsigned condition, int valueIfConditionNonZero,
SIMD_FORCE_INLINE float btSelect(unsigned condition, float valueIfConditionNonZero, float valueIfConditionZero)
{
#ifdef BT_HAVE_NATIVE_FSEL
return (float)btFsel((btScalar)condition - btScalar(1.0f), valueIfConditionNonZero, valueIfConditionZero);
return (float)btFsel((b3Scalar)condition - b3Scalar(1.0f), valueIfConditionNonZero, valueIfConditionZero);
#else
return (condition != 0) ? valueIfConditionNonZero : valueIfConditionZero;
#endif
@@ -602,7 +602,7 @@ SIMD_FORCE_INLINE double btUnswapEndianDouble(const unsigned char *src)
}
// returns normalized value in range [-SIMD_PI, SIMD_PI]
SIMD_FORCE_INLINE btScalar btNormalizeAngle(btScalar angleInRadians)
SIMD_FORCE_INLINE b3Scalar btNormalizeAngle(b3Scalar angleInRadians)
{
angleInRadians = btFmod(angleInRadians, SIMD_2_PI);
if(angleInRadians < -SIMD_PI)

12
src/BulletCommon/btStackAlloc.h → src/BulletCommon/b3StackAlloc.h
View File

@@ -20,10 +20,10 @@ Nov.2006
#ifndef BT_STACK_ALLOC
#define BT_STACK_ALLOC
#include "btScalar.h" //for btAssert
#include "btAlignedAllocator.h"
#include "b3Scalar.h" //for btAssert
#include "b3AlignedAllocator.h"
///The btBlock class is an internal structure for the btStackAlloc memory allocator.
///The btBlock class is an internal structure for the b3StackAlloc memory allocator.
struct btBlock
{
btBlock* previous;
@@ -31,12 +31,12 @@ struct btBlock
};
///The StackAlloc class provides some fast stack-based memory allocator (LIFO last-in first-out)
class btStackAlloc
class b3StackAlloc
{
public:
btStackAlloc(unsigned int size) { ctor();create(size); }
~btStackAlloc() { destroy(); }
b3StackAlloc(unsigned int size) { ctor();create(size); }
~b3StackAlloc() { destroy(); }
inline void create(unsigned int size)
{

124
src/BulletCommon/btTransform.h → src/BulletCommon/b3Transform.h
View File

@@ -18,7 +18,7 @@ subject to the following restrictions:
#define BT_TRANSFORM_H
#include "btMatrix3x3.h"
#include "b3Matrix3x3.h"
#ifdef BT_USE_DOUBLE_PRECISION
#define btTransformData btTransformDoubleData
@@ -29,44 +29,44 @@ subject to the following restrictions:
/**@brief The btTransform class supports rigid transforms with only translation and rotation and no scaling/shear.
*It can be used in combination with btVector3, btQuaternion and btMatrix3x3 linear algebra classes. */
ATTRIBUTE_ALIGNED16(class) btTransform {
/**@brief The b3Transform class supports rigid transforms with only translation and rotation and no scaling/shear.
*It can be used in combination with b3Vector3, b3Quaternion and b3Matrix3x3 linear algebra classes. */
ATTRIBUTE_ALIGNED16(class) b3Transform {
///Storage for the rotation
btMatrix3x3 m_basis;
b3Matrix3x3 m_basis;
///Storage for the translation
btVector3 m_origin;
b3Vector3 m_origin;
public:
/**@brief No initialization constructor */
btTransform() {}
/**@brief Constructor from btQuaternion (optional btVector3 )
b3Transform() {}
/**@brief Constructor from b3Quaternion (optional b3Vector3 )
* @param q Rotation from quaternion
* @param c Translation from Vector (default 0,0,0) */
explicit SIMD_FORCE_INLINE btTransform(const btQuaternion& q,
const btVector3& c = btVector3(btScalar(0), btScalar(0), btScalar(0)))
explicit SIMD_FORCE_INLINE b3Transform(const b3Quaternion& q,
const b3Vector3& c = b3Vector3(b3Scalar(0), b3Scalar(0), b3Scalar(0)))
: m_basis(q),
m_origin(c)
{}
/**@brief Constructor from btMatrix3x3 (optional btVector3)
/**@brief Constructor from b3Matrix3x3 (optional b3Vector3)
* @param b Rotation from Matrix
* @param c Translation from Vector default (0,0,0)*/
explicit SIMD_FORCE_INLINE btTransform(const btMatrix3x3& b,
const btVector3& c = btVector3(btScalar(0), btScalar(0), btScalar(0)))
explicit SIMD_FORCE_INLINE b3Transform(const b3Matrix3x3& b,
const b3Vector3& c = b3Vector3(b3Scalar(0), b3Scalar(0), b3Scalar(0)))
: m_basis(b),
m_origin(c)
{}
/**@brief Copy constructor */
SIMD_FORCE_INLINE btTransform (const btTransform& other)
SIMD_FORCE_INLINE b3Transform (const b3Transform& other)
: m_basis(other.m_basis),
m_origin(other.m_origin)
{
}
/**@brief Assignment Operator */
SIMD_FORCE_INLINE btTransform& operator=(const btTransform& other)
SIMD_FORCE_INLINE b3Transform& operator=(const b3Transform& other)
{
m_basis = other.m_basis;
m_origin = other.m_origin;
@@ -78,49 +78,49 @@ public:
* @param t1 Transform 1
* @param t2 Transform 2
* This = Transform1 * Transform2 */
SIMD_FORCE_INLINE void mult(const btTransform& t1, const btTransform& t2) {
SIMD_FORCE_INLINE void mult(const b3Transform& t1, const b3Transform& t2) {
m_basis = t1.m_basis * t2.m_basis;
m_origin = t1(t2.m_origin);
}
/* void multInverseLeft(const btTransform& t1, const btTransform& t2) {
btVector3 v = t2.m_origin - t1.m_origin;
/* void multInverseLeft(const b3Transform& t1, const b3Transform& t2) {
b3Vector3 v = t2.m_origin - t1.m_origin;
m_basis = btMultTransposeLeft(t1.m_basis, t2.m_basis);
m_origin = v * t1.m_basis;
}
*/
/**@brief Return the transform of the vector */
SIMD_FORCE_INLINE btVector3 operator()(const btVector3& x) const
SIMD_FORCE_INLINE b3Vector3 operator()(const b3Vector3& x) const
{
return x.dot3(m_basis[0], m_basis[1], m_basis[2]) + m_origin;
}
/**@brief Return the transform of the vector */
SIMD_FORCE_INLINE btVector3 operator*(const btVector3& x) const
SIMD_FORCE_INLINE b3Vector3 operator*(const b3Vector3& x) const
{
return (*this)(x);
}
/**@brief Return the transform of the btQuaternion */
SIMD_FORCE_INLINE btQuaternion operator*(const btQuaternion& q) const
/**@brief Return the transform of the b3Quaternion */
SIMD_FORCE_INLINE b3Quaternion operator*(const b3Quaternion& q) const
{
return getRotation() * q;
}
/**@brief Return the basis matrix for the rotation */
SIMD_FORCE_INLINE btMatrix3x3& getBasis() { return m_basis; }
SIMD_FORCE_INLINE b3Matrix3x3& getBasis() { return m_basis; }
/**@brief Return the basis matrix for the rotation */
SIMD_FORCE_INLINE const btMatrix3x3& getBasis() const { return m_basis; }
SIMD_FORCE_INLINE const b3Matrix3x3& getBasis() const { return m_basis; }
/**@brief Return the origin vector translation */
SIMD_FORCE_INLINE btVector3& getOrigin() { return m_origin; }
SIMD_FORCE_INLINE b3Vector3& getOrigin() { return m_origin; }
/**@brief Return the origin vector translation */
SIMD_FORCE_INLINE const btVector3& getOrigin() const { return m_origin; }
SIMD_FORCE_INLINE const b3Vector3& getOrigin() const { return m_origin; }
/**@brief Return a quaternion representing the rotation */
btQuaternion getRotation() const {
btQuaternion q;
b3Quaternion getRotation() const {
b3Quaternion q;
m_basis.getRotation(q);
return q;
}
@@ -128,7 +128,7 @@ public:
/**@brief Set from an array
* @param m A pointer to a 15 element array (12 rotation(row major padded on the right by 1), and 3 translation */
void setFromOpenGLMatrix(const btScalar *m)
void setFromOpenGLMatrix(const b3Scalar *m)
{
m_basis.setFromOpenGLSubMatrix(m);
m_origin.setValue(m[12],m[13],m[14]);
@@ -136,33 +136,33 @@ public:
/**@brief Fill an array representation
* @param m A pointer to a 15 element array (12 rotation(row major padded on the right by 1), and 3 translation */
void getOpenGLMatrix(btScalar *m) const
void getOpenGLMatrix(b3Scalar *m) const
{
m_basis.getOpenGLSubMatrix(m);
m[12] = m_origin.getX();
m[13] = m_origin.getY();
m[14] = m_origin.getZ();
m[15] = btScalar(1.0);
m[15] = b3Scalar(1.0);
}
/**@brief Set the translational element
* @param origin The vector to set the translation to */
SIMD_FORCE_INLINE void setOrigin(const btVector3& origin)
SIMD_FORCE_INLINE void setOrigin(const b3Vector3& origin)
{
m_origin = origin;
}
SIMD_FORCE_INLINE btVector3 invXform(const btVector3& inVec) const;
SIMD_FORCE_INLINE b3Vector3 invXform(const b3Vector3& inVec) const;
/**@brief Set the rotational element by btMatrix3x3 */
SIMD_FORCE_INLINE void setBasis(const btMatrix3x3& basis)
/**@brief Set the rotational element by b3Matrix3x3 */
SIMD_FORCE_INLINE void setBasis(const b3Matrix3x3& basis)
{
m_basis = basis;
}
/**@brief Set the rotational element by btQuaternion */
SIMD_FORCE_INLINE void setRotation(const btQuaternion& q)
/**@brief Set the rotational element by b3Quaternion */
SIMD_FORCE_INLINE void setRotation(const b3Quaternion& q)
{
m_basis.setRotation(q);
}
@@ -172,12 +172,12 @@ public:
void setIdentity()
{
m_basis.setIdentity();
m_origin.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0));
m_origin.setValue(b3Scalar(0.0), b3Scalar(0.0), b3Scalar(0.0));
}
/**@brief Multiply this Transform by another(this = this * another)
* @param t The other transform */
btTransform& operator*=(const btTransform& t)
b3Transform& operator*=(const b3Transform& t)
{
m_origin += m_basis * t.m_origin;
m_basis *= t.m_basis;
@@ -185,24 +185,24 @@ public:
}
/**@brief Return the inverse of this transform */
btTransform inverse() const
b3Transform inverse() const
{
btMatrix3x3 inv = m_basis.transpose();
return btTransform(inv, inv * -m_origin);
b3Matrix3x3 inv = m_basis.transpose();
return b3Transform(inv, inv * -m_origin);
}
/**@brief Return the inverse of this transform times the other transform
* @param t The other transform
* return this.inverse() * the other */
btTransform inverseTimes(const btTransform& t) const;
b3Transform inverseTimes(const b3Transform& t) const;
/**@brief Return the product of this transform and the other */
btTransform operator*(const btTransform& t) const;
b3Transform operator*(const b3Transform& t) const;
/**@brief Return an identity transform */
static const btTransform& getIdentity()
static const b3Transform& getIdentity()
{
static const btTransform identityTransform(btMatrix3x3::getIdentity());
static const b3Transform identityTransform(b3Matrix3x3::getIdentity());
return identityTransform;
}
@@ -219,30 +219,30 @@ public:
};
SIMD_FORCE_INLINE btVector3
btTransform::invXform(const btVector3& inVec) const
SIMD_FORCE_INLINE b3Vector3
b3Transform::invXform(const b3Vector3& inVec) const
{
btVector3 v = inVec - m_origin;
b3Vector3 v = inVec - m_origin;
return (m_basis.transpose() * v);
}
SIMD_FORCE_INLINE btTransform
btTransform::inverseTimes(const btTransform& t) const
SIMD_FORCE_INLINE b3Transform
b3Transform::inverseTimes(const b3Transform& t) const
{
btVector3 v = t.getOrigin() - m_origin;
return btTransform(m_basis.transposeTimes(t.m_basis),
b3Vector3 v = t.getOrigin() - m_origin;
return b3Transform(m_basis.transposeTimes(t.m_basis),
v * m_basis);
}
SIMD_FORCE_INLINE btTransform
btTransform::operator*(const btTransform& t) const
SIMD_FORCE_INLINE b3Transform
b3Transform::operator*(const b3Transform& t) const
{
return btTransform(m_basis * t.m_basis,
return b3Transform(m_basis * t.m_basis,
(*this)(t.m_origin));
}
/**@brief Test if two transforms have all elements equal */
SIMD_FORCE_INLINE bool operator==(const btTransform& t1, const btTransform& t2)
SIMD_FORCE_INLINE bool operator==(const b3Transform& t1, const b3Transform& t2)
{
return ( t1.getBasis() == t2.getBasis() &&
t1.getOrigin() == t2.getOrigin() );
@@ -264,32 +264,32 @@ struct btTransformDoubleData
SIMD_FORCE_INLINE void btTransform::serialize(btTransformData& dataOut) const
SIMD_FORCE_INLINE void b3Transform::serialize(btTransformData& dataOut) const
{
m_basis.serialize(dataOut.m_basis);
m_origin.serialize(dataOut.m_origin);
}
SIMD_FORCE_INLINE void btTransform::serializeFloat(btTransformFloatData& dataOut) const
SIMD_FORCE_INLINE void b3Transform::serializeFloat(btTransformFloatData& dataOut) const
{
m_basis.serializeFloat(dataOut.m_basis);
m_origin.serializeFloat(dataOut.m_origin);
}
SIMD_FORCE_INLINE void btTransform::deSerialize(const btTransformData& dataIn)
SIMD_FORCE_INLINE void b3Transform::deSerialize(const btTransformData& dataIn)
{
m_basis.deSerialize(dataIn.m_basis);
m_origin.deSerialize(dataIn.m_origin);
}
SIMD_FORCE_INLINE void btTransform::deSerializeFloat(const btTransformFloatData& dataIn)
SIMD_FORCE_INLINE void b3Transform::deSerializeFloat(const btTransformFloatData& dataIn)
{
m_basis.deSerializeFloat(dataIn.m_basis);
m_origin.deSerializeFloat(dataIn.m_origin);
}
SIMD_FORCE_INLINE void btTransform::deSerializeDouble(const btTransformDoubleData& dataIn)
SIMD_FORCE_INLINE void b3Transform::deSerializeDouble(const btTransformDoubleData& dataIn)
{
m_basis.deSerializeDouble(dataIn.m_basis);
m_origin.deSerializeDouble(dataIn.m_origin);

228
src/BulletCommon/b3TransformUtil.h Normal file
View File

@@ -0,0 +1,228 @@
/*
Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans http://continuousphysics.com/Bullet/
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef BT_TRANSFORM_UTIL_H
#define BT_TRANSFORM_UTIL_H
#include "b3Transform.h"
#define ANGULAR_MOTION_THRESHOLD b3Scalar(0.5)*SIMD_HALF_PI
SIMD_FORCE_INLINE b3Vector3 btAabbSupport(const b3Vector3& halfExtents,const b3Vector3& supportDir)
{
return b3Vector3(supportDir.getX() < b3Scalar(0.0) ? -halfExtents.getX() : halfExtents.getX(),
supportDir.getY() < b3Scalar(0.0) ? -halfExtents.getY() : halfExtents.getY(),
supportDir.getZ() < b3Scalar(0.0) ? -halfExtents.getZ() : halfExtents.getZ());
}
/// Utils related to temporal transforms
class b3TransformUtil
{
public:
static void integrateTransform(const b3Transform& curTrans,const b3Vector3& linvel,const b3Vector3& angvel,b3Scalar timeStep,b3Transform& predictedTransform)
{
predictedTransform.setOrigin(curTrans.getOrigin() + linvel * timeStep);
// #define QUATERNION_DERIVATIVE
#ifdef QUATERNION_DERIVATIVE
b3Quaternion predictedOrn = curTrans.getRotation();
predictedOrn += (angvel * predictedOrn) * (timeStep * b3Scalar(0.5));
predictedOrn.normalize();
#else
//Exponential map
//google for "Practical Parameterization of Rotations Using the Exponential Map", F. Sebastian Grassia
b3Vector3 axis;
b3Scalar fAngle = angvel.length();
//limit the angular motion
if (fAngle*timeStep > ANGULAR_MOTION_THRESHOLD)
{
fAngle = ANGULAR_MOTION_THRESHOLD / timeStep;
}
if ( fAngle < b3Scalar(0.001) )
{
// use Taylor's expansions of sync function
axis = angvel*( b3Scalar(0.5)*timeStep-(timeStep*timeStep*timeStep)*(b3Scalar(0.020833333333))*fAngle*fAngle );
}
else
{
// sync(fAngle) = sin(c*fAngle)/t
axis = angvel*( btSin(b3Scalar(0.5)*fAngle*timeStep)/fAngle );
}
b3Quaternion dorn (axis.getX(),axis.getY(),axis.getZ(),btCos( fAngle*timeStep*b3Scalar(0.5) ));
b3Quaternion orn0 = curTrans.getRotation();
b3Quaternion predictedOrn = dorn * orn0;
predictedOrn.normalize();
#endif
predictedTransform.setRotation(predictedOrn);
}
static void calculateVelocityQuaternion(const b3Vector3& pos0,const b3Vector3& pos1,const b3Quaternion& orn0,const b3Quaternion& orn1,b3Scalar timeStep,b3Vector3& linVel,b3Vector3& angVel)
{
linVel = (pos1 - pos0) / timeStep;
b3Vector3 axis;
b3Scalar angle;
if (orn0 != orn1)
{
calculateDiffAxisAngleQuaternion(orn0,orn1,axis,angle);
angVel = axis * angle / timeStep;
} else
{
angVel.setValue(0,0,0);
}
}
static void calculateDiffAxisAngleQuaternion(const b3Quaternion& orn0,const b3Quaternion& orn1a,b3Vector3& axis,b3Scalar& angle)
{
b3Quaternion orn1 = orn0.nearest(orn1a);
b3Quaternion dorn = orn1 * orn0.inverse();
angle = dorn.getAngle();
axis = b3Vector3(dorn.getX(),dorn.getY(),dorn.getZ());
axis[3] = b3Scalar(0.);
//check for axis length
b3Scalar len = axis.length2();
if (len < SIMD_EPSILON*SIMD_EPSILON)
axis = b3Vector3(b3Scalar(1.),b3Scalar(0.),b3Scalar(0.));
else
axis /= btSqrt(len);
}
static void calculateVelocity(const b3Transform& transform0,const b3Transform& transform1,b3Scalar timeStep,b3Vector3& linVel,b3Vector3& angVel)
{
linVel = (transform1.getOrigin() - transform0.getOrigin()) / timeStep;
b3Vector3 axis;
b3Scalar angle;
calculateDiffAxisAngle(transform0,transform1,axis,angle);
angVel = axis * angle / timeStep;
}
static void calculateDiffAxisAngle(const b3Transform& transform0,const b3Transform& transform1,b3Vector3& axis,b3Scalar& angle)
{
b3Matrix3x3 dmat = transform1.getBasis() * transform0.getBasis().inverse();
b3Quaternion dorn;
dmat.getRotation(dorn);
///floating point inaccuracy can lead to w component > 1..., which breaks
dorn.normalize();
angle = dorn.getAngle();
axis = b3Vector3(dorn.getX(),dorn.getY(),dorn.getZ());
axis[3] = b3Scalar(0.);
//check for axis length
b3Scalar len = axis.length2();
if (len < SIMD_EPSILON*SIMD_EPSILON)
axis = b3Vector3(b3Scalar(1.),b3Scalar(0.),b3Scalar(0.));
else
axis /= btSqrt(len);
}
};
///The btConvexSeparatingDistanceUtil can help speed up convex collision detection
///by conservatively updating a cached separating distance/vector instead of re-calculating the closest distance
class btConvexSeparatingDistanceUtil
{
b3Quaternion m_ornA;
b3Quaternion m_ornB;
b3Vector3 m_posA;
b3Vector3 m_posB;
b3Vector3 m_separatingNormal;
b3Scalar m_boundingRadiusA;
b3Scalar m_boundingRadiusB;
b3Scalar m_separatingDistance;
public:
btConvexSeparatingDistanceUtil(b3Scalar boundingRadiusA,b3Scalar boundingRadiusB)
:m_boundingRadiusA(boundingRadiusA),
m_boundingRadiusB(boundingRadiusB),
m_separatingDistance(0.f)
{
}
b3Scalar getConservativeSeparatingDistance()
{
return m_separatingDistance;
}
void updateSeparatingDistance(const b3Transform& transA,const b3Transform& transB)
{
const b3Vector3& toPosA = transA.getOrigin();
const b3Vector3& toPosB = transB.getOrigin();
b3Quaternion toOrnA = transA.getRotation();
b3Quaternion toOrnB = transB.getRotation();
if (m_separatingDistance>0.f)
{
b3Vector3 linVelA,angVelA,linVelB,angVelB;
b3TransformUtil::calculateVelocityQuaternion(m_posA,toPosA,m_ornA,toOrnA,b3Scalar(1.),linVelA,angVelA);
b3TransformUtil::calculateVelocityQuaternion(m_posB,toPosB,m_ornB,toOrnB,b3Scalar(1.),linVelB,angVelB);
b3Scalar maxAngularProjectedVelocity = angVelA.length() * m_boundingRadiusA + angVelB.length() * m_boundingRadiusB;
b3Vector3 relLinVel = (linVelB-linVelA);
b3Scalar relLinVelocLength = relLinVel.dot(m_separatingNormal);
if (relLinVelocLength<0.f)
{
relLinVelocLength = 0.f;
}
b3Scalar projectedMotion = maxAngularProjectedVelocity +relLinVelocLength;
m_separatingDistance -= projectedMotion;
}
m_posA = toPosA;
m_posB = toPosB;
m_ornA = toOrnA;
m_ornB = toOrnB;
}
void initSeparatingDistance(const b3Vector3& separatingVector,b3Scalar separatingDistance,const b3Transform& transA,const b3Transform& transB)
{
m_separatingDistance = separatingDistance;
if (m_separatingDistance>0.f)
{
m_separatingNormal = separatingVector;
const b3Vector3& toPosA = transA.getOrigin();
const b3Vector3& toPosB = transB.getOrigin();
b3Quaternion toOrnA = transA.getRotation();
b3Quaternion toOrnB = transB.getRotation();
m_posA = toPosA;
m_posB = toPosB;
m_ornA = toOrnA;
m_ornB = toOrnB;
}
}
};
#endif //BT_TRANSFORM_UTIL_H

2
src/BulletCommon/btVector3.cpp → src/BulletCommon/b3Vector3.cpp
View File

@@ -19,7 +19,7 @@
#define BT_USE_SSE_IN_API
#endif
#include "btVector3.h"
#include "b3Vector3.h"
#if defined (BT_USE_SSE) || defined (BT_USE_NEON)

358
src/BulletCommon/btVector3.h → src/BulletCommon/b3Vector3.h
View File

@@ -18,9 +18,9 @@ subject to the following restrictions:
#define BT_VECTOR3_H
//#include <stdint.h>
#include "btScalar.h"
#include "btMinMax.h"
#include "btAlignedAllocator.h"
#include "b3Scalar.h"
#include "b3MinMax.h"
#include "b3AlignedAllocator.h"
#ifdef BT_USE_DOUBLE_PRECISION
#define btVector3Data btVector3DoubleData
@@ -71,18 +71,18 @@ const int32x4_t ATTRIBUTE_ALIGNED16(btv3AbsMask) = (int32x4_t){0x7FFFFFFF, 0x7FF
#endif
/**@brief btVector3 can be used to represent 3D points and vectors.
* It has an un-used w component to suit 16-byte alignment when btVector3 is stored in containers. This extra component can be used by derived classes (Quaternion?) or by user
/**@brief b3Vector3 can be used to represent 3D points and vectors.
* It has an un-used w component to suit 16-byte alignment when b3Vector3 is stored in containers. This extra component can be used by derived classes (Quaternion?) or by user
* Ideally, this class should be replaced by a platform optimized SIMD version that keeps the data in registers
*/
ATTRIBUTE_ALIGNED16(class) btVector3
ATTRIBUTE_ALIGNED16(class) b3Vector3
{
public:
BT_DECLARE_ALIGNED_ALLOCATOR();
#if defined (__SPU__) && defined (__CELLOS_LV2__)
btScalar m_floats[4];
b3Scalar m_floats[4];
public:
SIMD_FORCE_INLINE const vec_float4& get128() const
{
@@ -93,8 +93,8 @@ public:
#if defined (BT_USE_SSE) || defined(BT_USE_NEON) // _WIN32 || ARM
union {
btSimdFloat4 mVec128;
btScalar m_floats[4];
struct {btScalar x,y,z,w;};
b3Scalar m_floats[4];
struct {b3Scalar x,y,z,w;};
};
SIMD_FORCE_INLINE btSimdFloat4 get128() const
@@ -106,14 +106,14 @@ public:
mVec128 = v128;
}
#else
btScalar m_floats[4];
b3Scalar m_floats[4];
#endif
#endif //__CELLOS_LV2__ __SPU__
public:
/**@brief No initialization constructor */
SIMD_FORCE_INLINE btVector3()
SIMD_FORCE_INLINE b3Vector3()
{
}
@@ -125,30 +125,30 @@ public:
* @param y Y value
* @param z Z value
*/
SIMD_FORCE_INLINE btVector3(const btScalar& _x, const btScalar& _y, const btScalar& _z)
SIMD_FORCE_INLINE b3Vector3(const b3Scalar& _x, const b3Scalar& _y, const b3Scalar& _z)
{
m_floats[0] = _x;
m_floats[1] = _y;
m_floats[2] = _z;
m_floats[3] = btScalar(0.f);
m_floats[3] = b3Scalar(0.f);
}
#if (defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE) )|| defined (BT_USE_NEON)
// Set Vector
SIMD_FORCE_INLINE btVector3( btSimdFloat4 v)
SIMD_FORCE_INLINE b3Vector3( btSimdFloat4 v)
{
mVec128 = v;
}
// Copy constructor
SIMD_FORCE_INLINE btVector3(const btVector3& rhs)
SIMD_FORCE_INLINE b3Vector3(const b3Vector3& rhs)
{
mVec128 = rhs.mVec128;
}
// Assignment Operator
SIMD_FORCE_INLINE btVector3&
operator=(const btVector3& v)
SIMD_FORCE_INLINE b3Vector3&
operator=(const b3Vector3& v)
{
mVec128 = v.mVec128;
@@ -158,7 +158,7 @@ public:
/**@brief Add a vector to this one
* @param The vector to add to this one */
SIMD_FORCE_INLINE btVector3& operator+=(const btVector3& v)
SIMD_FORCE_INLINE b3Vector3& operator+=(const b3Vector3& v)
{
#if defined(BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
mVec128 = _mm_add_ps(mVec128, v.mVec128);
@@ -175,7 +175,7 @@ public:
/**@brief Subtract a vector from this one
* @param The vector to subtract */
SIMD_FORCE_INLINE btVector3& operator-=(const btVector3& v)
SIMD_FORCE_INLINE b3Vector3& operator-=(const b3Vector3& v)
{
#if defined(BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
mVec128 = _mm_sub_ps(mVec128, v.mVec128);
@@ -191,7 +191,7 @@ public:
/**@brief Scale the vector
* @param s Scale factor */
SIMD_FORCE_INLINE btVector3& operator*=(const btScalar& s)
SIMD_FORCE_INLINE b3Vector3& operator*=(const b3Scalar& s)
{
#if defined(BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
__m128 vs = _mm_load_ss(&s); // (S 0 0 0)
@@ -209,9 +209,9 @@ public:
/**@brief Inversely scale the vector
* @param s Scale factor to divide by */
SIMD_FORCE_INLINE btVector3& operator/=(const btScalar& s)
SIMD_FORCE_INLINE b3Vector3& operator/=(const b3Scalar& s)
{
btFullAssert(s != btScalar(0.0));
btFullAssert(s != b3Scalar(0.0));
#if 0 //defined(BT_USE_SSE_IN_API)
// this code is not faster !
@@ -223,13 +223,13 @@ public:
return *this;
#else
return *this *= btScalar(1.0) / s;
return *this *= b3Scalar(1.0) / s;
#endif
}
/**@brief Return the dot product
* @param v The other vector in the dot product */
SIMD_FORCE_INLINE btScalar dot(const btVector3& v) const
SIMD_FORCE_INLINE b3Scalar dot(const b3Vector3& v) const
{
#if defined(BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
__m128 vd = _mm_mul_ps(mVec128, v.mVec128);
@@ -251,28 +251,28 @@ public:
}
/**@brief Return the length of the vector squared */
SIMD_FORCE_INLINE btScalar length2() const
SIMD_FORCE_INLINE b3Scalar length2() const
{
return dot(*this);
}
/**@brief Return the length of the vector */
SIMD_FORCE_INLINE btScalar length() const
SIMD_FORCE_INLINE b3Scalar length() const
{
return btSqrt(length2());
}
/**@brief Return the distance squared between the ends of this and another vector
* This is symantically treating the vector like a point */
SIMD_FORCE_INLINE btScalar distance2(const btVector3& v) const;
SIMD_FORCE_INLINE b3Scalar distance2(const b3Vector3& v) const;
/**@brief Return the distance between the ends of this and another vector
* This is symantically treating the vector like a point */
SIMD_FORCE_INLINE btScalar distance(const btVector3& v) const;
SIMD_FORCE_INLINE b3Scalar distance(const b3Vector3& v) const;
SIMD_FORCE_INLINE btVector3& safeNormalize()
SIMD_FORCE_INLINE b3Vector3& safeNormalize()
{
btVector3 absVec = this->absolute();
b3Vector3 absVec = this->absolute();
int maxIndex = absVec.maxAxis();
if (absVec[maxIndex]>0)
{
@@ -285,7 +285,7 @@ public:
/**@brief Normalize this vector
* x^2 + y^2 + z^2 = 1 */
SIMD_FORCE_INLINE btVector3& normalize()
SIMD_FORCE_INLINE b3Vector3& normalize()
{
#if defined(BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
// dot product first
@@ -328,31 +328,31 @@ public:
}
/**@brief Return a normalized version of this vector */
SIMD_FORCE_INLINE btVector3 normalized() const;
SIMD_FORCE_INLINE b3Vector3 normalized() const;
/**@brief Return a rotated version of this vector
* @param wAxis The axis to rotate about
* @param angle The angle to rotate by */
SIMD_FORCE_INLINE btVector3 rotate( const btVector3& wAxis, const btScalar angle ) const;
SIMD_FORCE_INLINE b3Vector3 rotate( const b3Vector3& wAxis, const b3Scalar angle ) const;
/**@brief Return the angle between this and another vector
* @param v The other vector */
SIMD_FORCE_INLINE btScalar angle(const btVector3& v) const
SIMD_FORCE_INLINE b3Scalar angle(const b3Vector3& v) const
{
btScalar s = btSqrt(length2() * v.length2());
btFullAssert(s != btScalar(0.0));
b3Scalar s = btSqrt(length2() * v.length2());
btFullAssert(s != b3Scalar(0.0));
return btAcos(dot(v) / s);
}
/**@brief Return a vector will the absolute values of each element */
SIMD_FORCE_INLINE btVector3 absolute() const
SIMD_FORCE_INLINE b3Vector3 absolute() const
{
#if defined(BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
return btVector3(_mm_and_ps(mVec128, btv3AbsfMask));
return b3Vector3(_mm_and_ps(mVec128, btv3AbsfMask));
#elif defined(BT_USE_NEON)
return btVector3(vabsq_f32(mVec128));
return b3Vector3(vabsq_f32(mVec128));
#else
return btVector3(
return b3Vector3(
btFabs(m_floats[0]),
btFabs(m_floats[1]),
btFabs(m_floats[2]));
@@ -361,7 +361,7 @@ public:
/**@brief Return the cross product between this and another vector
* @param v The other vector */
SIMD_FORCE_INLINE btVector3 cross(const btVector3& v) const
SIMD_FORCE_INLINE b3Vector3 cross(const b3Vector3& v) const
{
#if defined(BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
__m128 T, V;
@@ -374,7 +374,7 @@ public:
V = _mm_sub_ps(V, T);
V = bt_pshufd_ps(V, BT_SHUFFLE(1, 2, 0, 3));
return btVector3(V);
return b3Vector3(V);
#elif defined(BT_USE_NEON)
float32x4_t T, V;
// form (Y, Z, X, _) of mVec128 and v.mVec128
@@ -391,16 +391,16 @@ public:
V = vcombine_f32(vext_f32(Vlow, vget_high_f32(V), 1), Vlow);
V = (float32x4_t)vandq_s32((int32x4_t)V, btvFFF0Mask);
return btVector3(V);
return b3Vector3(V);
#else
return btVector3(
return b3Vector3(
m_floats[1] * v.m_floats[2] - m_floats[2] * v.m_floats[1],
m_floats[2] * v.m_floats[0] - m_floats[0] * v.m_floats[2],
m_floats[0] * v.m_floats[1] - m_floats[1] * v.m_floats[0]);
#endif
}
SIMD_FORCE_INLINE btScalar triple(const btVector3& v1, const btVector3& v2) const
SIMD_FORCE_INLINE b3Scalar triple(const b3Vector3& v1, const b3Vector3& v2) const
{
#if defined(BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
// cross:
@@ -475,11 +475,11 @@ public:
}
SIMD_FORCE_INLINE void setInterpolate3(const btVector3& v0, const btVector3& v1, btScalar rt)
SIMD_FORCE_INLINE void setInterpolate3(const b3Vector3& v0, const b3Vector3& v1, b3Scalar rt)
{
#if defined(BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
__m128 vrt = _mm_load_ss(&rt); // (rt 0 0 0)
btScalar s = btScalar(1.0) - rt;
b3Scalar s = b3Scalar(1.0) - rt;
__m128 vs = _mm_load_ss(&s); // (S 0 0 0)
vs = bt_pshufd_ps(vs, 0x80); // (S S S 0.0)
__m128 r0 = _mm_mul_ps(v0.mVec128, vs);
@@ -492,7 +492,7 @@ public:
mVec128 = vmulq_n_f32(mVec128, rt);
mVec128 = vaddq_f32(mVec128, v0.mVec128);
#else
btScalar s = btScalar(1.0) - rt;
b3Scalar s = b3Scalar(1.0) - rt;
m_floats[0] = s * v0.m_floats[0] + rt * v1.m_floats[0];
m_floats[1] = s * v0.m_floats[1] + rt * v1.m_floats[1];
m_floats[2] = s * v0.m_floats[2] + rt * v1.m_floats[2];
@@ -504,7 +504,7 @@ public:
/**@brief Return the linear interpolation between this and another vector
* @param v The other vector
* @param t The ration of this to v (t = 0 => return this, t=1 => return other) */
SIMD_FORCE_INLINE btVector3 lerp(const btVector3& v, const btScalar& t) const
SIMD_FORCE_INLINE b3Vector3 lerp(const b3Vector3& v, const b3Scalar& t) const
{
#if defined(BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
__m128 vt = _mm_load_ss(&t); // (t 0 0 0)
@@ -513,16 +513,16 @@ public:
vl = _mm_mul_ps(vl, vt);
vl = _mm_add_ps(vl, mVec128);
return btVector3(vl);
return b3Vector3(vl);
#elif defined(BT_USE_NEON)
float32x4_t vl = vsubq_f32(v.mVec128, mVec128);
vl = vmulq_n_f32(vl, t);
vl = vaddq_f32(vl, mVec128);
return btVector3(vl);
return b3Vector3(vl);
#else
return
btVector3( m_floats[0] + (v.m_floats[0] - m_floats[0]) * t,
b3Vector3( m_floats[0] + (v.m_floats[0] - m_floats[0]) * t,
m_floats[1] + (v.m_floats[1] - m_floats[1]) * t,
m_floats[2] + (v.m_floats[2] - m_floats[2]) * t);
#endif
@@ -530,7 +530,7 @@ public:
/**@brief Elementwise multiply this vector by the other
* @param v The other vector */
SIMD_FORCE_INLINE btVector3& operator*=(const btVector3& v)
SIMD_FORCE_INLINE b3Vector3& operator*=(const b3Vector3& v)
{
#if defined(BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
mVec128 = _mm_mul_ps(mVec128, v.mVec128);
@@ -545,30 +545,30 @@ public:
}
/**@brief Return the x value */
SIMD_FORCE_INLINE const btScalar& getX() const { return m_floats[0]; }
SIMD_FORCE_INLINE const b3Scalar& getX() const { return m_floats[0]; }
/**@brief Return the y value */
SIMD_FORCE_INLINE const btScalar& getY() const { return m_floats[1]; }
SIMD_FORCE_INLINE const b3Scalar& getY() const { return m_floats[1]; }
/**@brief Return the z value */
SIMD_FORCE_INLINE const btScalar& getZ() const { return m_floats[2]; }
SIMD_FORCE_INLINE const b3Scalar& getZ() const { return m_floats[2]; }
/**@brief Return the w value */
SIMD_FORCE_INLINE const btScalar& getW() const { return m_floats[3]; }
SIMD_FORCE_INLINE const b3Scalar& getW() const { return m_floats[3]; }
/**@brief Set the x value */
SIMD_FORCE_INLINE void setX(btScalar _x) { m_floats[0] = _x;};
SIMD_FORCE_INLINE void setX(b3Scalar _x) { m_floats[0] = _x;};
/**@brief Set the y value */
SIMD_FORCE_INLINE void setY(btScalar _y) { m_floats[1] = _y;};
SIMD_FORCE_INLINE void setY(b3Scalar _y) { m_floats[1] = _y;};
/**@brief Set the z value */
SIMD_FORCE_INLINE void setZ(btScalar _z) { m_floats[2] = _z;};
SIMD_FORCE_INLINE void setZ(b3Scalar _z) { m_floats[2] = _z;};
/**@brief Set the w value */
SIMD_FORCE_INLINE void setW(btScalar _w) { m_floats[3] = _w;};
SIMD_FORCE_INLINE void setW(b3Scalar _w) { m_floats[3] = _w;};
//SIMD_FORCE_INLINE btScalar& operator[](int i) { return (&m_floats[0])[i]; }
//SIMD_FORCE_INLINE const btScalar& operator[](int i) const { return (&m_floats[0])[i]; }
///operator btScalar*() replaces operator[], using implicit conversion. We added operator != and operator == to avoid pointer comparisons.
SIMD_FORCE_INLINE operator btScalar *() { return &m_floats[0]; }
SIMD_FORCE_INLINE operator const btScalar *() const { return &m_floats[0]; }
//SIMD_FORCE_INLINE b3Scalar& operator[](int i) { return (&m_floats[0])[i]; }
//SIMD_FORCE_INLINE const b3Scalar& operator[](int i) const { return (&m_floats[0])[i]; }
///operator b3Scalar*() replaces operator[], using implicit conversion. We added operator != and operator == to avoid pointer comparisons.
SIMD_FORCE_INLINE operator b3Scalar *() { return &m_floats[0]; }
SIMD_FORCE_INLINE operator const b3Scalar *() const { return &m_floats[0]; }
SIMD_FORCE_INLINE bool operator==(const btVector3& other) const
SIMD_FORCE_INLINE bool operator==(const b3Vector3& other) const
{
#if defined(BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
return (0xf == _mm_movemask_ps((__m128)_mm_cmpeq_ps(mVec128, other.mVec128)));
@@ -580,15 +580,15 @@ public:
#endif
}
SIMD_FORCE_INLINE bool operator!=(const btVector3& other) const
SIMD_FORCE_INLINE bool operator!=(const b3Vector3& other) const
{
return !(*this == other);
}
/**@brief Set each element to the max of the current values and the values of another btVector3
* @param other The other btVector3 to compare with
/**@brief Set each element to the max of the current values and the values of another b3Vector3
* @param other The other b3Vector3 to compare with
*/
SIMD_FORCE_INLINE void setMax(const btVector3& other)
SIMD_FORCE_INLINE void setMax(const b3Vector3& other)
{
#if defined(BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
mVec128 = _mm_max_ps(mVec128, other.mVec128);
@@ -602,10 +602,10 @@ public:
#endif
}
/**@brief Set each element to the min of the current values and the values of another btVector3
* @param other The other btVector3 to compare with
/**@brief Set each element to the min of the current values and the values of another b3Vector3
* @param other The other b3Vector3 to compare with
*/
SIMD_FORCE_INLINE void setMin(const btVector3& other)
SIMD_FORCE_INLINE void setMin(const b3Vector3& other)
{
#if defined(BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
mVec128 = _mm_min_ps(mVec128, other.mVec128);
@@ -619,15 +619,15 @@ public:
#endif
}
SIMD_FORCE_INLINE void setValue(const btScalar& _x, const btScalar& _y, const btScalar& _z)
SIMD_FORCE_INLINE void setValue(const b3Scalar& _x, const b3Scalar& _y, const b3Scalar& _z)
{
m_floats[0]=_x;
m_floats[1]=_y;
m_floats[2]=_z;
m_floats[3] = btScalar(0.f);
m_floats[3] = b3Scalar(0.f);
}
void getSkewSymmetricMatrix(btVector3* v0,btVector3* v1,btVector3* v2) const
void getSkewSymmetricMatrix(b3Vector3* v0,b3Vector3* v1,b3Vector3* v2) const
{
#if defined(BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
@@ -658,13 +658,13 @@ public:
int32x4_t vi = vdupq_n_s32(0);
mVec128 = vreinterpretq_f32_s32(vi);
#else
setValue(btScalar(0.),btScalar(0.),btScalar(0.));
setValue(b3Scalar(0.),b3Scalar(0.),b3Scalar(0.));
#endif
}
SIMD_FORCE_INLINE bool isZero() const
{
return m_floats[0] == btScalar(0) && m_floats[1] == btScalar(0) && m_floats[2] == btScalar(0);
return m_floats[0] == b3Scalar(0) && m_floats[1] == b3Scalar(0) && m_floats[2] == b3Scalar(0);
}
SIMD_FORCE_INLINE bool fuzzyZero() const
@@ -688,16 +688,16 @@ public:
* @param array The other vectors
* @param array_count The number of other vectors
* @param dotOut The maximum dot product */
SIMD_FORCE_INLINE long maxDot( const btVector3 *array, long array_count, btScalar &dotOut ) const;
SIMD_FORCE_INLINE long maxDot( const b3Vector3 *array, long array_count, b3Scalar &dotOut ) const;
/**@brief returns index of minimum dot product between this and vectors in array[]
* @param array The other vectors
* @param array_count The number of other vectors
* @param dotOut The minimum dot product */
SIMD_FORCE_INLINE long minDot( const btVector3 *array, long array_count, btScalar &dotOut ) const;
SIMD_FORCE_INLINE long minDot( const b3Vector3 *array, long array_count, b3Scalar &dotOut ) const;
/* create a vector as btVector3( this->dot( btVector3 v0 ), this->dot( btVector3 v1), this->dot( btVector3 v2 )) */
SIMD_FORCE_INLINE btVector3 dot3( const btVector3 &v0, const btVector3 &v1, const btVector3 &v2 ) const
/* create a vector as b3Vector3( this->dot( b3Vector3 v0 ), this->dot( b3Vector3 v1), this->dot( b3Vector3 v2 )) */
SIMD_FORCE_INLINE b3Vector3 dot3( const b3Vector3 &v0, const b3Vector3 &v1, const b3Vector3 &v2 ) const
{
#if defined(BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
@@ -711,7 +711,7 @@ public:
r = _mm_add_ps( r, _mm_movehl_ps( b2, b0 ));
a2 = _mm_and_ps( a2, btvxyzMaskf);
r = _mm_add_ps( r, btCastdTo128f (_mm_move_sd( btCastfTo128d(a2), btCastfTo128d(b1) )));
return btVector3(r);
return b3Vector3(r);
#elif defined(BT_USE_NEON)
static const uint32x4_t xyzMask = (const uint32x4_t){ -1, -1, -1, 0 };
@@ -722,23 +722,23 @@ public:
a2 = (float32x4_t) vandq_u32((uint32x4_t) a2, xyzMask );
float32x2_t b0 = vadd_f32( vpadd_f32( vget_low_f32(a0), vget_low_f32(a1)), zLo.val[0] );
float32x2_t b1 = vpadd_f32( vpadd_f32( vget_low_f32(a2), vget_high_f32(a2)), vdup_n_f32(0.0f));
return btVector3( vcombine_f32(b0, b1) );
return b3Vector3( vcombine_f32(b0, b1) );
#else
return btVector3( dot(v0), dot(v1), dot(v2));
return b3Vector3( dot(v0), dot(v1), dot(v2));
#endif
}
};
/**@brief Return the sum of two vectors (Point symantics)*/
SIMD_FORCE_INLINE btVector3
operator+(const btVector3& v1, const btVector3& v2)
SIMD_FORCE_INLINE b3Vector3
operator+(const b3Vector3& v1, const b3Vector3& v2)
{
#if defined(BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
return btVector3(_mm_add_ps(v1.mVec128, v2.mVec128));
return b3Vector3(_mm_add_ps(v1.mVec128, v2.mVec128));
#elif defined(BT_USE_NEON)
return btVector3(vaddq_f32(v1.mVec128, v2.mVec128));
return b3Vector3(vaddq_f32(v1.mVec128, v2.mVec128));
#else
return btVector3(
return b3Vector3(
v1.m_floats[0] + v2.m_floats[0],
v1.m_floats[1] + v2.m_floats[1],
v1.m_floats[2] + v2.m_floats[2]);
@@ -746,15 +746,15 @@ operator+(const btVector3& v1, const btVector3& v2)
}
/**@brief Return the elementwise product of two vectors */
SIMD_FORCE_INLINE btVector3
operator*(const btVector3& v1, const btVector3& v2)
SIMD_FORCE_INLINE b3Vector3
operator*(const b3Vector3& v1, const b3Vector3& v2)
{
#if defined(BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
return btVector3(_mm_mul_ps(v1.mVec128, v2.mVec128));
return b3Vector3(_mm_mul_ps(v1.mVec128, v2.mVec128));
#elif defined(BT_USE_NEON)
return btVector3(vmulq_f32(v1.mVec128, v2.mVec128));
return b3Vector3(vmulq_f32(v1.mVec128, v2.mVec128));
#else
return btVector3(
return b3Vector3(
v1.m_floats[0] * v2.m_floats[0],
v1.m_floats[1] * v2.m_floats[1],
v1.m_floats[2] * v2.m_floats[2]);
@@ -762,19 +762,19 @@ operator*(const btVector3& v1, const btVector3& v2)
}
/**@brief Return the difference between two vectors */
SIMD_FORCE_INLINE btVector3
operator-(const btVector3& v1, const btVector3& v2)
SIMD_FORCE_INLINE b3Vector3
operator-(const b3Vector3& v1, const b3Vector3& v2)
{
#if (defined(BT_USE_SSE_IN_API) && defined(BT_USE_SSE))
// without _mm_and_ps this code causes slowdown in Concave moving
__m128 r = _mm_sub_ps(v1.mVec128, v2.mVec128);
return btVector3(_mm_and_ps(r, btvFFF0fMask));
return b3Vector3(_mm_and_ps(r, btvFFF0fMask));
#elif defined(BT_USE_NEON)
float32x4_t r = vsubq_f32(v1.mVec128, v2.mVec128);
return btVector3((float32x4_t)vandq_s32((int32x4_t)r, btvFFF0Mask));
return b3Vector3((float32x4_t)vandq_s32((int32x4_t)r, btvFFF0Mask));
#else
return btVector3(
return b3Vector3(
v1.m_floats[0] - v2.m_floats[0],
v1.m_floats[1] - v2.m_floats[1],
v1.m_floats[2] - v2.m_floats[2]);
@@ -782,67 +782,67 @@ operator-(const btVector3& v1, const btVector3& v2)
}
/**@brief Return the negative of the vector */
SIMD_FORCE_INLINE btVector3
operator-(const btVector3& v)
SIMD_FORCE_INLINE b3Vector3
operator-(const b3Vector3& v)
{
#if (defined(BT_USE_SSE_IN_API) && defined (BT_USE_SSE))
__m128 r = _mm_xor_ps(v.mVec128, btvMzeroMask);
return btVector3(_mm_and_ps(r, btvFFF0fMask));
return b3Vector3(_mm_and_ps(r, btvFFF0fMask));
#elif defined(BT_USE_NEON)
return btVector3((btSimdFloat4)veorq_s32((int32x4_t)v.mVec128, (int32x4_t)btvMzeroMask));
return b3Vector3((btSimdFloat4)veorq_s32((int32x4_t)v.mVec128, (int32x4_t)btvMzeroMask));
#else
return btVector3(-v.m_floats[0], -v.m_floats[1], -v.m_floats[2]);
return b3Vector3(-v.m_floats[0], -v.m_floats[1], -v.m_floats[2]);
#endif
}
/**@brief Return the vector scaled by s */
SIMD_FORCE_INLINE btVector3
operator*(const btVector3& v, const btScalar& s)
SIMD_FORCE_INLINE b3Vector3
operator*(const b3Vector3& v, const b3Scalar& s)
{
#if defined(BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
__m128 vs = _mm_load_ss(&s); // (S 0 0 0)
vs = bt_pshufd_ps(vs, 0x80); // (S S S 0.0)
return btVector3(_mm_mul_ps(v.mVec128, vs));
return b3Vector3(_mm_mul_ps(v.mVec128, vs));
#elif defined(BT_USE_NEON)
float32x4_t r = vmulq_n_f32(v.mVec128, s);
return btVector3((float32x4_t)vandq_s32((int32x4_t)r, btvFFF0Mask));
return b3Vector3((float32x4_t)vandq_s32((int32x4_t)r, btvFFF0Mask));
#else
return btVector3(v.m_floats[0] * s, v.m_floats[1] * s, v.m_floats[2] * s);
return b3Vector3(v.m_floats[0] * s, v.m_floats[1] * s, v.m_floats[2] * s);
#endif
}
/**@brief Return the vector scaled by s */
SIMD_FORCE_INLINE btVector3
operator*(const btScalar& s, const btVector3& v)
SIMD_FORCE_INLINE b3Vector3
operator*(const b3Scalar& s, const b3Vector3& v)
{
return v * s;
}
/**@brief Return the vector inversely scaled by s */
SIMD_FORCE_INLINE btVector3
operator/(const btVector3& v, const btScalar& s)
SIMD_FORCE_INLINE b3Vector3
operator/(const b3Vector3& v, const b3Scalar& s)
{
btFullAssert(s != btScalar(0.0));
btFullAssert(s != b3Scalar(0.0));
#if 0 //defined(BT_USE_SSE_IN_API)
// this code is not faster !
__m128 vs = _mm_load_ss(&s);
vs = _mm_div_ss(v1110, vs);
vs = bt_pshufd_ps(vs, 0x00); // (S S S S)
return btVector3(_mm_mul_ps(v.mVec128, vs));
return b3Vector3(_mm_mul_ps(v.mVec128, vs));
#else
return v * (btScalar(1.0) / s);
return v * (b3Scalar(1.0) / s);
#endif
}
/**@brief Return the vector inversely scaled by s */
SIMD_FORCE_INLINE btVector3
operator/(const btVector3& v1, const btVector3& v2)
SIMD_FORCE_INLINE b3Vector3
operator/(const b3Vector3& v1, const b3Vector3& v2)
{
#if (defined(BT_USE_SSE_IN_API)&& defined (BT_USE_SSE))
__m128 vec = _mm_div_ps(v1.mVec128, v2.mVec128);
vec = _mm_and_ps(vec, btvFFF0fMask);
return btVector3(vec);
return b3Vector3(vec);
#elif defined(BT_USE_NEON)
float32x4_t x, y, v, m;
@@ -856,9 +856,9 @@ operator/(const btVector3& v1, const btVector3& v2)
v = vmulq_f32(v, x); // x*vv
v = vmulq_f32(v, m); // (x*vv)*(2-vv*y) = x*(vv(2-vv*y)) ~~~ x/y
return btVector3(v);
return b3Vector3(v);
#else
return btVector3(
return b3Vector3(
v1.m_floats[0] / v2.m_floats[0],
v1.m_floats[1] / v2.m_floats[1],
v1.m_floats[2] / v2.m_floats[2]);
@@ -866,44 +866,44 @@ operator/(const btVector3& v1, const btVector3& v2)
}
/**@brief Return the dot product between two vectors */
SIMD_FORCE_INLINE btScalar
btDot(const btVector3& v1, const btVector3& v2)
SIMD_FORCE_INLINE b3Scalar
btDot(const b3Vector3& v1, const b3Vector3& v2)
{
return v1.dot(v2);
}
/**@brief Return the distance squared between two vectors */
SIMD_FORCE_INLINE btScalar
btDistance2(const btVector3& v1, const btVector3& v2)
SIMD_FORCE_INLINE b3Scalar
btDistance2(const b3Vector3& v1, const b3Vector3& v2)
{
return v1.distance2(v2);
}
/**@brief Return the distance between two vectors */
SIMD_FORCE_INLINE btScalar
btDistance(const btVector3& v1, const btVector3& v2)
SIMD_FORCE_INLINE b3Scalar
btDistance(const b3Vector3& v1, const b3Vector3& v2)
{
return v1.distance(v2);
}
/**@brief Return the angle between two vectors */
SIMD_FORCE_INLINE btScalar
btAngle(const btVector3& v1, const btVector3& v2)
SIMD_FORCE_INLINE b3Scalar
btAngle(const b3Vector3& v1, const b3Vector3& v2)
{
return v1.angle(v2);
}
/**@brief Return the cross product of two vectors */
SIMD_FORCE_INLINE btVector3
btCross(const btVector3& v1, const btVector3& v2)
SIMD_FORCE_INLINE b3Vector3
btCross(const b3Vector3& v1, const b3Vector3& v2)
{
return v1.cross(v2);
}
SIMD_FORCE_INLINE btScalar
btTriple(const btVector3& v1, const btVector3& v2, const btVector3& v3)
SIMD_FORCE_INLINE b3Scalar
btTriple(const b3Vector3& v1, const b3Vector3& v2, const b3Vector3& v3)
{
return v1.triple(v2, v3);
}
@@ -912,28 +912,28 @@ btTriple(const btVector3& v1, const btVector3& v2, const btVector3& v3)
* @param v1 One vector
* @param v2 The other vector
* @param t The ration of this to v (t = 0 => return v1, t=1 => return v2) */
SIMD_FORCE_INLINE btVector3
lerp(const btVector3& v1, const btVector3& v2, const btScalar& t)
SIMD_FORCE_INLINE b3Vector3
lerp(const b3Vector3& v1, const b3Vector3& v2, const b3Scalar& t)
{
return v1.lerp(v2, t);
}
SIMD_FORCE_INLINE btScalar btVector3::distance2(const btVector3& v) const
SIMD_FORCE_INLINE b3Scalar b3Vector3::distance2(const b3Vector3& v) const
{
return (v - *this).length2();
}
SIMD_FORCE_INLINE btScalar btVector3::distance(const btVector3& v) const
SIMD_FORCE_INLINE b3Scalar b3Vector3::distance(const b3Vector3& v) const
{
return (v - *this).length();
}
SIMD_FORCE_INLINE btVector3 btVector3::normalized() const
SIMD_FORCE_INLINE b3Vector3 b3Vector3::normalized() const
{
#if defined(BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
btVector3 norm = *this;
b3Vector3 norm = *this;
return norm.normalize();
#else
@@ -941,17 +941,17 @@ SIMD_FORCE_INLINE btVector3 btVector3::normalized() const
#endif
}
SIMD_FORCE_INLINE btVector3 btVector3::rotate( const btVector3& wAxis, const btScalar _angle ) const
SIMD_FORCE_INLINE b3Vector3 b3Vector3::rotate( const b3Vector3& wAxis, const b3Scalar _angle ) const
{
// wAxis must be a unit lenght vector
#if defined(BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
__m128 O = _mm_mul_ps(wAxis.mVec128, mVec128);
btScalar ssin = btSin( _angle );
b3Scalar ssin = btSin( _angle );
__m128 C = wAxis.cross( mVec128 ).mVec128;
O = _mm_and_ps(O, btvFFF0fMask);
btScalar scos = btCos( _angle );
b3Scalar scos = btCos( _angle );
__m128 vsin = _mm_load_ss(&ssin); // (S 0 0 0)
__m128 vcos = _mm_load_ss(&scos); // (S 0 0 0)
@@ -971,11 +971,11 @@ SIMD_FORCE_INLINE btVector3 btVector3::rotate( const btVector3& wAxis, const btS
vcos = vcos * X;
O = O + vcos;
return btVector3(O);
return b3Vector3(O);
#else
btVector3 o = wAxis * wAxis.dot( *this );
btVector3 _x = *this - o;
btVector3 _y;
b3Vector3 o = wAxis * wAxis.dot( *this );
b3Vector3 _x = *this - o;
b3Vector3 _y;
_y = wAxis.cross( *this );
@@ -983,7 +983,7 @@ SIMD_FORCE_INLINE btVector3 btVector3::rotate( const btVector3& wAxis, const btS
#endif
}
SIMD_FORCE_INLINE long btVector3::maxDot( const btVector3 *array, long array_count, btScalar &dotOut ) const
SIMD_FORCE_INLINE long b3Vector3::maxDot( const b3Vector3 *array, long array_count, b3Scalar &dotOut ) const
{
#if defined (BT_USE_SSE) || defined (BT_USE_NEON)
#if defined _WIN32 || defined (BT_USE_SSE)
@@ -998,12 +998,12 @@ SIMD_FORCE_INLINE long btVector3::maxDot( const btVector3 *array, long arra
#endif//BT_USE_SSE || BT_USE_NEON
{
btScalar maxDot = -SIMD_INFINITY;
b3Scalar maxDot = -SIMD_INFINITY;
int i = 0;
int ptIndex = -1;
for( i = 0; i < array_count; i++ )
{
btScalar dot = array[i].dot(*this);
b3Scalar dot = array[i].dot(*this);
if( dot > maxDot )
{
@@ -1020,7 +1020,7 @@ SIMD_FORCE_INLINE long btVector3::maxDot( const btVector3 *array, long arra
#endif
}
SIMD_FORCE_INLINE long btVector3::minDot( const btVector3 *array, long array_count, btScalar &dotOut ) const
SIMD_FORCE_INLINE long b3Vector3::minDot( const b3Vector3 *array, long array_count, b3Scalar &dotOut ) const
{
#if defined (BT_USE_SSE) || defined (BT_USE_NEON)
#if defined BT_USE_SSE
@@ -1036,13 +1036,13 @@ SIMD_FORCE_INLINE long btVector3::minDot( const btVector3 *array, long arra
if( array_count < scalar_cutoff )
#endif//BT_USE_SSE || BT_USE_NEON
{
btScalar minDot = SIMD_INFINITY;
b3Scalar minDot = SIMD_INFINITY;
int i = 0;
int ptIndex = -1;
for( i = 0; i < array_count; i++ )
{
btScalar dot = array[i].dot(*this);
b3Scalar dot = array[i].dot(*this);
if( dot < minDot )
{
@@ -1061,15 +1061,15 @@ SIMD_FORCE_INLINE long btVector3::minDot( const btVector3 *array, long arra
}
class btVector4 : public btVector3
class btVector4 : public b3Vector3
{
public:
SIMD_FORCE_INLINE btVector4() {}
SIMD_FORCE_INLINE btVector4(const btScalar& _x, const btScalar& _y, const btScalar& _z,const btScalar& _w)
: btVector3(_x,_y,_z)
SIMD_FORCE_INLINE btVector4(const b3Scalar& _x, const b3Scalar& _y, const b3Scalar& _z,const b3Scalar& _w)
: b3Vector3(_x,_y,_z)
{
m_floats[3] = _w;
}
@@ -1080,7 +1080,7 @@ public:
mVec128 = vec;
}
SIMD_FORCE_INLINE btVector4(const btVector3& rhs)
SIMD_FORCE_INLINE btVector4(const b3Vector3& rhs)
{
mVec128 = rhs.mVec128;
}
@@ -1109,13 +1109,13 @@ public:
}
btScalar getW() const { return m_floats[3];}
b3Scalar getW() const { return m_floats[3];}
SIMD_FORCE_INLINE int maxAxis4() const
{
int maxIndex = -1;
btScalar maxVal = btScalar(-BT_LARGE_FLOAT);
b3Scalar maxVal = b3Scalar(-BT_LARGE_FLOAT);
if (m_floats[0] > maxVal)
{
maxIndex = 0;
@@ -1144,7 +1144,7 @@ public:
SIMD_FORCE_INLINE int minAxis4() const
{
int minIndex = -1;
btScalar minVal = btScalar(BT_LARGE_FLOAT);
b3Scalar minVal = b3Scalar(BT_LARGE_FLOAT);
if (m_floats[0] < minVal)
{
minIndex = 0;
@@ -1185,7 +1185,7 @@ public:
*/
/* void getValue(btScalar *m) const
/* void getValue(b3Scalar *m) const
{
m[0] = m_floats[0];
m[1] = m_floats[1];
@@ -1198,7 +1198,7 @@ public:
* @param z Value of z
* @param w Value of w
*/
SIMD_FORCE_INLINE void setValue(const btScalar& _x, const btScalar& _y, const btScalar& _z,const btScalar& _w)
SIMD_FORCE_INLINE void setValue(const b3Scalar& _x, const b3Scalar& _y, const b3Scalar& _z,const b3Scalar& _w)
{
m_floats[0]=_x;
m_floats[1]=_y;
@@ -1211,7 +1211,7 @@ public:
///btSwapVector3Endian swaps vector endianness, useful for network and cross-platform serialization
SIMD_FORCE_INLINE void btSwapScalarEndian(const btScalar& sourceVal, btScalar& destVal)
SIMD_FORCE_INLINE void btSwapScalarEndian(const b3Scalar& sourceVal, b3Scalar& destVal)
{
#ifdef BT_USE_DOUBLE_PRECISION
unsigned char* dest = (unsigned char*) &destVal;
@@ -1234,7 +1234,7 @@ SIMD_FORCE_INLINE void btSwapScalarEndian(const btScalar& sourceVal, btScalar& d
#endif //BT_USE_DOUBLE_PRECISION
}
///btSwapVector3Endian swaps vector endianness, useful for network and cross-platform serialization
SIMD_FORCE_INLINE void btSwapVector3Endian(const btVector3& sourceVec, btVector3& destVec)
SIMD_FORCE_INLINE void btSwapVector3Endian(const b3Vector3& sourceVec, b3Vector3& destVec)
{
for (int i=0;i<4;i++)
{
@@ -1244,10 +1244,10 @@ SIMD_FORCE_INLINE void btSwapVector3Endian(const btVector3& sourceVec, btVector3
}
///btUnSwapVector3Endian swaps vector endianness, useful for network and cross-platform serialization
SIMD_FORCE_INLINE void btUnSwapVector3Endian(btVector3& vector)
SIMD_FORCE_INLINE void btUnSwapVector3Endian(b3Vector3& vector)
{
btVector3 swappedVec;
b3Vector3 swappedVec;
for (int i=0;i<4;i++)
{
btSwapScalarEndian(vector[i],swappedVec[i]);
@@ -1260,8 +1260,8 @@ SIMD_FORCE_INLINE void btPlaneSpace1 (const T& n, T& p, T& q)
{
if (btFabs(n[2]) > SIMDSQRT12) {
// choose p in y-z plane
btScalar a = n[1]*n[1] + n[2]*n[2];
btScalar k = btRecipSqrt (a);
b3Scalar a = n[1]*n[1] + n[2]*n[2];
b3Scalar k = btRecipSqrt (a);
p[0] = 0;
p[1] = -n[2]*k;
p[2] = n[1]*k;
@@ -1272,8 +1272,8 @@ SIMD_FORCE_INLINE void btPlaneSpace1 (const T& n, T& p, T& q)
}
else {
// choose p in x-y plane
btScalar a = n[0]*n[0] + n[1]*n[1];
btScalar k = btRecipSqrt (a);
b3Scalar a = n[0]*n[0] + n[1]*n[1];
b3Scalar k = btRecipSqrt (a);
p[0] = -n[1]*k;
p[1] = n[0]*k;
p[2] = 0;
@@ -1296,42 +1296,42 @@ struct btVector3DoubleData
};
SIMD_FORCE_INLINE void btVector3::serializeFloat(struct btVector3FloatData& dataOut) const
SIMD_FORCE_INLINE void b3Vector3::serializeFloat(struct btVector3FloatData& dataOut) const
{
///could also do a memcpy, check if it is worth it
for (int i=0;i<4;i++)
dataOut.m_floats[i] = float(m_floats[i]);
}
SIMD_FORCE_INLINE void btVector3::deSerializeFloat(const struct btVector3FloatData& dataIn)
SIMD_FORCE_INLINE void b3Vector3::deSerializeFloat(const struct btVector3FloatData& dataIn)
{
for (int i=0;i<4;i++)
m_floats[i] = btScalar(dataIn.m_floats[i]);
m_floats[i] = b3Scalar(dataIn.m_floats[i]);
}
SIMD_FORCE_INLINE void btVector3::serializeDouble(struct btVector3DoubleData& dataOut) const
SIMD_FORCE_INLINE void b3Vector3::serializeDouble(struct btVector3DoubleData& dataOut) const
{
///could also do a memcpy, check if it is worth it
for (int i=0;i<4;i++)
dataOut.m_floats[i] = double(m_floats[i]);
}
SIMD_FORCE_INLINE void btVector3::deSerializeDouble(const struct btVector3DoubleData& dataIn)
SIMD_FORCE_INLINE void b3Vector3::deSerializeDouble(const struct btVector3DoubleData& dataIn)
{
for (int i=0;i<4;i++)
m_floats[i] = btScalar(dataIn.m_floats[i]);
m_floats[i] = b3Scalar(dataIn.m_floats[i]);
}
SIMD_FORCE_INLINE void btVector3::serialize(struct btVector3Data& dataOut) const
SIMD_FORCE_INLINE void b3Vector3::serialize(struct btVector3Data& dataOut) const
{
///could also do a memcpy, check if it is worth it
for (int i=0;i<4;i++)
dataOut.m_floats[i] = m_floats[i];
}
SIMD_FORCE_INLINE void btVector3::deSerialize(const struct btVector3Data& dataIn)
SIMD_FORCE_INLINE void b3Vector3::deSerialize(const struct btVector3Data& dataIn)
{
for (int i=0;i<4;i++)
m_floats[i] = dataIn.m_floats[i];

42
src/BulletCommon/btDefaultMotionState.h
View File

@@ -1,42 +0,0 @@
#ifndef BT_DEFAULT_MOTION_STATE_H
#define BT_DEFAULT_MOTION_STATE_H
#include "btMotionState.h"
///The btDefaultMotionState provides a common implementation to synchronize world transforms with offsets.
ATTRIBUTE_ALIGNED16(struct) btDefaultMotionState : public btMotionState
{
btTransform m_graphicsWorldTrans;
btTransform m_centerOfMassOffset;
btTransform m_startWorldTrans;
void* m_userPointer;
BT_DECLARE_ALIGNED_ALLOCATOR();
btDefaultMotionState(const btTransform& startTrans = btTransform::getIdentity(),const btTransform& centerOfMassOffset = btTransform::getIdentity())
: m_graphicsWorldTrans(startTrans),
m_centerOfMassOffset(centerOfMassOffset),
m_startWorldTrans(startTrans),
m_userPointer(0)
{
}
///synchronizes world transform from user to physics
virtual void getWorldTransform(btTransform& centerOfMassWorldTrans ) const
{
centerOfMassWorldTrans = m_centerOfMassOffset.inverse() * m_graphicsWorldTrans ;
}
///synchronizes world transform from physics to user
///Bullet only calls the update of worldtransform for active objects
virtual void setWorldTransform(const btTransform& centerOfMassWorldTrans)
{
m_graphicsWorldTrans = centerOfMassWorldTrans * m_centerOfMassOffset ;
}
};
#endif //BT_DEFAULT_MOTION_STATE_H

411
src/BulletCommon/btIDebugDraw.h
View File

@@ -1,411 +0,0 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef BT_IDEBUG_DRAW__H
#define BT_IDEBUG_DRAW__H
#include "btVector3.h"
#include "btTransform.h"
///The btIDebugDraw interface class allows hooking up a debug renderer to visually debug simulations.
///Typical use case: create a debug drawer object, and assign it to a btCollisionWorld or btDynamicsWorld using setDebugDrawer and call debugDrawWorld.
///A class that implements the btIDebugDraw interface has to implement the drawLine method at a minimum.
///For color arguments the X,Y,Z components refer to Red, Green and Blue each in the range [0..1]
class btIDebugDraw
{
public:
enum DebugDrawModes
{
DBG_NoDebug=0,
DBG_DrawWireframe = 1,
DBG_DrawAabb=2,
DBG_DrawFeaturesText=4,
DBG_DrawContactPoints=8,
DBG_NoDeactivation=16,
DBG_NoHelpText = 32,
DBG_DrawText=64,
DBG_ProfileTimings = 128,
DBG_EnableSatComparison = 256,
DBG_DisableBulletLCP = 512,
DBG_EnableCCD = 1024,
DBG_DrawConstraints = (1 << 11),
DBG_DrawConstraintLimits = (1 << 12),
DBG_FastWireframe = (1<<13),
DBG_DrawNormals = (1<<14),
DBG_DrawBroadphasePairs= (1<<15),
DBG_MAX_DEBUG_DRAW_MODE
};
virtual ~btIDebugDraw() {};
virtual void drawLine(const btVector3& from,const btVector3& to,const btVector3& color)=0;
virtual void drawLine(const btVector3& from,const btVector3& to, const btVector3& fromColor, const btVector3& toColor)
{
(void) toColor;
drawLine (from, to, fromColor);
}
virtual void drawSphere(btScalar radius, const btTransform& transform, const btVector3& color)
{
btVector3 start = transform.getOrigin();
const btVector3 xoffs = transform.getBasis() * btVector3(1,0,0);
const btVector3 yoffs = transform.getBasis() * btVector3(0,1,0);
const btVector3 zoffs = transform.getBasis() * btVector3(0,0,1);
drawArc(start,xoffs,yoffs,radius,radius,0,SIMD_2_PI,color,true,btScalar(10.0));
drawArc(start,yoffs,zoffs,radius,radius,0,SIMD_2_PI,color,true,btScalar(10.0));
drawArc(start,zoffs,xoffs,radius,radius,0,SIMD_2_PI,color,true,btScalar(10.0));
// drawArc(start,zoffs,yoffs,radius,radius,0,SIMD_2_PI,color,false,btScalar(10.0));
}
virtual void drawSphere (const btVector3& p, btScalar radius, const btVector3& color)
{
btTransform tr;
tr.setIdentity();
tr.setOrigin(p);
drawSphere(radius,tr,color);
}
virtual void drawTriangle(const btVector3& v0,const btVector3& v1,const btVector3& v2,const btVector3& /*n0*/,const btVector3& /*n1*/,const btVector3& /*n2*/,const btVector3& color, btScalar alpha)
{
drawTriangle(v0,v1,v2,color,alpha);
}
virtual void drawTriangle(const btVector3& v0,const btVector3& v1,const btVector3& v2,const btVector3& color, btScalar /*alpha*/)
{
drawLine(v0,v1,color);
drawLine(v1,v2,color);
drawLine(v2,v0,color);
}
virtual void drawContactPoint(const btVector3& PointOnB,const btVector3& normalOnB,btScalar distance,int lifeTime,const btVector3& color)=0;
virtual void reportErrorWarning(const char* warningString) = 0;
virtual void draw3dText(const btVector3& location,const char* textString) = 0;
virtual void setDebugMode(int debugMode) =0;
virtual int getDebugMode() const = 0;
virtual void drawAabb(const btVector3& from,const btVector3& to,const btVector3& color)
{
btVector3 halfExtents = (to-from)* 0.5f;
btVector3 center = (to+from) *0.5f;
int i,j;
btVector3 edgecoord(1.f,1.f,1.f),pa,pb;
for (i=0;i<4;i++)
{
for (j=0;j<3;j++)
{
pa = btVector3(edgecoord[0]*halfExtents[0], edgecoord[1]*halfExtents[1],
edgecoord[2]*halfExtents[2]);
pa+=center;
int othercoord = j%3;
edgecoord[othercoord]*=-1.f;
pb = btVector3(edgecoord[0]*halfExtents[0], edgecoord[1]*halfExtents[1],
edgecoord[2]*halfExtents[2]);
pb+=center;
drawLine(pa,pb,color);
}
edgecoord = btVector3(-1.f,-1.f,-1.f);
if (i<3)
edgecoord[i]*=-1.f;
}
}
virtual void drawTransform(const btTransform& transform, btScalar orthoLen)
{
btVector3 start = transform.getOrigin();
drawLine(start, start+transform.getBasis() * btVector3(orthoLen, 0, 0), btVector3(0.7f,0,0));
drawLine(start, start+transform.getBasis() * btVector3(0, orthoLen, 0), btVector3(0,0.7f,0));
drawLine(start, start+transform.getBasis() * btVector3(0, 0, orthoLen), btVector3(0,0,0.7f));
}
virtual void drawArc(const btVector3& center, const btVector3& normal, const btVector3& axis, btScalar radiusA, btScalar radiusB, btScalar minAngle, btScalar maxAngle,
const btVector3& color, bool drawSect, btScalar stepDegrees = btScalar(10.f))
{
const btVector3& vx = axis;
btVector3 vy = normal.cross(axis);
btScalar step = stepDegrees * SIMD_RADS_PER_DEG;
int nSteps = (int)((maxAngle - minAngle) / step);
if(!nSteps) nSteps = 1;
btVector3 prev = center + radiusA * vx * btCos(minAngle) + radiusB * vy * btSin(minAngle);
if(drawSect)
{
drawLine(center, prev, color);
}
for(int i = 1; i <= nSteps; i++)
{
btScalar angle = minAngle + (maxAngle - minAngle) * btScalar(i) / btScalar(nSteps);
btVector3 next = center + radiusA * vx * btCos(angle) + radiusB * vy * btSin(angle);
drawLine(prev, next, color);
prev = next;
}
if(drawSect)
{
drawLine(center, prev, color);
}
}
virtual void drawSpherePatch(const btVector3& center, const btVector3& up, const btVector3& axis, btScalar radius,
btScalar minTh, btScalar maxTh, btScalar minPs, btScalar maxPs, const btVector3& color, btScalar stepDegrees = btScalar(10.f))
{
btVector3 vA[74];
btVector3 vB[74];
btVector3 *pvA = vA, *pvB = vB, *pT;
btVector3 npole = center + up * radius;
btVector3 spole = center - up * radius;
btVector3 arcStart;
btScalar step = stepDegrees * SIMD_RADS_PER_DEG;
const btVector3& kv = up;
const btVector3& iv = axis;
btVector3 jv = kv.cross(iv);
bool drawN = false;
bool drawS = false;
if(minTh <= -SIMD_HALF_PI)
{
minTh = -SIMD_HALF_PI + step;
drawN = true;
}
if(maxTh >= SIMD_HALF_PI)
{
maxTh = SIMD_HALF_PI - step;
drawS = true;
}
if(minTh > maxTh)
{
minTh = -SIMD_HALF_PI + step;
maxTh = SIMD_HALF_PI - step;
drawN = drawS = true;
}
int n_hor = (int)((maxTh - minTh) / step) + 1;
if(n_hor < 2) n_hor = 2;
btScalar step_h = (maxTh - minTh) / btScalar(n_hor - 1);
bool isClosed = false;
if(minPs > maxPs)
{
minPs = -SIMD_PI + step;
maxPs = SIMD_PI;
isClosed = true;
}
else if((maxPs - minPs) >= SIMD_PI * btScalar(2.f))
{
isClosed = true;
}
else
{
isClosed = false;
}
int n_vert = (int)((maxPs - minPs) / step) + 1;
if(n_vert < 2) n_vert = 2;
btScalar step_v = (maxPs - minPs) / btScalar(n_vert - 1);
for(int i = 0; i < n_hor; i++)
{
btScalar th = minTh + btScalar(i) * step_h;
btScalar sth = radius * btSin(th);
btScalar cth = radius * btCos(th);
for(int j = 0; j < n_vert; j++)
{
btScalar psi = minPs + btScalar(j) * step_v;
btScalar sps = btSin(psi);
btScalar cps = btCos(psi);
pvB[j] = center + cth * cps * iv + cth * sps * jv + sth * kv;
if(i)
{
drawLine(pvA[j], pvB[j], color);
}
else if(drawS)
{
drawLine(spole, pvB[j], color);
}
if(j)
{
drawLine(pvB[j-1], pvB[j], color);
}
else
{
arcStart = pvB[j];
}
if((i == (n_hor - 1)) && drawN)
{
drawLine(npole, pvB[j], color);
}
if(isClosed)
{
if(j == (n_vert-1))
{
drawLine(arcStart, pvB[j], color);
}
}
else
{
if(((!i) || (i == (n_hor-1))) && ((!j) || (j == (n_vert-1))))
{
drawLine(center, pvB[j], color);
}
}
}
pT = pvA; pvA = pvB; pvB = pT;
}
}
virtual void drawBox(const btVector3& bbMin, const btVector3& bbMax, const btVector3& color)
{
drawLine(btVector3(bbMin[0], bbMin[1], bbMin[2]), btVector3(bbMax[0], bbMin[1], bbMin[2]), color);
drawLine(btVector3(bbMax[0], bbMin[1], bbMin[2]), btVector3(bbMax[0], bbMax[1], bbMin[2]), color);
drawLine(btVector3(bbMax[0], bbMax[1], bbMin[2]), btVector3(bbMin[0], bbMax[1], bbMin[2]), color);
drawLine(btVector3(bbMin[0], bbMax[1], bbMin[2]), btVector3(bbMin[0], bbMin[1], bbMin[2]), color);
drawLine(btVector3(bbMin[0], bbMin[1], bbMin[2]), btVector3(bbMin[0], bbMin[1], bbMax[2]), color);
drawLine(btVector3(bbMax[0], bbMin[1], bbMin[2]), btVector3(bbMax[0], bbMin[1], bbMax[2]), color);
drawLine(btVector3(bbMax[0], bbMax[1], bbMin[2]), btVector3(bbMax[0], bbMax[1], bbMax[2]), color);
drawLine(btVector3(bbMin[0], bbMax[1], bbMin[2]), btVector3(bbMin[0], bbMax[1], bbMax[2]), color);
drawLine(btVector3(bbMin[0], bbMin[1], bbMax[2]), btVector3(bbMax[0], bbMin[1], bbMax[2]), color);
drawLine(btVector3(bbMax[0], bbMin[1], bbMax[2]), btVector3(bbMax[0], bbMax[1], bbMax[2]), color);
drawLine(btVector3(bbMax[0], bbMax[1], bbMax[2]), btVector3(bbMin[0], bbMax[1], bbMax[2]), color);
drawLine(btVector3(bbMin[0], bbMax[1], bbMax[2]), btVector3(bbMin[0], bbMin[1], bbMax[2]), color);
}
virtual void drawBox(const btVector3& bbMin, const btVector3& bbMax, const btTransform& trans, const btVector3& color)
{
drawLine(trans * btVector3(bbMin[0], bbMin[1], bbMin[2]), trans * btVector3(bbMax[0], bbMin[1], bbMin[2]), color);
drawLine(trans * btVector3(bbMax[0], bbMin[1], bbMin[2]), trans * btVector3(bbMax[0], bbMax[1], bbMin[2]), color);
drawLine(trans * btVector3(bbMax[0], bbMax[1], bbMin[2]), trans * btVector3(bbMin[0], bbMax[1], bbMin[2]), color);
drawLine(trans * btVector3(bbMin[0], bbMax[1], bbMin[2]), trans * btVector3(bbMin[0], bbMin[1], bbMin[2]), color);
drawLine(trans * btVector3(bbMin[0], bbMin[1], bbMin[2]), trans * btVector3(bbMin[0], bbMin[1], bbMax[2]), color);
drawLine(trans * btVector3(bbMax[0], bbMin[1], bbMin[2]), trans * btVector3(bbMax[0], bbMin[1], bbMax[2]), color);
drawLine(trans * btVector3(bbMax[0], bbMax[1], bbMin[2]), trans * btVector3(bbMax[0], bbMax[1], bbMax[2]), color);
drawLine(trans * btVector3(bbMin[0], bbMax[1], bbMin[2]), trans * btVector3(bbMin[0], bbMax[1], bbMax[2]), color);
drawLine(trans * btVector3(bbMin[0], bbMin[1], bbMax[2]), trans * btVector3(bbMax[0], bbMin[1], bbMax[2]), color);
drawLine(trans * btVector3(bbMax[0], bbMin[1], bbMax[2]), trans * btVector3(bbMax[0], bbMax[1], bbMax[2]), color);
drawLine(trans * btVector3(bbMax[0], bbMax[1], bbMax[2]), trans * btVector3(bbMin[0], bbMax[1], bbMax[2]), color);
drawLine(trans * btVector3(bbMin[0], bbMax[1], bbMax[2]), trans * btVector3(bbMin[0], bbMin[1], bbMax[2]), color);
}
virtual void drawCapsule(btScalar radius, btScalar halfHeight, int upAxis, const btTransform& transform, const btVector3& color)
{
btVector3 capStart(0.f,0.f,0.f);
capStart[upAxis] = -halfHeight;
btVector3 capEnd(0.f,0.f,0.f);
capEnd[upAxis] = halfHeight;
// Draw the ends
{
btTransform childTransform = transform;
childTransform.getOrigin() = transform * capStart;
drawSphere(radius, childTransform, color);
}
{
btTransform childTransform = transform;
childTransform.getOrigin() = transform * capEnd;
drawSphere(radius, childTransform, color);
}
// Draw some additional lines
btVector3 start = transform.getOrigin();
capStart[(upAxis+1)%3] = radius;
capEnd[(upAxis+1)%3] = radius;
drawLine(start+transform.getBasis() * capStart,start+transform.getBasis() * capEnd, color);
capStart[(upAxis+1)%3] = -radius;
capEnd[(upAxis+1)%3] = -radius;
drawLine(start+transform.getBasis() * capStart,start+transform.getBasis() * capEnd, color);
capStart[(upAxis+1)%3] = 0.f;
capEnd[(upAxis+1)%3] = 0.f;
capStart[(upAxis+2)%3] = radius;
capEnd[(upAxis+2)%3] = radius;
drawLine(start+transform.getBasis() * capStart,start+transform.getBasis() * capEnd, color);
capStart[(upAxis+2)%3] = -radius;
capEnd[(upAxis+2)%3] = -radius;
drawLine(start+transform.getBasis() * capStart,start+transform.getBasis() * capEnd, color);
}
virtual void drawCylinder(btScalar radius, btScalar halfHeight, int upAxis, const btTransform& transform, const btVector3& color)
{
btVector3 start = transform.getOrigin();
btVector3 offsetHeight(0,0,0);
offsetHeight[upAxis] = halfHeight;
btVector3 offsetRadius(0,0,0);
offsetRadius[(upAxis+1)%3] = radius;
drawLine(start+transform.getBasis() * (offsetHeight+offsetRadius),start+transform.getBasis() * (-offsetHeight+offsetRadius),color);
drawLine(start+transform.getBasis() * (offsetHeight-offsetRadius),start+transform.getBasis() * (-offsetHeight-offsetRadius),color);
// Drawing top and bottom caps of the cylinder
btVector3 yaxis(0,0,0);
yaxis[upAxis] = btScalar(1.0);
btVector3 xaxis(0,0,0);
xaxis[(upAxis+1)%3] = btScalar(1.0);
drawArc(start-transform.getBasis()*(offsetHeight),transform.getBasis()*yaxis,transform.getBasis()*xaxis,radius,radius,0,SIMD_2_PI,color,false,btScalar(10.0));
drawArc(start+transform.getBasis()*(offsetHeight),transform.getBasis()*yaxis,transform.getBasis()*xaxis,radius,radius,0,SIMD_2_PI,color,false,btScalar(10.0));
}
virtual void drawCone(btScalar radius, btScalar height, int upAxis, const btTransform& transform, const btVector3& color)
{
btVector3 start = transform.getOrigin();
btVector3 offsetHeight(0,0,0);
offsetHeight[upAxis] = height * btScalar(0.5);
btVector3 offsetRadius(0,0,0);
offsetRadius[(upAxis+1)%3] = radius;
btVector3 offset2Radius(0,0,0);
offset2Radius[(upAxis+2)%3] = radius;
drawLine(start+transform.getBasis() * (offsetHeight),start+transform.getBasis() * (-offsetHeight+offsetRadius),color);
drawLine(start+transform.getBasis() * (offsetHeight),start+transform.getBasis() * (-offsetHeight-offsetRadius),color);
drawLine(start+transform.getBasis() * (offsetHeight),start+transform.getBasis() * (-offsetHeight+offset2Radius),color);
drawLine(start+transform.getBasis() * (offsetHeight),start+transform.getBasis() * (-offsetHeight-offset2Radius),color);
// Drawing the base of the cone
btVector3 yaxis(0,0,0);
yaxis[upAxis] = btScalar(1.0);
btVector3 xaxis(0,0,0);
xaxis[(upAxis+1)%3] = btScalar(1.0);
drawArc(start-transform.getBasis()*(offsetHeight),transform.getBasis()*yaxis,transform.getBasis()*xaxis,radius,radius,0,SIMD_2_PI,color,false,10.0);
}
virtual void drawPlane(const btVector3& planeNormal, btScalar planeConst, const btTransform& transform, const btVector3& color)
{
btVector3 planeOrigin = planeNormal * planeConst;
btVector3 vec0,vec1;
btPlaneSpace1(planeNormal,vec0,vec1);
btScalar vecLen = 100.f;
btVector3 pt0 = planeOrigin + vec0*vecLen;
btVector3 pt1 = planeOrigin - vec0*vecLen;
btVector3 pt2 = planeOrigin + vec1*vecLen;
btVector3 pt3 = planeOrigin - vec1*vecLen;
drawLine(transform*pt0,transform*pt1,color);
drawLine(transform*pt2,transform*pt3,color);
}
};
#endif //BT_IDEBUG_DRAW__H

73
src/BulletCommon/btList.h
View File

@@ -1,73 +0,0 @@
/*
Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans http://continuousphysics.com/Bullet/
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef BT_GEN_LIST_H
#define BT_GEN_LIST_H
class btGEN_Link {
public:
btGEN_Link() : m_next(0), m_prev(0) {}
btGEN_Link(btGEN_Link *next, btGEN_Link *prev) : m_next(next), m_prev(prev) {}
btGEN_Link *getNext() const { return m_next; }
btGEN_Link *getPrev() const { return m_prev; }
bool isHead() const { return m_prev == 0; }
bool isTail() const { return m_next == 0; }
void insertBefore(btGEN_Link *link) {
m_next = link;
m_prev = link->m_prev;
m_next->m_prev = this;
m_prev->m_next = this;
}
void insertAfter(btGEN_Link *link) {
m_next = link->m_next;
m_prev = link;
m_next->m_prev = this;
m_prev->m_next = this;
}
void remove() {
m_next->m_prev = m_prev;
m_prev->m_next = m_next;
}
private:
btGEN_Link *m_next;
btGEN_Link *m_prev;
};
class btGEN_List {
public:
btGEN_List() : m_head(&m_tail, 0), m_tail(0, &m_head) {}
btGEN_Link *getHead() const { return m_head.getNext(); }
btGEN_Link *getTail() const { return m_tail.getPrev(); }
void addHead(btGEN_Link *link) { link->insertAfter(&m_head); }
void addTail(btGEN_Link *link) { link->insertBefore(&m_tail); }
private:
btGEN_Link m_head;
btGEN_Link m_tail;
};
#endif //BT_GEN_LIST_H

40
src/BulletCommon/btMotionState.h
View File

@@ -1,40 +0,0 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef BT_MOTIONSTATE_H
#define BT_MOTIONSTATE_H
#include "btTransform.h"
///The btMotionState interface class allows the dynamics world to synchronize and interpolate the updated world transforms with graphics
///For optimizations, potentially only moving objects get synchronized (using setWorldPosition/setWorldOrientation)
class btMotionState
{
public:
virtual ~btMotionState()
{
}
virtual void getWorldTransform(btTransform& worldTrans ) const =0;
//Bullet only calls the update of worldtransform for active objects
virtual void setWorldTransform(const btTransform& worldTrans)=0;
};
#endif //BT_MOTIONSTATE_H

228
src/BulletCommon/btTransformUtil.h
View File

@@ -1,228 +0,0 @@
/*
Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans http://continuousphysics.com/Bullet/
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef BT_TRANSFORM_UTIL_H
#define BT_TRANSFORM_UTIL_H
#include "btTransform.h"
#define ANGULAR_MOTION_THRESHOLD btScalar(0.5)*SIMD_HALF_PI
SIMD_FORCE_INLINE btVector3 btAabbSupport(const btVector3& halfExtents,const btVector3& supportDir)
{
return btVector3(supportDir.getX() < btScalar(0.0) ? -halfExtents.getX() : halfExtents.getX(),
supportDir.getY() < btScalar(0.0) ? -halfExtents.getY() : halfExtents.getY(),
supportDir.getZ() < btScalar(0.0) ? -halfExtents.getZ() : halfExtents.getZ());
}
/// Utils related to temporal transforms
class btTransformUtil
{
public:
static void integrateTransform(const btTransform& curTrans,const btVector3& linvel,const btVector3& angvel,btScalar timeStep,btTransform& predictedTransform)
{
predictedTransform.setOrigin(curTrans.getOrigin() + linvel * timeStep);
// #define QUATERNION_DERIVATIVE
#ifdef QUATERNION_DERIVATIVE
btQuaternion predictedOrn = curTrans.getRotation();
predictedOrn += (angvel * predictedOrn) * (timeStep * btScalar(0.5));
predictedOrn.normalize();
#else
//Exponential map
//google for "Practical Parameterization of Rotations Using the Exponential Map", F. Sebastian Grassia
btVector3 axis;
btScalar fAngle = angvel.length();
//limit the angular motion
if (fAngle*timeStep > ANGULAR_MOTION_THRESHOLD)
{
fAngle = ANGULAR_MOTION_THRESHOLD / timeStep;
}
if ( fAngle < btScalar(0.001) )
{
// use Taylor's expansions of sync function
axis = angvel*( btScalar(0.5)*timeStep-(timeStep*timeStep*timeStep)*(btScalar(0.020833333333))*fAngle*fAngle );
}
else
{
// sync(fAngle) = sin(c*fAngle)/t
axis = angvel*( btSin(btScalar(0.5)*fAngle*timeStep)/fAngle );
}
btQuaternion dorn (axis.getX(),axis.getY(),axis.getZ(),btCos( fAngle*timeStep*btScalar(0.5) ));
btQuaternion orn0 = curTrans.getRotation();
btQuaternion predictedOrn = dorn * orn0;
predictedOrn.normalize();
#endif
predictedTransform.setRotation(predictedOrn);
}
static void calculateVelocityQuaternion(const btVector3& pos0,const btVector3& pos1,const btQuaternion& orn0,const btQuaternion& orn1,btScalar timeStep,btVector3& linVel,btVector3& angVel)
{
linVel = (pos1 - pos0) / timeStep;
btVector3 axis;
btScalar angle;
if (orn0 != orn1)
{
calculateDiffAxisAngleQuaternion(orn0,orn1,axis,angle);
angVel = axis * angle / timeStep;
} else
{
angVel.setValue(0,0,0);
}
}
static void calculateDiffAxisAngleQuaternion(const btQuaternion& orn0,const btQuaternion& orn1a,btVector3& axis,btScalar& angle)
{
btQuaternion orn1 = orn0.nearest(orn1a);
btQuaternion dorn = orn1 * orn0.inverse();
angle = dorn.getAngle();
axis = btVector3(dorn.getX(),dorn.getY(),dorn.getZ());
axis[3] = btScalar(0.);
//check for axis length
btScalar len = axis.length2();
if (len < SIMD_EPSILON*SIMD_EPSILON)
axis = btVector3(btScalar(1.),btScalar(0.),btScalar(0.));
else
axis /= btSqrt(len);
}
static void calculateVelocity(const btTransform& transform0,const btTransform& transform1,btScalar timeStep,btVector3& linVel,btVector3& angVel)
{
linVel = (transform1.getOrigin() - transform0.getOrigin()) / timeStep;
btVector3 axis;
btScalar angle;
calculateDiffAxisAngle(transform0,transform1,axis,angle);
angVel = axis * angle / timeStep;
}
static void calculateDiffAxisAngle(const btTransform& transform0,const btTransform& transform1,btVector3& axis,btScalar& angle)
{
btMatrix3x3 dmat = transform1.getBasis() * transform0.getBasis().inverse();
btQuaternion dorn;
dmat.getRotation(dorn);
///floating point inaccuracy can lead to w component > 1..., which breaks
dorn.normalize();
angle = dorn.getAngle();
axis = btVector3(dorn.getX(),dorn.getY(),dorn.getZ());
axis[3] = btScalar(0.);
//check for axis length
btScalar len = axis.length2();
if (len < SIMD_EPSILON*SIMD_EPSILON)
axis = btVector3(btScalar(1.),btScalar(0.),btScalar(0.));
else
axis /= btSqrt(len);
}
};
///The btConvexSeparatingDistanceUtil can help speed up convex collision detection
///by conservatively updating a cached separating distance/vector instead of re-calculating the closest distance
class btConvexSeparatingDistanceUtil
{
btQuaternion m_ornA;
btQuaternion m_ornB;
btVector3 m_posA;
btVector3 m_posB;
btVector3 m_separatingNormal;
btScalar m_boundingRadiusA;
btScalar m_boundingRadiusB;
btScalar m_separatingDistance;
public:
btConvexSeparatingDistanceUtil(btScalar boundingRadiusA,btScalar boundingRadiusB)
:m_boundingRadiusA(boundingRadiusA),
m_boundingRadiusB(boundingRadiusB),
m_separatingDistance(0.f)
{
}
btScalar getConservativeSeparatingDistance()
{
return m_separatingDistance;
}
void updateSeparatingDistance(const btTransform& transA,const btTransform& transB)
{
const btVector3& toPosA = transA.getOrigin();
const btVector3& toPosB = transB.getOrigin();
btQuaternion toOrnA = transA.getRotation();
btQuaternion toOrnB = transB.getRotation();
if (m_separatingDistance>0.f)
{
btVector3 linVelA,angVelA,linVelB,angVelB;
btTransformUtil::calculateVelocityQuaternion(m_posA,toPosA,m_ornA,toOrnA,btScalar(1.),linVelA,angVelA);
btTransformUtil::calculateVelocityQuaternion(m_posB,toPosB,m_ornB,toOrnB,btScalar(1.),linVelB,angVelB);
btScalar maxAngularProjectedVelocity = angVelA.length() * m_boundingRadiusA + angVelB.length() * m_boundingRadiusB;
btVector3 relLinVel = (linVelB-linVelA);
btScalar relLinVelocLength = relLinVel.dot(m_separatingNormal);
if (relLinVelocLength<0.f)
{
relLinVelocLength = 0.f;
}
btScalar projectedMotion = maxAngularProjectedVelocity +relLinVelocLength;
m_separatingDistance -= projectedMotion;
}
m_posA = toPosA;
m_posB = toPosB;
m_ornA = toOrnA;
m_ornB = toOrnB;
}
void initSeparatingDistance(const btVector3& separatingVector,btScalar separatingDistance,const btTransform& transA,const btTransform& transB)
{
m_separatingDistance = separatingDistance;
if (m_separatingDistance>0.f)
{
m_separatingNormal = separatingVector;
const btVector3& toPosA = transA.getOrigin();
const btVector3& toPosB = transB.getOrigin();
btQuaternion toOrnA = transA.getRotation();
btQuaternion toOrnB = transB.getRotation();
m_posA = toPosA;
m_posB = toPosB;
m_ornA = toOrnA;
m_ornB = toOrnB;
}
}
};
#endif //BT_TRANSFORM_UTIL_H

106
src/BulletGeometry/btAabbUtil2.h
View File

@@ -17,24 +17,24 @@ subject to the following restrictions:
#ifndef BT_AABB_UTIL2
#define BT_AABB_UTIL2
#include "BulletCommon/btTransform.h"
#include "BulletCommon/btVector3.h"
#include "BulletCommon/btMinMax.h"
#include "BulletCommon/b3Transform.h"
#include "BulletCommon/b3Vector3.h"
#include "BulletCommon/b3MinMax.h"
SIMD_FORCE_INLINE void AabbExpand (btVector3& aabbMin,
btVector3& aabbMax,
const btVector3& expansionMin,
const btVector3& expansionMax)
SIMD_FORCE_INLINE void AabbExpand (b3Vector3& aabbMin,
b3Vector3& aabbMax,
const b3Vector3& expansionMin,
const b3Vector3& expansionMax)
{
aabbMin = aabbMin + expansionMin;
aabbMax = aabbMax + expansionMax;
}
/// conservative test for overlap between two aabbs
SIMD_FORCE_INLINE bool TestPointAgainstAabb2(const btVector3 &aabbMin1, const btVector3 &aabbMax1,
const btVector3 &point)
SIMD_FORCE_INLINE bool TestPointAgainstAabb2(const b3Vector3 &aabbMin1, const b3Vector3 &aabbMax1,
const b3Vector3 &point)
{
bool overlap = true;
overlap = (aabbMin1.getX() > point.getX() || aabbMax1.getX() < point.getX()) ? false : overlap;
@@ -45,8 +45,8 @@ SIMD_FORCE_INLINE bool TestPointAgainstAabb2(const btVector3 &aabbMin1, const bt
/// conservative test for overlap between two aabbs
SIMD_FORCE_INLINE bool TestAabbAgainstAabb2(const btVector3 &aabbMin1, const btVector3 &aabbMax1,
const btVector3 &aabbMin2, const btVector3 &aabbMax2)
SIMD_FORCE_INLINE bool TestAabbAgainstAabb2(const b3Vector3 &aabbMin1, const b3Vector3 &aabbMax1,
const b3Vector3 &aabbMin2, const b3Vector3 &aabbMax2)
{
bool overlap = true;
overlap = (aabbMin1.getX() > aabbMax2.getX() || aabbMax1.getX() < aabbMin2.getX()) ? false : overlap;
@@ -56,12 +56,12 @@ SIMD_FORCE_INLINE bool TestAabbAgainstAabb2(const btVector3 &aabbMin1, const btV
}
/// conservative test for overlap between triangle and aabb
SIMD_FORCE_INLINE bool TestTriangleAgainstAabb2(const btVector3 *vertices,
const btVector3 &aabbMin, const btVector3 &aabbMax)
SIMD_FORCE_INLINE bool TestTriangleAgainstAabb2(const b3Vector3 *vertices,
const b3Vector3 &aabbMin, const b3Vector3 &aabbMax)
{
const btVector3 &p1 = vertices[0];
const btVector3 &p2 = vertices[1];
const btVector3 &p3 = vertices[2];
const b3Vector3 &p1 = vertices[0];
const b3Vector3 &p2 = vertices[1];
const b3Vector3 &p3 = vertices[2];
if (btMin(btMin(p1[0], p2[0]), p3[0]) > aabbMax[0]) return false;
if (btMax(btMax(p1[0], p2[0]), p3[0]) < aabbMin[0]) return false;
@@ -75,7 +75,7 @@ SIMD_FORCE_INLINE bool TestTriangleAgainstAabb2(const btVector3 *vertices,
}
SIMD_FORCE_INLINE int btOutcode(const btVector3& p,const btVector3& halfExtent)
SIMD_FORCE_INLINE int btOutcode(const b3Vector3& p,const b3Vector3& halfExtent)
{
return (p.getX() < -halfExtent.getX() ? 0x01 : 0x0) |
(p.getX() > halfExtent.getX() ? 0x08 : 0x0) |
@@ -87,15 +87,15 @@ SIMD_FORCE_INLINE int btOutcode(const btVector3& p,const btVector3& halfExtent)
SIMD_FORCE_INLINE bool btRayAabb2(const btVector3& rayFrom,
const btVector3& rayInvDirection,
SIMD_FORCE_INLINE bool btRayAabb2(const b3Vector3& rayFrom,
const b3Vector3& rayInvDirection,
const unsigned int raySign[3],
const btVector3 bounds[2],
btScalar& tmin,
btScalar lambda_min,
btScalar lambda_max)
const b3Vector3 bounds[2],
b3Scalar& tmin,
b3Scalar lambda_min,
b3Scalar lambda_max)
{
btScalar tmax, tymin, tymax, tzmin, tzmax;
b3Scalar tmax, tymin, tymax, tzmin, tzmax;
tmin = (bounds[raySign[0]].getX() - rayFrom.getX()) * rayInvDirection.getX();
tmax = (bounds[1-raySign[0]].getX() - rayFrom.getX()) * rayInvDirection.getX();
tymin = (bounds[raySign[1]].getY() - rayFrom.getY()) * rayInvDirection.getY();
@@ -122,26 +122,26 @@ SIMD_FORCE_INLINE bool btRayAabb2(const btVector3& rayFrom,
return ( (tmin < lambda_max) && (tmax > lambda_min) );
}
SIMD_FORCE_INLINE bool btRayAabb(const btVector3& rayFrom,
const btVector3& rayTo,
const btVector3& aabbMin,
const btVector3& aabbMax,
btScalar& param, btVector3& normal)
SIMD_FORCE_INLINE bool btRayAabb(const b3Vector3& rayFrom,
const b3Vector3& rayTo,
const b3Vector3& aabbMin,
const b3Vector3& aabbMax,
b3Scalar& param, b3Vector3& normal)
{
btVector3 aabbHalfExtent = (aabbMax-aabbMin)* btScalar(0.5);
btVector3 aabbCenter = (aabbMax+aabbMin)* btScalar(0.5);
btVector3 source = rayFrom - aabbCenter;
btVector3 target = rayTo - aabbCenter;
b3Vector3 aabbHalfExtent = (aabbMax-aabbMin)* b3Scalar(0.5);
b3Vector3 aabbCenter = (aabbMax+aabbMin)* b3Scalar(0.5);
b3Vector3 source = rayFrom - aabbCenter;
b3Vector3 target = rayTo - aabbCenter;
int sourceOutcode = btOutcode(source,aabbHalfExtent);
int targetOutcode = btOutcode(target,aabbHalfExtent);
if ((sourceOutcode & targetOutcode) == 0x0)
{
btScalar lambda_enter = btScalar(0.0);
btScalar lambda_exit = param;
btVector3 r = target - source;
b3Scalar lambda_enter = b3Scalar(0.0);
b3Scalar lambda_exit = param;
b3Vector3 r = target - source;
int i;
btScalar normSign = 1;
btVector3 hitNormal(0,0,0);
b3Scalar normSign = 1;
b3Vector3 hitNormal(0,0,0);
int bit=1;
for (int j=0;j<2;j++)
@@ -150,7 +150,7 @@ SIMD_FORCE_INLINE bool btRayAabb(const btVector3& rayFrom,
{
if (sourceOutcode & bit)
{
btScalar lambda = (-source[i] - aabbHalfExtent[i]*normSign) / r[i];
b3Scalar lambda = (-source[i] - aabbHalfExtent[i]*normSign) / r[i];
if (lambda_enter <= lambda)
{
lambda_enter = lambda;
@@ -160,12 +160,12 @@ SIMD_FORCE_INLINE bool btRayAabb(const btVector3& rayFrom,
}
else if (targetOutcode & bit)
{
btScalar lambda = (-source[i] - aabbHalfExtent[i]*normSign) / r[i];
b3Scalar lambda = (-source[i] - aabbHalfExtent[i]*normSign) / r[i];
btSetMin(lambda_exit, lambda);
}
bit<<=1;
}
normSign = btScalar(-1.);
normSign = b3Scalar(-1.);
}
if (lambda_enter <= lambda_exit)
{
@@ -179,29 +179,29 @@ SIMD_FORCE_INLINE bool btRayAabb(const btVector3& rayFrom,
SIMD_FORCE_INLINE void btTransformAabb(const btVector3& halfExtents, btScalar margin,const btTransform& t,btVector3& aabbMinOut,btVector3& aabbMaxOut)
SIMD_FORCE_INLINE void btTransformAabb(const b3Vector3& halfExtents, b3Scalar margin,const b3Transform& t,b3Vector3& aabbMinOut,b3Vector3& aabbMaxOut)
{
btVector3 halfExtentsWithMargin = halfExtents+btVector3(margin,margin,margin);
btMatrix3x3 abs_b = t.getBasis().absolute();
btVector3 center = t.getOrigin();
btVector3 extent = halfExtentsWithMargin.dot3( abs_b[0], abs_b[1], abs_b[2] );
b3Vector3 halfExtentsWithMargin = halfExtents+b3Vector3(margin,margin,margin);
b3Matrix3x3 abs_b = t.getBasis().absolute();
b3Vector3 center = t.getOrigin();
b3Vector3 extent = halfExtentsWithMargin.dot3( abs_b[0], abs_b[1], abs_b[2] );
aabbMinOut = center - extent;
aabbMaxOut = center + extent;
}
SIMD_FORCE_INLINE void btTransformAabb(const btVector3& localAabbMin,const btVector3& localAabbMax, btScalar margin,const btTransform& trans,btVector3& aabbMinOut,btVector3& aabbMaxOut)
SIMD_FORCE_INLINE void btTransformAabb(const b3Vector3& localAabbMin,const b3Vector3& localAabbMax, b3Scalar margin,const b3Transform& trans,b3Vector3& aabbMinOut,b3Vector3& aabbMaxOut)
{
btAssert(localAabbMin.getX() <= localAabbMax.getX());
btAssert(localAabbMin.getY() <= localAabbMax.getY());
btAssert(localAabbMin.getZ() <= localAabbMax.getZ());
btVector3 localHalfExtents = btScalar(0.5)*(localAabbMax-localAabbMin);
localHalfExtents+=btVector3(margin,margin,margin);
b3Vector3 localHalfExtents = b3Scalar(0.5)*(localAabbMax-localAabbMin);
localHalfExtents+=b3Vector3(margin,margin,margin);
btVector3 localCenter = btScalar(0.5)*(localAabbMax+localAabbMin);
btMatrix3x3 abs_b = trans.getBasis().absolute();
btVector3 center = trans(localCenter);
btVector3 extent = localHalfExtents.dot3( abs_b[0], abs_b[1], abs_b[2] );
b3Vector3 localCenter = b3Scalar(0.5)*(localAabbMax+localAabbMin);
b3Matrix3x3 abs_b = trans.getBasis().absolute();
b3Vector3 center = trans(localCenter);
b3Vector3 extent = localHalfExtents.dot3( abs_b[0], abs_b[1], abs_b[2] );
aabbMinOut = center-extent;
aabbMaxOut = center+extent;
}

316
src/BulletGeometry/btConvexHull.cpp
View File

@@ -16,9 +16,9 @@ subject to the following restrictions:
#include <string.h>
#include "btConvexHull.h"
#include "BulletCommon/btAlignedObjectArray.h"
#include "BulletCommon/btMinMax.h"
#include "BulletCommon/btVector3.h"
#include "BulletCommon/b3AlignedObjectArray.h"
#include "BulletCommon/b3MinMax.h"
#include "BulletCommon/b3Vector3.h"
@@ -47,77 +47,77 @@ inline int coplanar( const btPlane &a, const btPlane &b ) { return (a==b || a==P
//--------- Utility Functions ------
btVector3 PlaneLineIntersection(const btPlane &plane, const btVector3 &p0, const btVector3 &p1);
btVector3 PlaneProject(const btPlane &plane, const btVector3 &point);
b3Vector3 PlaneLineIntersection(const btPlane &plane, const b3Vector3 &p0, const b3Vector3 &p1);
b3Vector3 PlaneProject(const btPlane &plane, const b3Vector3 &point);
btVector3 ThreePlaneIntersection(const btPlane &p0,const btPlane &p1, const btPlane &p2);
btVector3 ThreePlaneIntersection(const btPlane &p0,const btPlane &p1, const btPlane &p2)
b3Vector3 ThreePlaneIntersection(const btPlane &p0,const btPlane &p1, const btPlane &p2);
b3Vector3 ThreePlaneIntersection(const btPlane &p0,const btPlane &p1, const btPlane &p2)
{
btVector3 N1 = p0.normal;
btVector3 N2 = p1.normal;
btVector3 N3 = p2.normal;
b3Vector3 N1 = p0.normal;
b3Vector3 N2 = p1.normal;
b3Vector3 N3 = p2.normal;
btVector3 n2n3; n2n3 = N2.cross(N3);
btVector3 n3n1; n3n1 = N3.cross(N1);
btVector3 n1n2; n1n2 = N1.cross(N2);
b3Vector3 n2n3; n2n3 = N2.cross(N3);
b3Vector3 n3n1; n3n1 = N3.cross(N1);
b3Vector3 n1n2; n1n2 = N1.cross(N2);
btScalar quotient = (N1.dot(n2n3));
b3Scalar quotient = (N1.dot(n2n3));
btAssert(btFabs(quotient) > btScalar(0.000001));
btAssert(btFabs(quotient) > b3Scalar(0.000001));
quotient = btScalar(-1.) / quotient;
quotient = b3Scalar(-1.) / quotient;
n2n3 *= p0.dist;
n3n1 *= p1.dist;
n1n2 *= p2.dist;
btVector3 potentialVertex = n2n3;
b3Vector3 potentialVertex = n2n3;
potentialVertex += n3n1;
potentialVertex += n1n2;
potentialVertex *= quotient;
btVector3 result(potentialVertex.getX(),potentialVertex.getY(),potentialVertex.getZ());
b3Vector3 result(potentialVertex.getX(),potentialVertex.getY(),potentialVertex.getZ());
return result;
}
btScalar DistanceBetweenLines(const btVector3 &ustart, const btVector3 &udir, const btVector3 &vstart, const btVector3 &vdir, btVector3 *upoint=NULL, btVector3 *vpoint=NULL);
btVector3 TriNormal(const btVector3 &v0, const btVector3 &v1, const btVector3 &v2);
btVector3 NormalOf(const btVector3 *vert, const int n);
b3Scalar DistanceBetweenLines(const b3Vector3 &ustart, const b3Vector3 &udir, const b3Vector3 &vstart, const b3Vector3 &vdir, b3Vector3 *upoint=NULL, b3Vector3 *vpoint=NULL);
b3Vector3 TriNormal(const b3Vector3 &v0, const b3Vector3 &v1, const b3Vector3 &v2);
b3Vector3 NormalOf(const b3Vector3 *vert, const int n);
btVector3 PlaneLineIntersection(const btPlane &plane, const btVector3 &p0, const btVector3 &p1)
b3Vector3 PlaneLineIntersection(const btPlane &plane, const b3Vector3 &p0, const b3Vector3 &p1)
{
// returns the point where the line p0-p1 intersects the plane n&d
static btVector3 dif;
static b3Vector3 dif;
dif = p1-p0;
btScalar dn= btDot(plane.normal,dif);
btScalar t = -(plane.dist+btDot(plane.normal,p0) )/dn;
b3Scalar dn= btDot(plane.normal,dif);
b3Scalar t = -(plane.dist+btDot(plane.normal,p0) )/dn;
return p0 + (dif*t);
}
btVector3 PlaneProject(const btPlane &plane, const btVector3 &point)
b3Vector3 PlaneProject(const btPlane &plane, const b3Vector3 &point)
{
return point - plane.normal * (btDot(point,plane.normal)+plane.dist);
}
btVector3 TriNormal(const btVector3 &v0, const btVector3 &v1, const btVector3 &v2)
b3Vector3 TriNormal(const b3Vector3 &v0, const b3Vector3 &v1, const b3Vector3 &v2)
{
// return the normal of the triangle
// inscribed by v0, v1, and v2
btVector3 cp=btCross(v1-v0,v2-v1);
btScalar m=cp.length();
if(m==0) return btVector3(1,0,0);
return cp*(btScalar(1.0)/m);
b3Vector3 cp=btCross(v1-v0,v2-v1);
b3Scalar m=cp.length();
if(m==0) return b3Vector3(1,0,0);
return cp*(b3Scalar(1.0)/m);
}
btScalar DistanceBetweenLines(const btVector3 &ustart, const btVector3 &udir, const btVector3 &vstart, const btVector3 &vdir, btVector3 *upoint, btVector3 *vpoint)
b3Scalar DistanceBetweenLines(const b3Vector3 &ustart, const b3Vector3 &udir, const b3Vector3 &vstart, const b3Vector3 &vdir, b3Vector3 *upoint, b3Vector3 *vpoint)
{
static btVector3 cp;
static b3Vector3 cp;
cp = btCross(udir,vdir).normalized();
btScalar distu = -btDot(cp,ustart);
btScalar distv = -btDot(cp,vstart);
btScalar dist = (btScalar)fabs(distu-distv);
b3Scalar distu = -btDot(cp,ustart);
b3Scalar distv = -btDot(cp,vstart);
b3Scalar dist = (b3Scalar)fabs(distu-distv);
if(upoint)
{
btPlane plane;
@@ -145,9 +145,9 @@ btScalar DistanceBetweenLines(const btVector3 &ustart, const btVector3 &udir, co
#define UNDER (1)
#define OVER (2)
#define SPLIT (OVER|UNDER)
#define PAPERWIDTH (btScalar(0.001))
#define PAPERWIDTH (b3Scalar(0.001))
btScalar planetestepsilon = PAPERWIDTH;
b3Scalar planetestepsilon = PAPERWIDTH;
@@ -161,9 +161,9 @@ ConvexH::ConvexH(int vertices_size,int edges_size,int facets_size)
}
int PlaneTest(const btPlane &p, const btVector3 &v);
int PlaneTest(const btPlane &p, const btVector3 &v) {
btScalar a = btDot(v,p.normal)+p.dist;
int PlaneTest(const btPlane &p, const b3Vector3 &v);
int PlaneTest(const btPlane &p, const b3Vector3 &v) {
b3Scalar a = btDot(v,p.normal)+p.dist;
int flag = (a>planetestepsilon)?OVER:((a<-planetestepsilon)?UNDER:COPLANAR);
return flag;
}
@@ -214,7 +214,7 @@ public:
template<class T>
int maxdirfiltered(const T *p,int count,const T &dir,btAlignedObjectArray<int> &allow)
int maxdirfiltered(const T *p,int count,const T &dir,b3AlignedObjectArray<int> &allow)
{
btAssert(count);
int m=-1;
@@ -228,11 +228,11 @@ int maxdirfiltered(const T *p,int count,const T &dir,btAlignedObjectArray<int> &
return m;
}
btVector3 orth(const btVector3 &v);
btVector3 orth(const btVector3 &v)
b3Vector3 orth(const b3Vector3 &v);
b3Vector3 orth(const b3Vector3 &v)
{
btVector3 a=btCross(v,btVector3(0,0,1));
btVector3 b=btCross(v,btVector3(0,1,0));
b3Vector3 a=btCross(v,b3Vector3(0,0,1));
b3Vector3 b=btCross(v,b3Vector3(0,1,0));
if (a.length() > b.length())
{
return a.normalized();
@@ -243,7 +243,7 @@ btVector3 orth(const btVector3 &v)
template<class T>
int maxdirsterid(const T *p,int count,const T &dir,btAlignedObjectArray<int> &allow)
int maxdirsterid(const T *p,int count,const T &dir,b3AlignedObjectArray<int> &allow)
{
int m=-1;
while(m==-1)
@@ -253,11 +253,11 @@ int maxdirsterid(const T *p,int count,const T &dir,btAlignedObjectArray<int> &al
T u = orth(dir);
T v = btCross(u,dir);
int ma=-1;
for(btScalar x = btScalar(0.0) ; x<= btScalar(360.0) ; x+= btScalar(45.0))
for(b3Scalar x = b3Scalar(0.0) ; x<= b3Scalar(360.0) ; x+= b3Scalar(45.0))
{
btScalar s = btSin(SIMD_RADS_PER_DEG*(x));
btScalar c = btCos(SIMD_RADS_PER_DEG*(x));
int mb = maxdirfiltered(p,count,dir+(u*s+v*c)*btScalar(0.025),allow);
b3Scalar s = btSin(SIMD_RADS_PER_DEG*(x));
b3Scalar c = btCos(SIMD_RADS_PER_DEG*(x));
int mb = maxdirfiltered(p,count,dir+(u*s+v*c)*b3Scalar(0.025),allow);
if(ma==m && mb==m)
{
allow[m]=3;
@@ -266,11 +266,11 @@ int maxdirsterid(const T *p,int count,const T &dir,btAlignedObjectArray<int> &al
if(ma!=-1 && ma!=mb) // Yuck - this is really ugly
{
int mc = ma;
for(btScalar xx = x-btScalar(40.0) ; xx <= x ; xx+= btScalar(5.0))
for(b3Scalar xx = x-b3Scalar(40.0) ; xx <= x ; xx+= b3Scalar(5.0))
{
btScalar s = btSin(SIMD_RADS_PER_DEG*(xx));
btScalar c = btCos(SIMD_RADS_PER_DEG*(xx));
int md = maxdirfiltered(p,count,dir+(u*s+v*c)*btScalar(0.025),allow);
b3Scalar s = btSin(SIMD_RADS_PER_DEG*(xx));
b3Scalar c = btCos(SIMD_RADS_PER_DEG*(xx));
int md = maxdirfiltered(p,count,dir+(u*s+v*c)*b3Scalar(0.025),allow);
if(mc==m && md==m)
{
allow[m]=3;
@@ -302,10 +302,10 @@ int operator ==(const int3 &a,const int3 &b)
}
int above(btVector3* vertices,const int3& t, const btVector3 &p, btScalar epsilon);
int above(btVector3* vertices,const int3& t, const btVector3 &p, btScalar epsilon)
int above(b3Vector3* vertices,const int3& t, const b3Vector3 &p, b3Scalar epsilon);
int above(b3Vector3* vertices,const int3& t, const b3Vector3 &p, b3Scalar epsilon)
{
btVector3 n=TriNormal(vertices[t[0]],vertices[t[1]],vertices[t[2]]);
b3Vector3 n=TriNormal(vertices[t[0]],vertices[t[1]],vertices[t[2]]);
return (btDot(n,p-vertices[t[0]]) > epsilon); // EPSILON???
}
int hasedge(const int3 &t, int a,int b);
@@ -345,11 +345,11 @@ public:
int3 n;
int id;
int vmax;
btScalar rise;
b3Scalar rise;
btHullTriangle(int a,int b,int c):int3(a,b,c),n(-1,-1,-1)
{
vmax=-1;
rise = btScalar(0.0);
rise = b3Scalar(0.0);
}
~btHullTriangle()
{
@@ -462,7 +462,7 @@ void HullLibrary::extrude(btHullTriangle *t0,int v)
}
btHullTriangle* HullLibrary::extrudable(btScalar epsilon)
btHullTriangle* HullLibrary::extrudable(b3Scalar epsilon)
{
int i;
btHullTriangle *t=NULL;
@@ -479,17 +479,17 @@ btHullTriangle* HullLibrary::extrudable(btScalar epsilon)
int4 HullLibrary::FindSimplex(btVector3 *verts,int verts_count,btAlignedObjectArray<int> &allow)
int4 HullLibrary::FindSimplex(b3Vector3 *verts,int verts_count,b3AlignedObjectArray<int> &allow)
{
btVector3 basis[3];
basis[0] = btVector3( btScalar(0.01), btScalar(0.02), btScalar(1.0) );
b3Vector3 basis[3];
basis[0] = b3Vector3( b3Scalar(0.01), b3Scalar(0.02), b3Scalar(1.0) );
int p0 = maxdirsterid(verts,verts_count, basis[0],allow);
int p1 = maxdirsterid(verts,verts_count,-basis[0],allow);
basis[0] = verts[p0]-verts[p1];
if(p0==p1 || basis[0]==btVector3(0,0,0))
if(p0==p1 || basis[0]==b3Vector3(0,0,0))
return int4(-1,-1,-1,-1);
basis[1] = btCross(btVector3( btScalar(1),btScalar(0.02), btScalar(0)),basis[0]);
basis[2] = btCross(btVector3(btScalar(-0.02), btScalar(1), btScalar(0)),basis[0]);
basis[1] = btCross(b3Vector3( b3Scalar(1),b3Scalar(0.02), b3Scalar(0)),basis[0]);
basis[2] = btCross(b3Vector3(b3Scalar(-0.02), b3Scalar(1), b3Scalar(0)),basis[0]);
if (basis[1].length() > basis[2].length())
{
basis[1].normalize();
@@ -515,15 +515,15 @@ int4 HullLibrary::FindSimplex(btVector3 *verts,int verts_count,btAlignedObjectAr
return int4(p0,p1,p2,p3);
}
int HullLibrary::calchullgen(btVector3 *verts,int verts_count, int vlimit)
int HullLibrary::calchullgen(b3Vector3 *verts,int verts_count, int vlimit)
{
if(verts_count <4) return 0;
if(vlimit==0) vlimit=1000000000;
int j;
btVector3 bmin(*verts),bmax(*verts);
btAlignedObjectArray<int> isextreme;
b3Vector3 bmin(*verts),bmax(*verts);
b3AlignedObjectArray<int> isextreme;
isextreme.reserve(verts_count);
btAlignedObjectArray<int> allow;
b3AlignedObjectArray<int> allow;
allow.reserve(verts_count);
for(j=0;j<verts_count;j++)
@@ -533,7 +533,7 @@ int HullLibrary::calchullgen(btVector3 *verts,int verts_count, int vlimit)
bmin.setMin (verts[j]);
bmax.setMax (verts[j]);
}
btScalar epsilon = (bmax-bmin).length() * btScalar(0.001);
b3Scalar epsilon = (bmax-bmin).length() * b3Scalar(0.001);
btAssert (epsilon != 0.0);
@@ -542,7 +542,7 @@ int HullLibrary::calchullgen(btVector3 *verts,int verts_count, int vlimit)
btVector3 center = (verts[p[0]]+verts[p[1]]+verts[p[2]]+verts[p[3]]) / btScalar(4.0); // a valid interior point
b3Vector3 center = (verts[p[0]]+verts[p[1]]+verts[p[2]]+verts[p[3]]) / b3Scalar(4.0); // a valid interior point
btHullTriangle *t0 = allocateTriangle(p[2],p[3],p[1]); t0->n=int3(2,3,1);
btHullTriangle *t1 = allocateTriangle(p[3],p[2],p[0]); t1->n=int3(3,2,0);
btHullTriangle *t2 = allocateTriangle(p[0],p[1],p[3]); t2->n=int3(0,1,3);
@@ -555,7 +555,7 @@ int HullLibrary::calchullgen(btVector3 *verts,int verts_count, int vlimit)
btHullTriangle *t=m_tris[j];
btAssert(t);
btAssert(t->vmax<0);
btVector3 n=TriNormal(verts[(*t)[0]],verts[(*t)[1]],verts[(*t)[2]]);
b3Vector3 n=TriNormal(verts[(*t)[0]],verts[(*t)[1]],verts[(*t)[2]]);
t->vmax = maxdirsterid(verts,verts_count,n,allow);
t->rise = btDot(n,verts[t->vmax]-verts[(*t)[0]]);
}
@@ -573,7 +573,7 @@ int HullLibrary::calchullgen(btVector3 *verts,int verts_count, int vlimit)
while(j--) {
if(!m_tris[j]) continue;
int3 t=*m_tris[j];
if(above(verts,t,verts[v],btScalar(0.01)*epsilon))
if(above(verts,t,verts[v],b3Scalar(0.01)*epsilon))
{
extrude(m_tris[j],v);
}
@@ -585,7 +585,7 @@ int HullLibrary::calchullgen(btVector3 *verts,int verts_count, int vlimit)
if(!m_tris[j]) continue;
if(!hasvert(*m_tris[j],v)) break;
int3 nt=*m_tris[j];
if(above(verts,nt,center,btScalar(0.01)*epsilon) || btCross(verts[nt[1]]-verts[nt[0]],verts[nt[2]]-verts[nt[1]]).length()< epsilon*epsilon*btScalar(0.1) )
if(above(verts,nt,center,b3Scalar(0.01)*epsilon) || btCross(verts[nt[1]]-verts[nt[0]],verts[nt[2]]-verts[nt[1]]).length()< epsilon*epsilon*b3Scalar(0.1) )
{
btHullTriangle *nb = m_tris[m_tris[j]->n[0]];
btAssert(nb);btAssert(!hasvert(*nb,v));btAssert(nb->id<j);
@@ -599,7 +599,7 @@ int HullLibrary::calchullgen(btVector3 *verts,int verts_count, int vlimit)
btHullTriangle *t=m_tris[j];
if(!t) continue;
if(t->vmax>=0) break;
btVector3 n=TriNormal(verts[(*t)[0]],verts[(*t)[1]],verts[(*t)[2]]);
b3Vector3 n=TriNormal(verts[(*t)[0]],verts[(*t)[1]],verts[(*t)[2]]);
t->vmax = maxdirsterid(verts,verts_count,n,allow);
if(isextreme[t->vmax])
{
@@ -615,11 +615,11 @@ int HullLibrary::calchullgen(btVector3 *verts,int verts_count, int vlimit)
return 1;
}
int HullLibrary::calchull(btVector3 *verts,int verts_count, TUIntArray& tris_out, int &tris_count,int vlimit)
int HullLibrary::calchull(b3Vector3 *verts,int verts_count, TUIntArray& tris_out, int &tris_count,int vlimit)
{
int rc=calchullgen(verts,verts_count, vlimit) ;
if(!rc) return 0;
btAlignedObjectArray<int> ts;
b3AlignedObjectArray<int> ts;
int i;
for(i=0;i<m_tris.size();i++)
@@ -647,15 +647,15 @@ int HullLibrary::calchull(btVector3 *verts,int verts_count, TUIntArray& tris_out
bool HullLibrary::ComputeHull(unsigned int vcount,const btVector3 *vertices,PHullResult &result,unsigned int vlimit)
bool HullLibrary::ComputeHull(unsigned int vcount,const b3Vector3 *vertices,PHullResult &result,unsigned int vlimit)
{
int tris_count;
int ret = calchull( (btVector3 *) vertices, (int) vcount, result.m_Indices, tris_count, static_cast<int>(vlimit) );
int ret = calchull( (b3Vector3 *) vertices, (int) vcount, result.m_Indices, tris_count, static_cast<int>(vlimit) );
if(!ret) return false;
result.mIndexCount = (unsigned int) (tris_count*3);
result.mFaceCount = (unsigned int) tris_count;
result.mVertices = (btVector3*) vertices;
result.mVertices = (b3Vector3*) vertices;
result.mVcount = (unsigned int) vcount;
return true;
@@ -699,10 +699,10 @@ HullError HullLibrary::CreateConvexHull(const HullDesc &desc, //
unsigned int vcount = desc.mVcount;
if ( vcount < 8 ) vcount = 8;
btAlignedObjectArray<btVector3> vertexSource;
b3AlignedObjectArray<b3Vector3> vertexSource;
vertexSource.resize(static_cast<int>(vcount));
btVector3 scale;
b3Vector3 scale;
unsigned int ovcount;
@@ -716,7 +716,7 @@ HullError HullLibrary::CreateConvexHull(const HullDesc &desc, //
{
for (unsigned int i=0; i<ovcount; i++)
{
btVector3& v = vertexSource[static_cast<int>(i)];
b3Vector3& v = vertexSource[static_cast<int>(i)];
v[0]*=scale[0];
v[1]*=scale[1];
v[2]*=scale[2];
@@ -729,7 +729,7 @@ HullError HullLibrary::CreateConvexHull(const HullDesc &desc, //
{
// re-index triangle mesh so it refers to only used vertices, rebuild a new vertex table.
btAlignedObjectArray<btVector3> vertexScratch;
b3AlignedObjectArray<b3Vector3> vertexScratch;
vertexScratch.resize(static_cast<int>(hr.mVcount));
BringOutYourDead(hr.mVertices,hr.mVcount, &vertexScratch[0], ovcount, &hr.m_Indices[0], hr.mIndexCount );
@@ -746,7 +746,7 @@ HullError HullLibrary::CreateConvexHull(const HullDesc &desc, //
result.m_Indices.resize(static_cast<int>(hr.mIndexCount));
memcpy(&result.m_OutputVertices[0], &vertexScratch[0], sizeof(btVector3)*ovcount );
memcpy(&result.m_OutputVertices[0], &vertexScratch[0], sizeof(b3Vector3)*ovcount );
if ( desc.HasHullFlag(QF_REVERSE_ORDER) )
{
@@ -777,7 +777,7 @@ HullError HullLibrary::CreateConvexHull(const HullDesc &desc, //
result.mNumFaces = hr.mFaceCount;
result.mNumIndices = hr.mIndexCount+hr.mFaceCount;
result.m_Indices.resize(static_cast<int>(result.mNumIndices));
memcpy(&result.m_OutputVertices[0], &vertexScratch[0], sizeof(btVector3)*ovcount );
memcpy(&result.m_OutputVertices[0], &vertexScratch[0], sizeof(b3Vector3)*ovcount );
// if ( 1 )
{
@@ -829,23 +829,23 @@ HullError HullLibrary::ReleaseResult(HullResult &result) // release memory alloc
}
static void addPoint(unsigned int &vcount,btVector3 *p,btScalar x,btScalar y,btScalar z)
static void addPoint(unsigned int &vcount,b3Vector3 *p,b3Scalar x,b3Scalar y,b3Scalar z)
{
// XXX, might be broken
btVector3& dest = p[vcount];
b3Vector3& dest = p[vcount];
dest[0] = x;
dest[1] = y;
dest[2] = z;
vcount++;
}
btScalar GetDist(btScalar px,btScalar py,btScalar pz,const btScalar *p2);
btScalar GetDist(btScalar px,btScalar py,btScalar pz,const btScalar *p2)
b3Scalar GetDist(b3Scalar px,b3Scalar py,b3Scalar pz,const b3Scalar *p2);
b3Scalar GetDist(b3Scalar px,b3Scalar py,b3Scalar pz,const b3Scalar *p2)
{
btScalar dx = px - p2[0];
btScalar dy = py - p2[1];
btScalar dz = pz - p2[2];
b3Scalar dx = px - p2[0];
b3Scalar dy = py - p2[1];
b3Scalar dz = pz - p2[2];
return dx*dx+dy*dy+dz*dz;
}
@@ -853,23 +853,23 @@ btScalar GetDist(btScalar px,btScalar py,btScalar pz,const btScalar *p2)
bool HullLibrary::CleanupVertices(unsigned int svcount,
const btVector3 *svertices,
const b3Vector3 *svertices,
unsigned int stride,
unsigned int &vcount, // output number of vertices
btVector3 *vertices, // location to store the results.
btScalar normalepsilon,
btVector3& scale)
b3Vector3 *vertices, // location to store the results.
b3Scalar normalepsilon,
b3Vector3& scale)
{
if ( svcount == 0 ) return false;
m_vertexIndexMapping.resize(0);
#define EPSILON btScalar(0.000001) /* close enough to consider two btScalaring point numbers to be 'the same'. */
#define EPSILON b3Scalar(0.000001) /* close enough to consider two btScalaring point numbers to be 'the same'. */
vcount = 0;
btScalar recip[3]={0.f,0.f,0.f};
b3Scalar recip[3]={0.f,0.f,0.f};
if ( scale )
{
@@ -878,8 +878,8 @@ bool HullLibrary::CleanupVertices(unsigned int svcount,
scale[2] = 1;
}
btScalar bmin[3] = { FLT_MAX, FLT_MAX, FLT_MAX };
btScalar bmax[3] = { -FLT_MAX, -FLT_MAX, -FLT_MAX };
b3Scalar bmin[3] = { FLT_MAX, FLT_MAX, FLT_MAX };
b3Scalar bmax[3] = { -FLT_MAX, -FLT_MAX, -FLT_MAX };
const char *vtx = (const char *) svertices;
@@ -887,7 +887,7 @@ bool HullLibrary::CleanupVertices(unsigned int svcount,
{
for (unsigned int i=0; i<svcount; i++)
{
const btScalar *p = (const btScalar *) vtx;
const b3Scalar *p = (const b3Scalar *) vtx;
vtx+=stride;
@@ -899,20 +899,20 @@ bool HullLibrary::CleanupVertices(unsigned int svcount,
}
}
btScalar dx = bmax[0] - bmin[0];
btScalar dy = bmax[1] - bmin[1];
btScalar dz = bmax[2] - bmin[2];
b3Scalar dx = bmax[0] - bmin[0];
b3Scalar dy = bmax[1] - bmin[1];
b3Scalar dz = bmax[2] - bmin[2];
btVector3 center;
b3Vector3 center;
center[0] = dx*btScalar(0.5) + bmin[0];
center[1] = dy*btScalar(0.5) + bmin[1];
center[2] = dz*btScalar(0.5) + bmin[2];
center[0] = dx*b3Scalar(0.5) + bmin[0];
center[1] = dy*b3Scalar(0.5) + bmin[1];
center[2] = dz*b3Scalar(0.5) + bmin[2];
if ( dx < EPSILON || dy < EPSILON || dz < EPSILON || svcount < 3 )
{
btScalar len = FLT_MAX;
b3Scalar len = FLT_MAX;
if ( dx > EPSILON && dx < len ) len = dx;
if ( dy > EPSILON && dy < len ) len = dy;
@@ -920,23 +920,23 @@ bool HullLibrary::CleanupVertices(unsigned int svcount,
if ( len == FLT_MAX )
{
dx = dy = dz = btScalar(0.01); // one centimeter
dx = dy = dz = b3Scalar(0.01); // one centimeter
}
else
{
if ( dx < EPSILON ) dx = len * btScalar(0.05); // 1/5th the shortest non-zero edge.
if ( dy < EPSILON ) dy = len * btScalar(0.05);
if ( dz < EPSILON ) dz = len * btScalar(0.05);
if ( dx < EPSILON ) dx = len * b3Scalar(0.05); // 1/5th the shortest non-zero edge.
if ( dy < EPSILON ) dy = len * b3Scalar(0.05);
if ( dz < EPSILON ) dz = len * b3Scalar(0.05);
}
btScalar x1 = center[0] - dx;
btScalar x2 = center[0] + dx;
b3Scalar x1 = center[0] - dx;
b3Scalar x2 = center[0] + dx;
btScalar y1 = center[1] - dy;
btScalar y2 = center[1] + dy;
b3Scalar y1 = center[1] - dy;
b3Scalar y2 = center[1] + dy;
btScalar z1 = center[2] - dz;
btScalar z2 = center[2] + dz;
b3Scalar z1 = center[2] - dz;
b3Scalar z2 = center[2] + dz;
addPoint(vcount,vertices,x1,y1,z1);
addPoint(vcount,vertices,x2,y1,z1);
@@ -977,12 +977,12 @@ bool HullLibrary::CleanupVertices(unsigned int svcount,
for (unsigned int i=0; i<svcount; i++)
{
const btVector3 *p = (const btVector3 *)vtx;
const b3Vector3 *p = (const b3Vector3 *)vtx;
vtx+=stride;
btScalar px = p->getX();
btScalar py = p->getY();
btScalar pz = p->getZ();
b3Scalar px = p->getX();
b3Scalar py = p->getY();
b3Scalar pz = p->getZ();
if ( scale )
{
@@ -998,15 +998,15 @@ bool HullLibrary::CleanupVertices(unsigned int svcount,
for (j=0; j<vcount; j++)
{
/// XXX might be broken
btVector3& v = vertices[j];
b3Vector3& v = vertices[j];
btScalar x = v[0];
btScalar y = v[1];
btScalar z = v[2];
b3Scalar x = v[0];
b3Scalar y = v[1];
b3Scalar z = v[2];
btScalar dx = btFabs(x - px );
btScalar dy = btFabs(y - py );
btScalar dz = btFabs(z - pz );
b3Scalar dx = btFabs(x - px );
b3Scalar dy = btFabs(y - py );
b3Scalar dz = btFabs(z - pz );
if ( dx < normalepsilon && dy < normalepsilon && dz < normalepsilon )
{
@@ -1014,8 +1014,8 @@ bool HullLibrary::CleanupVertices(unsigned int svcount,
// now let us see if it is further from the center of the point cloud than the one we already recorded.
// in which case we keep this one instead.
btScalar dist1 = GetDist(px,py,pz,center);
btScalar dist2 = GetDist(v[0],v[1],v[2],center);
b3Scalar dist1 = GetDist(px,py,pz,center);
b3Scalar dist2 = GetDist(v[0],v[1],v[2],center);
if ( dist1 > dist2 )
{
@@ -1031,7 +1031,7 @@ bool HullLibrary::CleanupVertices(unsigned int svcount,
if ( j == vcount )
{
btVector3& dest = vertices[vcount];
b3Vector3& dest = vertices[vcount];
dest[0] = px;
dest[1] = py;
dest[2] = pz;
@@ -1044,12 +1044,12 @@ bool HullLibrary::CleanupVertices(unsigned int svcount,
// ok..now make sure we didn't prune so many vertices it is now invalid.
// if ( 1 )
{
btScalar bmin[3] = { FLT_MAX, FLT_MAX, FLT_MAX };
btScalar bmax[3] = { -FLT_MAX, -FLT_MAX, -FLT_MAX };
b3Scalar bmin[3] = { FLT_MAX, FLT_MAX, FLT_MAX };
b3Scalar bmax[3] = { -FLT_MAX, -FLT_MAX, -FLT_MAX };
for (unsigned int i=0; i<vcount; i++)
{
const btVector3& p = vertices[i];
const b3Vector3& p = vertices[i];
for (int j=0; j<3; j++)
{
if ( p[j] < bmin[j] ) bmin[j] = p[j];
@@ -1057,17 +1057,17 @@ bool HullLibrary::CleanupVertices(unsigned int svcount,
}
}
btScalar dx = bmax[0] - bmin[0];
btScalar dy = bmax[1] - bmin[1];
btScalar dz = bmax[2] - bmin[2];
b3Scalar dx = bmax[0] - bmin[0];
b3Scalar dy = bmax[1] - bmin[1];
b3Scalar dz = bmax[2] - bmin[2];
if ( dx < EPSILON || dy < EPSILON || dz < EPSILON || vcount < 3)
{
btScalar cx = dx*btScalar(0.5) + bmin[0];
btScalar cy = dy*btScalar(0.5) + bmin[1];
btScalar cz = dz*btScalar(0.5) + bmin[2];
b3Scalar cx = dx*b3Scalar(0.5) + bmin[0];
b3Scalar cy = dy*b3Scalar(0.5) + bmin[1];
b3Scalar cz = dz*b3Scalar(0.5) + bmin[2];
btScalar len = FLT_MAX;
b3Scalar len = FLT_MAX;
if ( dx >= EPSILON && dx < len ) len = dx;
if ( dy >= EPSILON && dy < len ) len = dy;
@@ -1075,23 +1075,23 @@ bool HullLibrary::CleanupVertices(unsigned int svcount,
if ( len == FLT_MAX )
{
dx = dy = dz = btScalar(0.01); // one centimeter
dx = dy = dz = b3Scalar(0.01); // one centimeter
}
else
{
if ( dx < EPSILON ) dx = len * btScalar(0.05); // 1/5th the shortest non-zero edge.
if ( dy < EPSILON ) dy = len * btScalar(0.05);
if ( dz < EPSILON ) dz = len * btScalar(0.05);
if ( dx < EPSILON ) dx = len * b3Scalar(0.05); // 1/5th the shortest non-zero edge.
if ( dy < EPSILON ) dy = len * b3Scalar(0.05);
if ( dz < EPSILON ) dz = len * b3Scalar(0.05);
}
btScalar x1 = cx - dx;
btScalar x2 = cx + dx;
b3Scalar x1 = cx - dx;
b3Scalar x2 = cx + dx;
btScalar y1 = cy - dy;
btScalar y2 = cy + dy;
b3Scalar y1 = cy - dy;
b3Scalar y2 = cy + dy;
btScalar z1 = cz - dz;
btScalar z2 = cz + dz;
b3Scalar z1 = cz - dz;
b3Scalar z2 = cz + dz;
vcount = 0; // add box
@@ -1111,9 +1111,9 @@ bool HullLibrary::CleanupVertices(unsigned int svcount,
return true;
}
void HullLibrary::BringOutYourDead(const btVector3* verts,unsigned int vcount, btVector3* overts,unsigned int &ocount,unsigned int *indices,unsigned indexcount)
void HullLibrary::BringOutYourDead(const b3Vector3* verts,unsigned int vcount, b3Vector3* overts,unsigned int &ocount,unsigned int *indices,unsigned indexcount)
{
btAlignedObjectArray<int>tmpIndices;
b3AlignedObjectArray<int>tmpIndices;
tmpIndices.resize(m_vertexIndexMapping.size());
int i;

60
src/BulletGeometry/btConvexHull.h
View File

@@ -19,10 +19,10 @@ subject to the following restrictions:
#ifndef BT_CD_HULL_H
#define BT_CD_HULL_H
#include "BulletCommon/btVector3.h"
#include "BulletCommon/btAlignedObjectArray.h"
#include "BulletCommon/b3Vector3.h"
#include "BulletCommon/b3AlignedObjectArray.h"
typedef btAlignedObjectArray<unsigned int> TUIntArray;
typedef b3AlignedObjectArray<unsigned int> TUIntArray;
class HullResult
{
@@ -36,10 +36,10 @@ public:
}
bool mPolygons; // true if indices represents polygons, false indices are triangles
unsigned int mNumOutputVertices; // number of vertices in the output hull
btAlignedObjectArray<btVector3> m_OutputVertices; // array of vertices
b3AlignedObjectArray<b3Vector3> m_OutputVertices; // array of vertices
unsigned int mNumFaces; // the number of faces produced
unsigned int mNumIndices; // the total number of indices
btAlignedObjectArray<unsigned int> m_Indices; // pointer to indices.
b3AlignedObjectArray<unsigned int> m_Indices; // pointer to indices.
// If triangles, then indices are array indexes into the vertex list.
// If polygons, indices are in the form (number of points in face) (p1, p2, p3, ..) etc..
@@ -61,7 +61,7 @@ public:
mFlags = QF_DEFAULT;
mVcount = 0;
mVertices = 0;
mVertexStride = sizeof(btVector3);
mVertexStride = sizeof(b3Vector3);
mNormalEpsilon = 0.001f;
mMaxVertices = 4096; // maximum number of points to be considered for a convex hull.
mMaxFaces = 4096;
@@ -69,14 +69,14 @@ public:
HullDesc(HullFlag flag,
unsigned int vcount,
const btVector3 *vertices,
unsigned int stride = sizeof(btVector3))
const b3Vector3 *vertices,
unsigned int stride = sizeof(b3Vector3))
{
mFlags = flag;
mVcount = vcount;
mVertices = vertices;
mVertexStride = stride;
mNormalEpsilon = btScalar(0.001);
mNormalEpsilon = b3Scalar(0.001);
mMaxVertices = 4096;
}
@@ -98,9 +98,9 @@ public:
unsigned int mFlags; // flags to use when generating the convex hull.
unsigned int mVcount; // number of vertices in the input point cloud
const btVector3 *mVertices; // the array of vertices.
const b3Vector3 *mVertices; // the array of vertices.
unsigned int mVertexStride; // the stride of each vertex, in bytes.
btScalar mNormalEpsilon; // the epsilon for removing duplicates. This is a normalized value, if normalized bit is on.
b3Scalar mNormalEpsilon; // the epsilon for removing duplicates. This is a normalized value, if normalized bit is on.
unsigned int mMaxVertices; // maximum number of vertices to be considered for the hull!
unsigned int mMaxFaces;
};
@@ -114,9 +114,9 @@ enum HullError
class btPlane
{
public:
btVector3 normal;
btScalar dist; // distance below origin - the D from plane equasion Ax+By+Cz+D=0
btPlane(const btVector3 &n,btScalar d):normal(n),dist(d){}
b3Vector3 normal;
b3Scalar dist; // distance below origin - the D from plane equasion Ax+By+Cz+D=0
btPlane(const b3Vector3 &n,b3Scalar d):normal(n),dist(d){}
btPlane():normal(),dist(0){}
};
@@ -141,9 +141,9 @@ class ConvexH
~ConvexH()
{
}
btAlignedObjectArray<btVector3> vertices;
btAlignedObjectArray<HalfEdge> edges;
btAlignedObjectArray<btPlane> facets;
b3AlignedObjectArray<b3Vector3> vertices;
b3AlignedObjectArray<HalfEdge> edges;
b3AlignedObjectArray<btPlane> facets;
ConvexH(int vertices_size,int edges_size,int facets_size);
};
@@ -173,7 +173,7 @@ public:
unsigned int mVcount;
unsigned int mIndexCount;
unsigned int mFaceCount;
btVector3* mVertices;
b3Vector3* mVertices;
TUIntArray m_Indices;
};
@@ -184,11 +184,11 @@ public:
class HullLibrary
{
btAlignedObjectArray<class btHullTriangle*> m_tris;
b3AlignedObjectArray<class btHullTriangle*> m_tris;
public:
btAlignedObjectArray<int> m_vertexIndexMapping;
b3AlignedObjectArray<int> m_vertexIndexMapping;
HullError CreateConvexHull(const HullDesc& desc, // describes the input request
@@ -197,7 +197,7 @@ public:
private:
bool ComputeHull(unsigned int vcount,const btVector3 *vertices,PHullResult &result,unsigned int vlimit);
bool ComputeHull(unsigned int vcount,const b3Vector3 *vertices,PHullResult &result,unsigned int vlimit);
class btHullTriangle* allocateTriangle(int a,int b,int c);
void deAllocateTriangle(btHullTriangle*);
@@ -207,13 +207,13 @@ private:
void checkit(btHullTriangle *t);
btHullTriangle* extrudable(btScalar epsilon);
btHullTriangle* extrudable(b3Scalar epsilon);
int calchull(btVector3 *verts,int verts_count, TUIntArray& tris_out, int &tris_count,int vlimit);
int calchull(b3Vector3 *verts,int verts_count, TUIntArray& tris_out, int &tris_count,int vlimit);
int calchullgen(btVector3 *verts,int verts_count, int vlimit);
int calchullgen(b3Vector3 *verts,int verts_count, int vlimit);
int4 FindSimplex(btVector3 *verts,int verts_count,btAlignedObjectArray<int> &allow);
int4 FindSimplex(b3Vector3 *verts,int verts_count,b3AlignedObjectArray<int> &allow);
class ConvexH* ConvexHCrop(ConvexH& convex,const btPlane& slice);
@@ -225,15 +225,15 @@ private:
//After the hull is generated it give you back a set of polygon faces which index the *original* point cloud.
//The thing is, often times, there are many 'dead vertices' in the point cloud that are on longer referenced by the hull.
//The routine 'BringOutYourDead' find only the referenced vertices, copies them to an new buffer, and re-indexes the hull so that it is a minimal representation.
void BringOutYourDead(const btVector3* verts,unsigned int vcount, btVector3* overts,unsigned int &ocount,unsigned int* indices,unsigned indexcount);
void BringOutYourDead(const b3Vector3* verts,unsigned int vcount, b3Vector3* overts,unsigned int &ocount,unsigned int* indices,unsigned indexcount);
bool CleanupVertices(unsigned int svcount,
const btVector3* svertices,
const b3Vector3* svertices,
unsigned int stride,
unsigned int &vcount, // output number of vertices
btVector3* vertices, // location to store the results.
btScalar normalepsilon,
btVector3& scale);
b3Vector3* vertices, // location to store the results.
b3Scalar normalepsilon,
b3Vector3& scale);
};

116
src/BulletGeometry/btConvexHullComputer.cpp
View File

@@ -15,9 +15,9 @@ subject to the following restrictions:
#include <string.h>
#include "btConvexHullComputer.h"
#include "BulletCommon/btAlignedObjectArray.h"
#include "BulletCommon/btMinMax.h"
#include "BulletCommon/btVector3.h"
#include "BulletCommon/b3AlignedObjectArray.h"
#include "BulletCommon/b3MinMax.h"
#include "BulletCommon/b3Vector3.h"
#ifdef __GNUC__
#include <stdint.h>
@@ -230,9 +230,9 @@ class btConvexHullInternal
Int128 operator*(int64_t b) const;
btScalar toScalar() const
b3Scalar toScalar() const
{
return ((int64_t) high >= 0) ? btScalar(high) * (btScalar(0x100000000LL) * btScalar(0x100000000LL)) + btScalar(low)
return ((int64_t) high >= 0) ? b3Scalar(high) * (b3Scalar(0x100000000LL) * b3Scalar(0x100000000LL)) + b3Scalar(low)
: -(-*this).toScalar();
}
@@ -321,9 +321,9 @@ class btConvexHullInternal
int compare(const Rational64& b) const;
btScalar toScalar() const
b3Scalar toScalar() const
{
return sign * ((m_denominator == 0) ? SIMD_INFINITY : (btScalar) m_numerator / m_denominator);
return sign * ((m_denominator == 0) ? SIMD_INFINITY : (b3Scalar) m_numerator / m_denominator);
}
};
@@ -386,7 +386,7 @@ class btConvexHullInternal
int compare(int64_t b) const;
btScalar toScalar() const
b3Scalar toScalar() const
{
return sign * ((denominator.getSign() == 0) ? SIMD_INFINITY : numerator.toScalar() / denominator.toScalar());
}
@@ -408,17 +408,17 @@ class btConvexHullInternal
{
}
btScalar xvalue() const
b3Scalar xvalue() const
{
return x.toScalar() / denominator.toScalar();
}
btScalar yvalue() const
b3Scalar yvalue() const
{
return y.toScalar() / denominator.toScalar();
}
btScalar zvalue() const
b3Scalar zvalue() const
{
return z.toScalar() / denominator.toScalar();
}
@@ -464,19 +464,19 @@ class btConvexHullInternal
: Rational128(point128.x * b.x + point128.y * b.y + point128.z * b.z, point128.denominator);
}
btScalar xvalue() const
b3Scalar xvalue() const
{
return (point.index >= 0) ? btScalar(point.x) : point128.xvalue();
return (point.index >= 0) ? b3Scalar(point.x) : point128.xvalue();
}
btScalar yvalue() const
b3Scalar yvalue() const
{
return (point.index >= 0) ? btScalar(point.y) : point128.yvalue();
return (point.index >= 0) ? b3Scalar(point.y) : point128.yvalue();
}
btScalar zvalue() const
b3Scalar zvalue() const
{
return (point.index >= 0) ? btScalar(point.z) : point128.zvalue();
return (point.index >= 0) ? b3Scalar(point.z) : point128.zvalue();
}
void receiveNearbyFaces(Vertex* src)
@@ -757,12 +757,12 @@ class btConvexHullInternal
}
};
btVector3 scaling;
btVector3 center;
b3Vector3 scaling;
b3Vector3 center;
Pool<Vertex> vertexPool;
Pool<Edge> edgePool;
Pool<Face> facePool;
btAlignedObjectArray<Vertex*> originalVertices;
b3AlignedObjectArray<Vertex*> originalVertices;
int mergeStamp;
int minAxis;
int medAxis;
@@ -818,20 +818,20 @@ class btConvexHullInternal
void merge(IntermediateHull& h0, IntermediateHull& h1);
btVector3 toBtVector(const Point32& v);
b3Vector3 toBtVector(const Point32& v);
btVector3 getBtNormal(Face* face);
b3Vector3 getBtNormal(Face* face);
bool shiftFace(Face* face, btScalar amount, btAlignedObjectArray<Vertex*> stack);
bool shiftFace(Face* face, b3Scalar amount, b3AlignedObjectArray<Vertex*> stack);
public:
Vertex* vertexList;
void compute(const void* coords, bool doubleCoords, int stride, int count);
btVector3 getCoordinates(const Vertex* v);
b3Vector3 getCoordinates(const Vertex* v);
btScalar shrink(btScalar amount, btScalar clampAmount);
b3Scalar shrink(b3Scalar amount, b3Scalar clampAmount);
};
@@ -1939,14 +1939,14 @@ static bool pointCmp(const btConvexHullInternal::Point32& p, const btConvexHullI
void btConvexHullInternal::compute(const void* coords, bool doubleCoords, int stride, int count)
{
btVector3 min(btScalar(1e30), btScalar(1e30), btScalar(1e30)), max(btScalar(-1e30), btScalar(-1e30), btScalar(-1e30));
b3Vector3 min(b3Scalar(1e30), b3Scalar(1e30), b3Scalar(1e30)), max(b3Scalar(-1e30), b3Scalar(-1e30), b3Scalar(-1e30));
const char* ptr = (const char*) coords;
if (doubleCoords)
{
for (int i = 0; i < count; i++)
{
const double* v = (const double*) ptr;
btVector3 p((btScalar) v[0], (btScalar) v[1], (btScalar) v[2]);
b3Vector3 p((b3Scalar) v[0], (b3Scalar) v[1], (b3Scalar) v[2]);
ptr += stride;
min.setMin(p);
max.setMax(p);
@@ -1957,14 +1957,14 @@ void btConvexHullInternal::compute(const void* coords, bool doubleCoords, int st
for (int i = 0; i < count; i++)
{
const float* v = (const float*) ptr;
btVector3 p(v[0], v[1], v[2]);
b3Vector3 p(v[0], v[1], v[2]);
ptr += stride;
min.setMin(p);
max.setMax(p);
}
}
btVector3 s = max - min;
b3Vector3 s = max - min;
maxAxis = s.maxAxis();
minAxis = s.minAxis();
if (minAxis == maxAxis)
@@ -1973,7 +1973,7 @@ void btConvexHullInternal::compute(const void* coords, bool doubleCoords, int st
}
medAxis = 3 - maxAxis - minAxis;
s /= btScalar(10216);
s /= b3Scalar(10216);
if (((medAxis + 1) % 3) != maxAxis)
{
s *= -1;
@@ -1982,20 +1982,20 @@ void btConvexHullInternal::compute(const void* coords, bool doubleCoords, int st
if (s[0] != 0)
{
s[0] = btScalar(1) / s[0];
s[0] = b3Scalar(1) / s[0];
}
if (s[1] != 0)
{
s[1] = btScalar(1) / s[1];
s[1] = b3Scalar(1) / s[1];
}
if (s[2] != 0)
{
s[2] = btScalar(1) / s[2];
s[2] = b3Scalar(1) / s[2];
}
center = (min + max) * btScalar(0.5);
center = (min + max) * b3Scalar(0.5);
btAlignedObjectArray<Point32> points;
b3AlignedObjectArray<Point32> points;
points.resize(count);
ptr = (const char*) coords;
if (doubleCoords)
@@ -2003,7 +2003,7 @@ void btConvexHullInternal::compute(const void* coords, bool doubleCoords, int st
for (int i = 0; i < count; i++)
{
const double* v = (const double*) ptr;
btVector3 p((btScalar) v[0], (btScalar) v[1], (btScalar) v[2]);
b3Vector3 p((b3Scalar) v[0], (b3Scalar) v[1], (b3Scalar) v[2]);
ptr += stride;
p = (p - center) * s;
points[i].x = (int32_t) p[medAxis];
@@ -2017,7 +2017,7 @@ void btConvexHullInternal::compute(const void* coords, bool doubleCoords, int st
for (int i = 0; i < count; i++)
{
const float* v = (const float*) ptr;
btVector3 p(v[0], v[1], v[2]);
b3Vector3 p(v[0], v[1], v[2]);
ptr += stride;
p = (p - center) * s;
points[i].x = (int32_t) p[medAxis];
@@ -2058,40 +2058,40 @@ void btConvexHullInternal::compute(const void* coords, bool doubleCoords, int st
#endif
}
btVector3 btConvexHullInternal::toBtVector(const Point32& v)
b3Vector3 btConvexHullInternal::toBtVector(const Point32& v)
{
btVector3 p;
p[medAxis] = btScalar(v.x);
p[maxAxis] = btScalar(v.y);
p[minAxis] = btScalar(v.z);
b3Vector3 p;
p[medAxis] = b3Scalar(v.x);
p[maxAxis] = b3Scalar(v.y);
p[minAxis] = b3Scalar(v.z);
return p * scaling;
}
btVector3 btConvexHullInternal::getBtNormal(Face* face)
b3Vector3 btConvexHullInternal::getBtNormal(Face* face)
{
return toBtVector(face->dir0).cross(toBtVector(face->dir1)).normalized();
}
btVector3 btConvexHullInternal::getCoordinates(const Vertex* v)
b3Vector3 btConvexHullInternal::getCoordinates(const Vertex* v)
{
btVector3 p;
b3Vector3 p;
p[medAxis] = v->xvalue();
p[maxAxis] = v->yvalue();
p[minAxis] = v->zvalue();
return p * scaling + center;
}
btScalar btConvexHullInternal::shrink(btScalar amount, btScalar clampAmount)
b3Scalar btConvexHullInternal::shrink(b3Scalar amount, b3Scalar clampAmount)
{
if (!vertexList)
{
return 0;
}
int stamp = --mergeStamp;
btAlignedObjectArray<Vertex*> stack;
b3AlignedObjectArray<Vertex*> stack;
vertexList->copy = stamp;
stack.push_back(vertexList);
btAlignedObjectArray<Face*> faces;
b3AlignedObjectArray<Face*> faces;
Point32 ref = vertexList->point;
Int128 hullCenterX(0, 0);
@@ -2155,7 +2155,7 @@ btScalar btConvexHullInternal::shrink(btScalar amount, btScalar clampAmount)
return 0;
}
btVector3 hullCenter;
b3Vector3 hullCenter;
hullCenter[medAxis] = hullCenterX.toScalar();
hullCenter[maxAxis] = hullCenterY.toScalar();
hullCenter[minAxis] = hullCenterZ.toScalar();
@@ -2166,11 +2166,11 @@ btScalar btConvexHullInternal::shrink(btScalar amount, btScalar clampAmount)
if (clampAmount > 0)
{
btScalar minDist = SIMD_INFINITY;
b3Scalar minDist = SIMD_INFINITY;
for (int i = 0; i < faceCount; i++)
{
btVector3 normal = getBtNormal(faces[i]);
btScalar dist = normal.dot(toBtVector(faces[i]->origin) - hullCenter);
b3Vector3 normal = getBtNormal(faces[i]);
b3Scalar dist = normal.dot(toBtVector(faces[i]->origin) - hullCenter);
if (dist < minDist)
{
minDist = dist;
@@ -2202,9 +2202,9 @@ btScalar btConvexHullInternal::shrink(btScalar amount, btScalar clampAmount)
return amount;
}
bool btConvexHullInternal::shiftFace(Face* face, btScalar amount, btAlignedObjectArray<Vertex*> stack)
bool btConvexHullInternal::shiftFace(Face* face, b3Scalar amount, b3AlignedObjectArray<Vertex*> stack)
{
btVector3 origShift = getBtNormal(face) * -amount;
b3Vector3 origShift = getBtNormal(face) * -amount;
if (scaling[0] != 0)
{
origShift[0] /= scaling[0];
@@ -2623,7 +2623,7 @@ bool btConvexHullInternal::shiftFace(Face* face, btScalar amount, btAlignedObjec
}
static int getVertexCopy(btConvexHullInternal::Vertex* vertex, btAlignedObjectArray<btConvexHullInternal::Vertex*>& vertices)
static int getVertexCopy(btConvexHullInternal::Vertex* vertex, b3AlignedObjectArray<btConvexHullInternal::Vertex*>& vertices)
{
int index = vertex->copy;
if (index < 0)
@@ -2638,7 +2638,7 @@ static int getVertexCopy(btConvexHullInternal::Vertex* vertex, btAlignedObjectAr
return index;
}
btScalar btConvexHullComputer::compute(const void* coords, bool doubleCoords, int stride, int count, btScalar shrink, btScalar shrinkClamp)
b3Scalar btConvexHullComputer::compute(const void* coords, bool doubleCoords, int stride, int count, b3Scalar shrink, b3Scalar shrinkClamp)
{
if (count <= 0)
{
@@ -2651,7 +2651,7 @@ btScalar btConvexHullComputer::compute(const void* coords, bool doubleCoords, in
btConvexHullInternal hull;
hull.compute(coords, doubleCoords, stride, count);
btScalar shift = 0;
b3Scalar shift = 0;
if ((shrink > 0) && ((shift = hull.shrink(shrink, shrinkClamp)) < 0))
{
vertices.clear();
@@ -2664,7 +2664,7 @@ btScalar btConvexHullComputer::compute(const void* coords, bool doubleCoords, in
edges.resize(0);
faces.resize(0);
btAlignedObjectArray<btConvexHullInternal::Vertex*> oldVertices;
b3AlignedObjectArray<btConvexHullInternal::Vertex*> oldVertices;
getVertexCopy(hull.vertexList, oldVertices);
int copied = 0;
while (copied < oldVertices.size())

16
src/BulletGeometry/btConvexHullComputer.h
View File

@@ -15,8 +15,8 @@ subject to the following restrictions:
#ifndef BT_CONVEX_HULL_COMPUTER_H
#define BT_CONVEX_HULL_COMPUTER_H
#include "BulletCommon/btVector3.h"
#include "BulletCommon/btAlignedObjectArray.h"
#include "BulletCommon/b3Vector3.h"
#include "BulletCommon/b3AlignedObjectArray.h"
/// Convex hull implementation based on Preparata and Hong
/// See http://code.google.com/p/bullet/issues/detail?id=275
@@ -24,7 +24,7 @@ subject to the following restrictions:
class btConvexHullComputer
{
private:
btScalar compute(const void* coords, bool doubleCoords, int stride, int count, btScalar shrink, btScalar shrinkClamp);
b3Scalar compute(const void* coords, bool doubleCoords, int stride, int count, b3Scalar shrink, b3Scalar shrinkClamp);
public:
@@ -66,13 +66,13 @@ class btConvexHullComputer
// Vertices of the output hull
btAlignedObjectArray<btVector3> vertices;
b3AlignedObjectArray<b3Vector3> vertices;
// Edges of the output hull
btAlignedObjectArray<Edge> edges;
b3AlignedObjectArray<Edge> edges;
// Faces of the convex hull. Each entry is an index into the "edges" array pointing to an edge of the face. Faces are planar n-gons
btAlignedObjectArray<int> faces;
b3AlignedObjectArray<int> faces;
/*
Compute convex hull of "count" vertices stored in "coords". "stride" is the difference in bytes
@@ -86,13 +86,13 @@ class btConvexHullComputer
The output convex hull can be found in the member variables "vertices", "edges", "faces".
*/
btScalar compute(const float* coords, int stride, int count, btScalar shrink, btScalar shrinkClamp)
b3Scalar compute(const float* coords, int stride, int count, b3Scalar shrink, b3Scalar shrinkClamp)
{
return compute(coords, false, stride, count, shrink, shrinkClamp);
}
// same as above, but double precision
btScalar compute(const double* coords, int stride, int count, btScalar shrink, btScalar shrinkClamp)
b3Scalar compute(const double* coords, int stride, int count, b3Scalar shrink, b3Scalar shrinkClamp)
{
return compute(coords, true, stride, count, shrink, shrinkClamp);
}

72
src/BulletGeometry/btGeometryUtil.cpp
View File

@@ -30,14 +30,14 @@ extern "C"
}
bool btGeometryUtil::isPointInsidePlanes(const btAlignedObjectArray<btVector3>& planeEquations, const btVector3& point, btScalar margin)
bool btGeometryUtil::isPointInsidePlanes(const b3AlignedObjectArray<b3Vector3>& planeEquations, const b3Vector3& point, b3Scalar margin)
{
int numbrushes = planeEquations.size();
for (int i=0;i<numbrushes;i++)
{
const btVector3& N1 = planeEquations[i];
btScalar dist = btScalar(N1.dot(point))+btScalar(N1[3])-margin;
if (dist>btScalar(0.))
const b3Vector3& N1 = planeEquations[i];
b3Scalar dist = b3Scalar(N1.dot(point))+b3Scalar(N1[3])-margin;
if (dist>b3Scalar(0.))
{
return false;
}
@@ -47,14 +47,14 @@ bool btGeometryUtil::isPointInsidePlanes(const btAlignedObjectArray<btVector3>&
}
bool btGeometryUtil::areVerticesBehindPlane(const btVector3& planeNormal, const btAlignedObjectArray<btVector3>& vertices, btScalar margin)
bool btGeometryUtil::areVerticesBehindPlane(const b3Vector3& planeNormal, const b3AlignedObjectArray<b3Vector3>& vertices, b3Scalar margin)
{
int numvertices = vertices.size();
for (int i=0;i<numvertices;i++)
{
const btVector3& N1 = vertices[i];
btScalar dist = btScalar(planeNormal.dot(N1))+btScalar(planeNormal[3])-margin;
if (dist>btScalar(0.))
const b3Vector3& N1 = vertices[i];
b3Scalar dist = b3Scalar(planeNormal.dot(N1))+b3Scalar(planeNormal[3])-margin;
if (dist>b3Scalar(0.))
{
return false;
}
@@ -62,15 +62,15 @@ bool btGeometryUtil::areVerticesBehindPlane(const btVector3& planeNormal, const
return true;
}
bool notExist(const btVector3& planeEquation,const btAlignedObjectArray<btVector3>& planeEquations);
bool notExist(const b3Vector3& planeEquation,const b3AlignedObjectArray<b3Vector3>& planeEquations);
bool notExist(const btVector3& planeEquation,const btAlignedObjectArray<btVector3>& planeEquations)
bool notExist(const b3Vector3& planeEquation,const b3AlignedObjectArray<b3Vector3>& planeEquations)
{
int numbrushes = planeEquations.size();
for (int i=0;i<numbrushes;i++)
{
const btVector3& N1 = planeEquations[i];
if (planeEquation.dot(N1) > btScalar(0.999))
const b3Vector3& N1 = planeEquations[i];
if (planeEquation.dot(N1) > b3Scalar(0.999))
{
return false;
}
@@ -78,32 +78,32 @@ bool notExist(const btVector3& planeEquation,const btAlignedObjectArray<btVector
return true;
}
void btGeometryUtil::getPlaneEquationsFromVertices(btAlignedObjectArray<btVector3>& vertices, btAlignedObjectArray<btVector3>& planeEquationsOut )
void btGeometryUtil::getPlaneEquationsFromVertices(b3AlignedObjectArray<b3Vector3>& vertices, b3AlignedObjectArray<b3Vector3>& planeEquationsOut )
{
const int numvertices = vertices.size();
// brute force:
for (int i=0;i<numvertices;i++)
{
const btVector3& N1 = vertices[i];
const b3Vector3& N1 = vertices[i];
for (int j=i+1;j<numvertices;j++)
{
const btVector3& N2 = vertices[j];
const b3Vector3& N2 = vertices[j];
for (int k=j+1;k<numvertices;k++)
{
const btVector3& N3 = vertices[k];
const b3Vector3& N3 = vertices[k];
btVector3 planeEquation,edge0,edge1;
b3Vector3 planeEquation,edge0,edge1;
edge0 = N2-N1;
edge1 = N3-N1;
btScalar normalSign = btScalar(1.);
b3Scalar normalSign = b3Scalar(1.);
for (int ww=0;ww<2;ww++)
{
planeEquation = normalSign * edge0.cross(edge1);
if (planeEquation.length2() > btScalar(0.0001))
if (planeEquation.length2() > b3Scalar(0.0001))
{
planeEquation.normalize();
if (notExist(planeEquation,planeEquationsOut))
@@ -111,13 +111,13 @@ void btGeometryUtil::getPlaneEquationsFromVertices(btAlignedObjectArray<btVector
planeEquation[3] = -planeEquation.dot(N1);
//check if inside, and replace supportingVertexOut if needed
if (areVerticesBehindPlane(planeEquation,vertices,btScalar(0.01)))
if (areVerticesBehindPlane(planeEquation,vertices,b3Scalar(0.01)))
{
planeEquationsOut.push_back(planeEquation);
}
}
}
normalSign = btScalar(-1.);
normalSign = b3Scalar(-1.);
}
}
@@ -126,31 +126,31 @@ void btGeometryUtil::getPlaneEquationsFromVertices(btAlignedObjectArray<btVector
}
void btGeometryUtil::getVerticesFromPlaneEquations(const btAlignedObjectArray<btVector3>& planeEquations , btAlignedObjectArray<btVector3>& verticesOut )
void btGeometryUtil::getVerticesFromPlaneEquations(const b3AlignedObjectArray<b3Vector3>& planeEquations , b3AlignedObjectArray<b3Vector3>& verticesOut )
{
const int numbrushes = planeEquations.size();
// brute force:
for (int i=0;i<numbrushes;i++)
{
const btVector3& N1 = planeEquations[i];
const b3Vector3& N1 = planeEquations[i];
for (int j=i+1;j<numbrushes;j++)
{
const btVector3& N2 = planeEquations[j];
const b3Vector3& N2 = planeEquations[j];
for (int k=j+1;k<numbrushes;k++)
{
const btVector3& N3 = planeEquations[k];
const b3Vector3& N3 = planeEquations[k];
btVector3 n2n3; n2n3 = N2.cross(N3);
btVector3 n3n1; n3n1 = N3.cross(N1);
btVector3 n1n2; n1n2 = N1.cross(N2);
b3Vector3 n2n3; n2n3 = N2.cross(N3);
b3Vector3 n3n1; n3n1 = N3.cross(N1);
b3Vector3 n1n2; n1n2 = N1.cross(N2);
if ( ( n2n3.length2() > btScalar(0.0001) ) &&
( n3n1.length2() > btScalar(0.0001) ) &&
( n1n2.length2() > btScalar(0.0001) ) )
if ( ( n2n3.length2() > b3Scalar(0.0001) ) &&
( n3n1.length2() > b3Scalar(0.0001) ) &&
( n1n2.length2() > b3Scalar(0.0001) ) )
{
//point P out of 3 plane equations:
@@ -159,20 +159,20 @@ void btGeometryUtil::getVerticesFromPlaneEquations(const btAlignedObjectArray<bt
// N1 . ( N2 * N3 )
btScalar quotient = (N1.dot(n2n3));
if (btFabs(quotient) > btScalar(0.000001))
b3Scalar quotient = (N1.dot(n2n3));
if (btFabs(quotient) > b3Scalar(0.000001))
{
quotient = btScalar(-1.) / quotient;
quotient = b3Scalar(-1.) / quotient;
n2n3 *= N1[3];
n3n1 *= N2[3];
n1n2 *= N3[3];
btVector3 potentialVertex = n2n3;
b3Vector3 potentialVertex = n2n3;
potentialVertex += n3n1;
potentialVertex += n1n2;
potentialVertex *= quotient;
//check if inside, and replace supportingVertexOut if needed
if (isPointInsidePlanes(planeEquations,potentialVertex,btScalar(0.01)))
if (isPointInsidePlanes(planeEquations,potentialVertex,b3Scalar(0.01)))
{
verticesOut.push_back(potentialVertex);
}

14
src/BulletGeometry/btGeometryUtil.h
View File

@@ -16,8 +16,8 @@ subject to the following restrictions:
#ifndef BT_GEOMETRY_UTIL_H
#define BT_GEOMETRY_UTIL_H
#include "BulletCommon/btVector3.h"
#include "BulletCommon/btAlignedObjectArray.h"
#include "BulletCommon/b3Vector3.h"
#include "BulletCommon/b3AlignedObjectArray.h"
///The btGeometryUtil helper class provides a few methods to convert between plane equations and vertices.
class btGeometryUtil
@@ -25,15 +25,15 @@ class btGeometryUtil
public:
static void getPlaneEquationsFromVertices(btAlignedObjectArray<btVector3>& vertices, btAlignedObjectArray<btVector3>& planeEquationsOut );
static void getPlaneEquationsFromVertices(b3AlignedObjectArray<b3Vector3>& vertices, b3AlignedObjectArray<b3Vector3>& planeEquationsOut );
static void getVerticesFromPlaneEquations(const btAlignedObjectArray<btVector3>& planeEquations , btAlignedObjectArray<btVector3>& verticesOut );
static void getVerticesFromPlaneEquations(const b3AlignedObjectArray<b3Vector3>& planeEquations , b3AlignedObjectArray<b3Vector3>& verticesOut );
static bool isInside(const btAlignedObjectArray<btVector3>& vertices, const btVector3& planeNormal, btScalar margin);
static bool isInside(const b3AlignedObjectArray<b3Vector3>& vertices, const b3Vector3& planeNormal, b3Scalar margin);
static bool isPointInsidePlanes(const btAlignedObjectArray<btVector3>& planeEquations, const btVector3& point, btScalar margin);
static bool isPointInsidePlanes(const b3AlignedObjectArray<b3Vector3>& planeEquations, const b3Vector3& point, b3Scalar margin);
static bool areVerticesBehindPlane(const btVector3& planeNormal, const btAlignedObjectArray<btVector3>& vertices, btScalar margin);
static bool areVerticesBehindPlane(const b3Vector3& planeNormal, const b3AlignedObjectArray<b3Vector3>& vertices, b3Scalar margin);
};

40
src/BulletGeometry/btGrahamScan2dConvexHull.h
View File

@@ -18,24 +18,24 @@ subject to the following restrictions:
#define GRAHAM_SCAN_2D_CONVEX_HULL_H
#include "BulletCommon/btVector3.h"
#include "BulletCommon/btAlignedObjectArray.h"
#include "BulletCommon/b3Vector3.h"
#include "BulletCommon/b3AlignedObjectArray.h"
struct GrahamVector3 : public btVector3
struct GrahamVector3 : public b3Vector3
{
GrahamVector3(const btVector3& org, int orgIndex)
:btVector3(org),
GrahamVector3(const b3Vector3& org, int orgIndex)
:b3Vector3(org),
m_orgIndex(orgIndex)
{
}
btScalar m_angle;
b3Scalar m_angle;
int m_orgIndex;
};
struct btAngleCompareFunc {
btVector3 m_anchor;
btAngleCompareFunc(const btVector3& anchor)
b3Vector3 m_anchor;
btAngleCompareFunc(const b3Vector3& anchor)
: m_anchor(anchor)
{
}
@@ -44,8 +44,8 @@ struct btAngleCompareFunc {
return a.m_angle < b.m_angle;
else
{
btScalar al = (a-m_anchor).length2();
btScalar bl = (b-m_anchor).length2();
b3Scalar al = (a-m_anchor).length2();
b3Scalar bl = (b-m_anchor).length2();
if (al != bl)
return al < bl;
else
@@ -56,9 +56,9 @@ struct btAngleCompareFunc {
}
};
inline void GrahamScanConvexHull2D(btAlignedObjectArray<GrahamVector3>& originalPoints, btAlignedObjectArray<GrahamVector3>& hull, const btVector3& normalAxis)
inline void GrahamScanConvexHull2D(b3AlignedObjectArray<GrahamVector3>& originalPoints, b3AlignedObjectArray<GrahamVector3>& hull, const b3Vector3& normalAxis)
{
btVector3 axis0,axis1;
b3Vector3 axis0,axis1;
btPlaneSpace1(normalAxis,axis0,axis1);
@@ -71,10 +71,10 @@ inline void GrahamScanConvexHull2D(btAlignedObjectArray<GrahamVector3>& original
//step1 : find anchor point with smallest projection on axis0 and move it to first location
for (int i=0;i<originalPoints.size();i++)
{
// const btVector3& left = originalPoints[i];
// const btVector3& right = originalPoints[0];
btScalar projL = originalPoints[i].dot(axis0);
btScalar projR = originalPoints[0].dot(axis0);
// const b3Vector3& left = originalPoints[i];
// const b3Vector3& right = originalPoints[0];
b3Scalar projL = originalPoints[i].dot(axis0);
b3Scalar projR = originalPoints[0].dot(axis0);
if (projL < projR)
{
originalPoints.swap(0,i);
@@ -85,8 +85,8 @@ inline void GrahamScanConvexHull2D(btAlignedObjectArray<GrahamVector3>& original
originalPoints[0].m_angle = -1e30f;
for (int i=1;i<originalPoints.size();i++)
{
btVector3 xvec = axis0;
btVector3 ar = originalPoints[i]-originalPoints[0];
b3Vector3 xvec = axis0;
b3Vector3 ar = originalPoints[i]-originalPoints[0];
originalPoints[i].m_angle = btCross(xvec, ar).dot(normalAxis) / ar.length();
}
@@ -103,8 +103,8 @@ inline void GrahamScanConvexHull2D(btAlignedObjectArray<GrahamVector3>& original
{
bool isConvex = false;
while (!isConvex&& hull.size()>1) {
btVector3& a = hull[hull.size()-2];
btVector3& b = hull[hull.size()-1];
b3Vector3& a = hull[hull.size()-2];
b3Vector3& b = hull[hull.size()-1];
isConvex = btCross(a-b,a-originalPoints[i]).dot(normalAxis)> 0;
if (!isConvex)
hull.pop_back();

48
src/BulletGeometry/btPolarDecomposition.cpp
View File

@@ -1,45 +1,45 @@
#include "btPolarDecomposition.h"
#include "BulletCommon/btMinMax.h"
#include "BulletCommon/b3MinMax.h"
namespace
{
btScalar abs_column_sum(const btMatrix3x3& a, int i)
b3Scalar abs_column_sum(const b3Matrix3x3& a, int i)
{
return btFabs(a[0][i]) + btFabs(a[1][i]) + btFabs(a[2][i]);
}
btScalar abs_row_sum(const btMatrix3x3& a, int i)
b3Scalar abs_row_sum(const b3Matrix3x3& a, int i)
{
return btFabs(a[i][0]) + btFabs(a[i][1]) + btFabs(a[i][2]);
}
btScalar p1_norm(const btMatrix3x3& a)
b3Scalar p1_norm(const b3Matrix3x3& a)
{
const btScalar sum0 = abs_column_sum(a,0);
const btScalar sum1 = abs_column_sum(a,1);
const btScalar sum2 = abs_column_sum(a,2);
const b3Scalar sum0 = abs_column_sum(a,0);
const b3Scalar sum1 = abs_column_sum(a,1);
const b3Scalar sum2 = abs_column_sum(a,2);
return btMax(btMax(sum0, sum1), sum2);
}
btScalar pinf_norm(const btMatrix3x3& a)
b3Scalar pinf_norm(const b3Matrix3x3& a)
{
const btScalar sum0 = abs_row_sum(a,0);
const btScalar sum1 = abs_row_sum(a,1);
const btScalar sum2 = abs_row_sum(a,2);
const b3Scalar sum0 = abs_row_sum(a,0);
const b3Scalar sum1 = abs_row_sum(a,1);
const b3Scalar sum2 = abs_row_sum(a,2);
return btMax(btMax(sum0, sum1), sum2);
}
}
const btScalar btPolarDecomposition::DEFAULT_TOLERANCE = btScalar(0.0001);
const b3Scalar btPolarDecomposition::DEFAULT_TOLERANCE = b3Scalar(0.0001);
const unsigned int btPolarDecomposition::DEFAULT_MAX_ITERATIONS = 16;
btPolarDecomposition::btPolarDecomposition(btScalar tolerance, unsigned int maxIterations)
btPolarDecomposition::btPolarDecomposition(b3Scalar tolerance, unsigned int maxIterations)
: m_tolerance(tolerance)
, m_maxIterations(maxIterations)
{
}
unsigned int btPolarDecomposition::decompose(const btMatrix3x3& a, btMatrix3x3& u, btMatrix3x3& h) const
unsigned int btPolarDecomposition::decompose(const b3Matrix3x3& a, b3Matrix3x3& u, b3Matrix3x3& h) const
{
// Use the 'u' and 'h' matrices for intermediate calculations
u = a;
@@ -47,23 +47,23 @@ unsigned int btPolarDecomposition::decompose(const btMatrix3x3& a, btMatrix3x3&
for (unsigned int i = 0; i < m_maxIterations; ++i)
{
const btScalar h_1 = p1_norm(h);
const btScalar h_inf = pinf_norm(h);
const btScalar u_1 = p1_norm(u);
const btScalar u_inf = pinf_norm(u);
const b3Scalar h_1 = p1_norm(h);
const b3Scalar h_inf = pinf_norm(h);
const b3Scalar u_1 = p1_norm(u);
const b3Scalar u_inf = pinf_norm(u);
const btScalar h_norm = h_1 * h_inf;
const btScalar u_norm = u_1 * u_inf;
const b3Scalar h_norm = h_1 * h_inf;
const b3Scalar u_norm = u_1 * u_inf;
// The matrix is effectively singular so we cannot invert it
if (btFuzzyZero(h_norm) || btFuzzyZero(u_norm))
break;
const btScalar gamma = btPow(h_norm / u_norm, 0.25f);
const btScalar inv_gamma = 1.0 / gamma;
const b3Scalar gamma = btPow(h_norm / u_norm, 0.25f);
const b3Scalar inv_gamma = 1.0 / gamma;
// Determine the delta to 'u'
const btMatrix3x3 delta = (u * (gamma - 2.0) + h.transpose() * inv_gamma) * 0.5;
const b3Matrix3x3 delta = (u * (gamma - 2.0) + h.transpose() * inv_gamma) * 0.5;
// Update the matrices
u += delta;
@@ -91,7 +91,7 @@ unsigned int btPolarDecomposition::maxIterations() const
return m_maxIterations;
}
unsigned int polarDecompose(const btMatrix3x3& a, btMatrix3x3& u, btMatrix3x3& h)
unsigned int polarDecompose(const b3Matrix3x3& a, b3Matrix3x3& u, b3Matrix3x3& h)
{
static btPolarDecomposition polar;
return polar.decompose(a, u, h);

12
src/BulletGeometry/btPolarDecomposition.h
View File

@@ -1,7 +1,7 @@
#ifndef POLARDECOMPOSITION_H
#define POLARDECOMPOSITION_H
#include "BulletCommon/btMatrix3x3.h"
#include "BulletCommon/b3Matrix3x3.h"
/**
* This class is used to compute the polar decomposition of a matrix. In
@@ -14,7 +14,7 @@
class btPolarDecomposition
{
public:
static const btScalar DEFAULT_TOLERANCE;
static const b3Scalar DEFAULT_TOLERANCE;
static const unsigned int DEFAULT_MAX_ITERATIONS;
/**
@@ -25,7 +25,7 @@ class btPolarDecomposition
* @param maxIterations - the maximum number of iterations used to achieve
* convergence
*/
btPolarDecomposition(btScalar tolerance = DEFAULT_TOLERANCE,
btPolarDecomposition(b3Scalar tolerance = DEFAULT_TOLERANCE,
unsigned int maxIterations = DEFAULT_MAX_ITERATIONS);
/**
@@ -39,7 +39,7 @@ class btPolarDecomposition
*
* @return the number of iterations performed by the algorithm.
*/
unsigned int decompose(const btMatrix3x3& a, btMatrix3x3& u, btMatrix3x3& h) const;
unsigned int decompose(const b3Matrix3x3& a, b3Matrix3x3& u, b3Matrix3x3& h) const;
/**
* Returns the maximum number of iterations that this algorithm will perform
@@ -50,7 +50,7 @@ class btPolarDecomposition
unsigned int maxIterations() const;
private:
btScalar m_tolerance;
b3Scalar m_tolerance;
unsigned int m_maxIterations;
};
@@ -67,7 +67,7 @@ class btPolarDecomposition
*
* @return the number of iterations performed by the algorithm.
*/
unsigned int polarDecompose(const btMatrix3x3& a, btMatrix3x3& u, btMatrix3x3& h);
unsigned int polarDecompose(const b3Matrix3x3& a, b3Matrix3x3& u, b3Matrix3x3& h);
#endif // POLARDECOMPOSITION_H