diff --git a/src/BulletCollision/BroadphaseCollision/btQuantizedBvh.cpp b/src/BulletCollision/BroadphaseCollision/btQuantizedBvh.cpp index 41ff80d15..edd890ebb 100644 --- a/src/BulletCollision/BroadphaseCollision/btQuantizedBvh.cpp +++ b/src/BulletCollision/BroadphaseCollision/btQuantizedBvh.cpp @@ -830,7 +830,7 @@ unsigned int btQuantizedBvh::getAlignmentSerializationPadding() return 0;//BVH_ALIGNMENT_BLOCKS * BVH_ALIGNMENT; } -unsigned btQuantizedBvh::calculateSerializeBufferSize() +unsigned btQuantizedBvh::calculateSerializeBufferSize() const { unsigned baseSize = sizeof(btQuantizedBvh) + getAlignmentSerializationPadding(); baseSize += sizeof(btBvhSubtreeInfo) * m_subtreeHeaderCount; @@ -841,7 +841,7 @@ unsigned btQuantizedBvh::calculateSerializeBufferSize() return baseSize + m_curNodeIndex * sizeof(btOptimizedBvhNode); } -bool btQuantizedBvh::serialize(void *o_alignedDataBuffer, unsigned /*i_dataBufferSize */, bool i_swapEndian) +bool btQuantizedBvh::serialize(void *o_alignedDataBuffer, unsigned /*i_dataBufferSize */, bool i_swapEndian) const { btAssert(m_subtreeHeaderCount == m_SubtreeHeaders.size()); m_subtreeHeaderCount = m_SubtreeHeaders.size(); diff --git a/src/BulletCollision/BroadphaseCollision/btQuantizedBvh.h b/src/BulletCollision/BroadphaseCollision/btQuantizedBvh.h index ced457b60..1ad4614ca 100644 --- a/src/BulletCollision/BroadphaseCollision/btQuantizedBvh.h +++ b/src/BulletCollision/BroadphaseCollision/btQuantizedBvh.h @@ -190,7 +190,7 @@ protected: BvhSubtreeInfoArray m_SubtreeHeaders; //This is only used for serialization so we don't have to add serialization directly to btAlignedObjectArray - int m_subtreeHeaderCount; + mutable int m_subtreeHeaderCount; @@ -445,10 +445,10 @@ public: /////Calculate space needed to store BVH for serialization - unsigned calculateSerializeBufferSize(); + unsigned calculateSerializeBufferSize() const; /// Data buffer MUST be 16 byte aligned - virtual bool serialize(void *o_alignedDataBuffer, unsigned i_dataBufferSize, bool i_swapEndian); + virtual bool serialize(void *o_alignedDataBuffer, unsigned i_dataBufferSize, bool i_swapEndian) const; ///deSerializeInPlace loads and initializes a BVH from a buffer in memory 'in place' static btQuantizedBvh *deSerializeInPlace(void *i_alignedDataBuffer, unsigned int i_dataBufferSize, bool i_swapEndian); diff --git a/src/BulletCollision/CollisionDispatch/btCollisionObject.h b/src/BulletCollision/CollisionDispatch/btCollisionObject.h index c4968c9b7..47ce89113 100644 --- a/src/BulletCollision/CollisionDispatch/btCollisionObject.h +++ b/src/BulletCollision/CollisionDispatch/btCollisionObject.h @@ -27,11 +27,11 @@ subject to the following restrictions: struct btBroadphaseProxy; class btCollisionShape; +struct btCollisionShapeData; #include "LinearMath/btMotionState.h" #include "LinearMath/btAlignedAllocator.h" #include "LinearMath/btAlignedObjectArray.h" - typedef btAlignedObjectArray btCollisionObjectArray; @@ -53,9 +53,9 @@ protected: btVector3 m_interpolationLinearVelocity; btVector3 m_interpolationAngularVelocity; - btVector3 m_anisotropicFriction; - bool m_hasAnisotropicFriction; - btScalar m_contactProcessingThreshold; + btVector3 m_anisotropicFriction; + int m_hasAnisotropicFriction; + btScalar m_contactProcessingThreshold; btBroadphaseProxy* m_broadphaseHandle; btCollisionShape* m_collisionShape; @@ -76,13 +76,13 @@ protected: btScalar m_friction; btScalar m_restitution; - ///users can point to their objects, m_userPointer is not used by Bullet, see setUserPointer/getUserPointer - void* m_userObjectPointer; - ///m_internalType is reserved to distinguish Bullet's btCollisionObject, btRigidBody, btSoftBody, btGhostObject etc. ///do not assign your own m_internalType unless you write a new dynamics object class. int m_internalType; + ///users can point to their objects, m_userPointer is not used by Bullet, see setUserPointer/getUserPointer + void* m_userObjectPointer; + ///time of impact calculation btScalar m_hitFraction; @@ -93,9 +93,7 @@ protected: btScalar m_ccdMotionThreshold; /// If some object should have elaborate collision filtering by sub-classes - bool m_checkCollideWith; - - char m_pad[7]; + int m_checkCollideWith; virtual bool checkCollideWithOverride(btCollisionObject* /* co */) { @@ -143,7 +141,7 @@ public: } bool hasAnisotropicFriction() const { - return m_hasAnisotropicFriction; + return m_hasAnisotropicFriction!=0; } ///the constraint solver can discard solving contacts, if the distance is above this threshold. 0 by default. @@ -416,6 +414,85 @@ public: return true; } + + virtual int calculateSerializeBufferSize() const; + + ///fills the dataBuffer and returns the struct name (and 0 on failure) + virtual const char* serialize(void* dataBuffer) const; + + }; +///using offsetof for m_vtablePadding might break some compilers, in that case define m_vtablePadding manually + +///for serialization +struct btCollisionObjectData +{ + btTransformData m_worldTransform; + btTransformData m_interpolationWorldTransform; + btVector3Data m_interpolationLinearVelocity; + btVector3Data m_interpolationAngularVelocity; + btVector3Data m_anisotropicFriction; + int m_hasAnisotropicFriction; + btScalar m_contactProcessingThreshold; + void *m_broadphaseHandle; + void *m_collisionShape; + btCollisionShapeData *m_rootCollisionShape; + int m_collisionFlags; + int m_islandTag1; + int m_companionId; + int m_activationState1; + btScalar m_deactivationTime; + btScalar m_friction; + btScalar m_restitution; + int m_internalType; + void *m_userObjectPointer; + btScalar m_hitFraction; + btScalar m_ccdSweptSphereRadius; + btScalar m_ccdMotionThreshold; + int m_checkCollideWith; +}; + + +SIMD_FORCE_INLINE int btCollisionObject::calculateSerializeBufferSize() const +{ + return sizeof(btCollisionObjectData); +} + +SIMD_FORCE_INLINE const char* btCollisionObject::serialize(void* dataBuffer) const +{ + + btCollisionObjectData* dataOut = (btCollisionObjectData*)dataBuffer; + + m_worldTransform.serialize(dataOut->m_worldTransform); + m_interpolationWorldTransform.serialize(dataOut->m_interpolationWorldTransform); + m_interpolationLinearVelocity.serialize(dataOut->m_interpolationLinearVelocity); + m_interpolationAngularVelocity.serialize(dataOut->m_interpolationAngularVelocity); + m_anisotropicFriction.serialize(dataOut->m_anisotropicFriction); + dataOut->m_hasAnisotropicFriction = m_hasAnisotropicFriction; + dataOut->m_contactProcessingThreshold = m_contactProcessingThreshold; + dataOut->m_broadphaseHandle = 0; + dataOut->m_collisionShape = m_collisionShape; //@todo + dataOut->m_rootCollisionShape = 0;//@todo + dataOut->m_collisionFlags = m_collisionFlags; + dataOut->m_islandTag1 = m_islandTag1; + dataOut->m_companionId = m_companionId; + dataOut->m_activationState1 = m_activationState1; + dataOut->m_activationState1 = m_activationState1; + dataOut->m_deactivationTime = m_deactivationTime; + dataOut->m_friction = m_friction; + dataOut->m_restitution = m_restitution; + dataOut->m_internalType = m_internalType; + dataOut->m_userObjectPointer = m_userObjectPointer; + dataOut->m_hitFraction = m_hitFraction; + dataOut->m_ccdSweptSphereRadius = m_ccdSweptSphereRadius; + dataOut->m_ccdMotionThreshold = m_ccdMotionThreshold; + dataOut->m_ccdMotionThreshold = m_ccdMotionThreshold; + dataOut->m_checkCollideWith = m_checkCollideWith; + + return "btCollisionObjectData"; +} + + + #endif //COLLISION_OBJECT_H diff --git a/src/BulletCollision/CollisionShapes/btBoxShape.h b/src/BulletCollision/CollisionShapes/btBoxShape.h index c71318dd3..cbc8a6d97 100644 --- a/src/BulletCollision/CollisionShapes/btBoxShape.h +++ b/src/BulletCollision/CollisionShapes/btBoxShape.h @@ -310,8 +310,41 @@ public: } } + virtual int calculateSerializeBufferSize(); + + ///fills the dataBuffer and returns the struct name (and 0 on failure) + virtual const char* serialize(void* dataBuffer) const; + + + + }; + +struct btBoxShapeData +{ + btVector3Data m_halfExtents; + btVector3Data m_localScaling; +}; + + + +SIMD_FORCE_INLINE int btBoxShape::calculateSerializeBufferSize() +{ + return sizeof(btBoxShapeData); +} + +///fills the dataBuffer and returns the struct name (and 0 on failure) +SIMD_FORCE_INLINE const char* btBoxShape::serialize(void* dataBuffer) const +{ + btBoxShapeData* boxData = (btBoxShapeData*) dataBuffer; + + m_implicitShapeDimensions.serialize(boxData->m_halfExtents); + m_localScaling.serialize(boxData->m_localScaling); + return "btBoxShapeData"; +} + + #endif //OBB_BOX_MINKOWSKI_H diff --git a/src/BulletCollision/CollisionShapes/btCollisionShape.h b/src/BulletCollision/CollisionShapes/btCollisionShape.h index f20245113..5738252ae 100644 --- a/src/BulletCollision/CollisionShapes/btCollisionShape.h +++ b/src/BulletCollision/CollisionShapes/btCollisionShape.h @@ -21,6 +21,8 @@ subject to the following restrictions: #include "LinearMath/btMatrix3x3.h" #include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" //for the shape types + + ///The btCollisionShape class provides an interface for collision shapes that can be shared among btCollisionObjects. class btCollisionShape { @@ -113,5 +115,14 @@ public: }; +///for serialization +struct btCollisionShapeData +{ + void *m_userPointer; + int m_shapeType; + char m_padding[4]; +}; + + #endif //COLLISION_SHAPE_H diff --git a/src/BulletCollision/CollisionShapes/btOptimizedBvh.h b/src/BulletCollision/CollisionShapes/btOptimizedBvh.h index 8f3a29f7e..600c01fec 100644 --- a/src/BulletCollision/CollisionShapes/btOptimizedBvh.h +++ b/src/BulletCollision/CollisionShapes/btOptimizedBvh.h @@ -47,7 +47,7 @@ public: void updateBvhNodes(btStridingMeshInterface* meshInterface,int firstNode,int endNode,int index); /// Data buffer MUST be 16 byte aligned - virtual bool serialize(void *o_alignedDataBuffer, unsigned i_dataBufferSize, bool i_swapEndian) + virtual bool serialize(void *o_alignedDataBuffer, unsigned i_dataBufferSize, bool i_swapEndian) const { return btQuantizedBvh::serialize(o_alignedDataBuffer,i_dataBufferSize,i_swapEndian); diff --git a/src/LinearMath/CMakeLists.txt b/src/LinearMath/CMakeLists.txt index 99a561842..cdc2a5e67 100644 --- a/src/LinearMath/CMakeLists.txt +++ b/src/LinearMath/CMakeLists.txt @@ -29,6 +29,7 @@ SET(LinearMath_HDRS btQuickprof.h btRandom.h btScalar.h + btSerializer.h btStackAlloc.h btTransform.h btTransformUtil.h diff --git a/src/LinearMath/btAlignedObjectArray.h b/src/LinearMath/btAlignedObjectArray.h index 7c013d375..160618186 100644 --- a/src/LinearMath/btAlignedObjectArray.h +++ b/src/LinearMath/btAlignedObjectArray.h @@ -138,6 +138,16 @@ class btAlignedObjectArray return m_size; } + SIMD_FORCE_INLINE const T& at(int n) const + { + return m_data[n]; + } + + SIMD_FORCE_INLINE T& at(int n) + { + return m_data[n]; + } + SIMD_FORCE_INLINE const T& operator[](int n) const { return m_data[n]; diff --git a/src/LinearMath/btDefaultMotionState.h b/src/LinearMath/btDefaultMotionState.h index d758f77ed..7858a10d2 100644 --- a/src/LinearMath/btDefaultMotionState.h +++ b/src/LinearMath/btDefaultMotionState.h @@ -1,6 +1,8 @@ #ifndef DEFAULT_MOTION_STATE_H #define DEFAULT_MOTION_STATE_H +#include "btMotionState.h" + ///The btDefaultMotionState provides a common implementation to synchronize world transforms with offsets. struct btDefaultMotionState : public btMotionState { diff --git a/src/LinearMath/btHashMap.h b/src/LinearMath/btHashMap.h index 1fb84d640..8491fd234 100644 --- a/src/LinearMath/btHashMap.h +++ b/src/LinearMath/btHashMap.h @@ -95,14 +95,12 @@ public: }; - -template -class btHashKeyPtr +class btHashInt { int m_uid; public: - btHashKeyPtr(int uid) + btHashInt(int uid) :m_uid(uid) { } @@ -112,11 +110,10 @@ public: return m_uid; } - bool equals(const btHashKeyPtr& other) const + bool equals(const btHashInt& other) const { return getUid1() == other.getUid1(); } - //to our success SIMD_FORCE_INLINE unsigned int getHash()const { @@ -130,6 +127,46 @@ public: key ^= (key >> 16); return key; } +}; + +class btHashKeyPtr +{ + void* m_pointer; +public: + + btHashKeyPtr(void* ptr) + :m_pointer(ptr) + { + } + + const void* getPointer() const + { + return m_pointer; + } + + bool equals(const btHashKeyPtr& other) const + { + return getPointer() == other.getPointer(); + } + + //to our success + SIMD_FORCE_INLINE unsigned int getHash()const + { + const bool VOID_IS_8 = ((sizeof(void*)==8)); + int* intPtr = (int*)&m_pointer; + + int key = VOID_IS_8? intPtr[0]+intPtr[1] : intPtr[0]; + + // Thomas Wang's hash + key += ~(key << 15); + key ^= (key >> 10); + key += (key << 3); + key ^= (key >> 6); + key += ~(key << 11); + key ^= (key >> 16); + + return key; + } }; diff --git a/src/LinearMath/btMatrix3x3.h b/src/LinearMath/btMatrix3x3.h index ea26bbbba..c4b07c589 100644 --- a/src/LinearMath/btMatrix3x3.h +++ b/src/LinearMath/btMatrix3x3.h @@ -13,10 +13,8 @@ subject to the following restrictions: */ -#ifndef btMatrix3x3_H -#define btMatrix3x3_H - -#include "btScalar.h" +#ifndef BT_MATRIX3x3_H +#define BT_MATRIX3x3_H #include "btVector3.h" #include "btQuaternion.h" @@ -24,151 +22,155 @@ subject to the following restrictions: /**@brief The btMatrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with btQuaternion, btTransform and btVector3. - * Make sure to only include a pure orthogonal matrix without scaling. */ +* Make sure to only include a pure orthogonal matrix without scaling. */ class btMatrix3x3 { - public: - /** @brief No initializaion constructor */ - btMatrix3x3 () {} - -// explicit btMatrix3x3(const btScalar *m) { setFromOpenGLSubMatrix(m); } - - /**@brief Constructor from Quaternion */ - explicit btMatrix3x3(const btQuaternion& q) { setRotation(q); } - /* - template - Matrix3x3(const btScalar& yaw, const btScalar& pitch, const btScalar& 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) - { - setValue(xx, xy, xz, - yx, yy, yz, - zx, zy, zz); - } - /** @brief Copy constructor */ - SIMD_FORCE_INLINE btMatrix3x3 (const btMatrix3x3& other) - { - m_el[0] = other.m_el[0]; - m_el[1] = other.m_el[1]; - m_el[2] = other.m_el[2]; - } - /** @brief Assignment Operator */ - SIMD_FORCE_INLINE btMatrix3x3& operator=(const btMatrix3x3& other) - { - m_el[0] = other.m_el[0]; - m_el[1] = other.m_el[1]; - m_el[2] = other.m_el[2]; - return *this; - } - /** @brief Get a column of the matrix as a vector - * @param i Column number 0 indexed */ - SIMD_FORCE_INLINE btVector3 getColumn(int i) const - { - return btVector3(m_el[0][i],m_el[1][i],m_el[2][i]); - } - + ///Data storage for the matrix, each vector is a row of the matrix + btVector3 m_el[3]; - /** @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 - { - btFullAssert(0 <= i && i < 3); - return m_el[i]; - } +public: + /** @brief No initializaion constructor */ + btMatrix3x3 () {} - /** @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) - { - btFullAssert(0 <= i && i < 3); - return m_el[i]; - } - - /** @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 - { - btFullAssert(0 <= i && i < 3); - return m_el[i]; - } - - /** @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); - - /** @brief Set from a carray of btScalars - * @param m A pointer to the beginning of an array of 9 btScalars */ + // explicit btMatrix3x3(const btScalar *m) { setFromOpenGLSubMatrix(m); } + + /**@brief Constructor from Quaternion */ + explicit btMatrix3x3(const btQuaternion& q) { setRotation(q); } + /* + template + Matrix3x3(const btScalar& yaw, const btScalar& pitch, const btScalar& 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) + { + setValue(xx, xy, xz, + yx, yy, yz, + zx, zy, zz); + } + /** @brief Copy constructor */ + SIMD_FORCE_INLINE btMatrix3x3 (const btMatrix3x3& other) + { + m_el[0] = other.m_el[0]; + m_el[1] = other.m_el[1]; + m_el[2] = other.m_el[2]; + } + /** @brief Assignment Operator */ + SIMD_FORCE_INLINE btMatrix3x3& operator=(const btMatrix3x3& other) + { + m_el[0] = other.m_el[0]; + m_el[1] = other.m_el[1]; + m_el[2] = other.m_el[2]; + return *this; + } + + /** @brief Get a column of the matrix as a vector + * @param i Column number 0 indexed */ + SIMD_FORCE_INLINE btVector3 getColumn(int i) const + { + return btVector3(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 + { + btFullAssert(0 <= i && i < 3); + return m_el[i]; + } + + /** @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) + { + btFullAssert(0 <= i && i < 3); + return m_el[i]; + } + + /** @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 + { + btFullAssert(0 <= i && i < 3); + return m_el[i]; + } + + /** @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); + + /** @brief Set from a carray of btScalars + * @param m A pointer to the beginning of an array of 9 btScalars */ void setFromOpenGLSubMatrix(const btScalar *m) - { - m_el[0].setValue(m[0],m[4],m[8]); - m_el[1].setValue(m[1],m[5],m[9]); - m_el[2].setValue(m[2],m[6],m[10]); + { + m_el[0].setValue(m[0],m[4],m[8]); + m_el[1].setValue(m[1],m[5],m[9]); + m_el[2].setValue(m[2],m[6],m[10]); - } - /** @brief Set the values of the matrix explicitly (row major) - * @param xx Top left - * @param xy Top Middle - * @param xz Top Right - * @param yx Middle Left - * @param yy Middle Middle - * @param yz Middle Right - * @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) - { - m_el[0].setValue(xx,xy,xz); - m_el[1].setValue(yx,yy,yz); - m_el[2].setValue(zx,zy,zz); - } + } + /** @brief Set the values of the matrix explicitly (row major) + * @param xx Top left + * @param xy Top Middle + * @param xz Top Right + * @param yx Middle Left + * @param yy Middle Middle + * @param yz Middle Right + * @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) + { + m_el[0].setValue(xx,xy,xz); + m_el[1].setValue(yx,yy,yz); + m_el[2].setValue(zx,zy,zz); + } - /** @brief Set the matrix from a quaternion - * @param q The Quaternion to match */ - void setRotation(const btQuaternion& q) - { - btScalar d = q.length2(); - btFullAssert(d != btScalar(0.0)); - btScalar s = btScalar(2.0) / d; - btScalar xs = q.x() * s, ys = q.y() * s, zs = q.z() * s; - btScalar wx = q.w() * xs, wy = q.w() * ys, wz = q.w() * zs; - btScalar xx = q.x() * xs, xy = q.x() * ys, xz = q.x() * zs; - btScalar yy = q.y() * ys, yz = q.y() * zs, zz = q.z() * 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)); - } - + /** @brief Set the matrix from a quaternion + * @param q The Quaternion to match */ + void setRotation(const btQuaternion& q) + { + btScalar d = q.length2(); + btFullAssert(d != btScalar(0.0)); + btScalar s = btScalar(2.0) / d; + btScalar xs = q.x() * s, ys = q.y() * s, zs = q.z() * s; + btScalar wx = q.w() * xs, wy = q.w() * ys, wz = q.w() * zs; + btScalar xx = q.x() * xs, xy = q.x() * ys, xz = q.x() * zs; + btScalar yy = q.y() * ys, yz = q.y() * zs, zz = q.z() * 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)); + } - /** @brief Set the matrix from euler angles using YPR around YXZ respectively - * @param yaw Yaw about Y axis - * @param pitch Pitch about X axis - * @param roll Roll about Z axis - */ - void setEulerYPR(const btScalar& yaw, const btScalar& pitch, const btScalar& roll) - { - setEulerZYX(roll, pitch, yaw); - } + + /** @brief Set the matrix from euler angles using YPR around YXZ respectively + * @param yaw Yaw about Y axis + * @param pitch Pitch about X axis + * @param roll Roll about Z axis + */ + void setEulerYPR(const btScalar& yaw, const btScalar& pitch, const btScalar& roll) + { + setEulerZYX(roll, pitch, yaw); + } /** @brief Set the matrix from euler angles YPR around ZYX axes - * @param eulerX Roll about X axis - * @param eulerY Pitch around Y axis - * @param eulerZ Yaw aboud Z axis - * - * These angles are used to produce a rotation matrix. The euler - * angles are applied in ZYX order. I.e a vector is first rotated - * about X then Y and then Z - **/ + * @param eulerX Roll about X axis + * @param eulerY Pitch around Y axis + * @param eulerZ Yaw aboud Z axis + * + * These angles are used to produce a rotation matrix. The euler + * 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) { - ///@todo proposed to reverse this since it's labeled zyx but takes arguments xyz and it will match all other parts of the code + ///@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)); @@ -179,227 +181,233 @@ class btMatrix3x3 { btScalar cs = ci * sh; btScalar sc = si * ch; btScalar ss = si * sh; - + setValue(cj * ch, sj * sc - cs, sj * cc + ss, - cj * sh, sj * ss + cc, sj * cs - sc, - -sj, cj * si, cj * ci); + cj * sh, sj * ss + cc, sj * cs - sc, + -sj, cj * si, cj * ci); } - /**@brief Set the matrix to the identity */ - void setIdentity() - { - 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)); - } + /**@brief Set the matrix to the identity */ + void setIdentity() + { + 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)); + } - static const btMatrix3x3& getIdentity() + static const btMatrix3x3& getIdentity() + { + static const btMatrix3x3 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)); + return identityMatrix; + } + + /**@brief Fill the values of the matrix into a 9 element array + * @param m The array to be filled */ + void getOpenGLSubMatrix(btScalar *m) const + { + m[0] = btScalar(m_el[0].x()); + m[1] = btScalar(m_el[1].x()); + m[2] = btScalar(m_el[2].x()); + m[3] = btScalar(0.0); + m[4] = btScalar(m_el[0].y()); + m[5] = btScalar(m_el[1].y()); + m[6] = btScalar(m_el[2].y()); + m[7] = btScalar(0.0); + m[8] = btScalar(m_el[0].z()); + m[9] = btScalar(m_el[1].z()); + m[10] = btScalar(m_el[2].z()); + m[11] = btScalar(0.0); + } + + /**@brief Get the matrix represented as a quaternion + * @param q The quaternion which will be set */ + void getRotation(btQuaternion& q) const + { + btScalar trace = m_el[0].x() + m_el[1].y() + m_el[2].z(); + btScalar temp[4]; + + if (trace > btScalar(0.0)) { - static const btMatrix3x3 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)); - return identityMatrix; - } + btScalar s = btSqrt(trace + btScalar(1.0)); + temp[3]=(s * btScalar(0.5)); + s = btScalar(0.5) / s; - /**@brief Fill the values of the matrix into a 9 element array - * @param m The array to be filled */ - void getOpenGLSubMatrix(btScalar *m) const + temp[0]=((m_el[2].y() - m_el[1].z()) * s); + temp[1]=((m_el[0].z() - m_el[2].x()) * s); + temp[2]=((m_el[1].x() - m_el[0].y()) * s); + } + else { - m[0] = btScalar(m_el[0].x()); - m[1] = btScalar(m_el[1].x()); - m[2] = btScalar(m_el[2].x()); - m[3] = btScalar(0.0); - m[4] = btScalar(m_el[0].y()); - m[5] = btScalar(m_el[1].y()); - m[6] = btScalar(m_el[2].y()); - m[7] = btScalar(0.0); - m[8] = btScalar(m_el[0].z()); - m[9] = btScalar(m_el[1].z()); - m[10] = btScalar(m_el[2].z()); - m[11] = btScalar(0.0); + int i = m_el[0].x() < m_el[1].y() ? + (m_el[1].y() < m_el[2].z() ? 2 : 1) : + (m_el[0].x() < m_el[2].z() ? 2 : 0); + 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; + + temp[3] = (m_el[k][j] - m_el[j][k]) * s; + temp[j] = (m_el[j][i] + m_el[i][j]) * s; + temp[k] = (m_el[k][i] + m_el[i][k]) * s; } + q.setValue(temp[0],temp[1],temp[2],temp[3]); + } - /**@brief Get the matrix represented as a quaternion - * @param q The quaternion which will be set */ - void getRotation(btQuaternion& q) const + /**@brief Get the matrix represented as euler angles around YXZ, roundtrip with setEulerYPR + * @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 + { + + // first use the normal calculus + yaw = btScalar(btAtan2(m_el[1].x(), m_el[0].x())); + pitch = btScalar(btAsin(-m_el[2].x())); + roll = btScalar(btAtan2(m_el[2].y(), m_el[2].z())); + + // on pitch = +/-HalfPI + if (btFabs(pitch)==SIMD_HALF_PI) { - btScalar trace = m_el[0].x() + m_el[1].y() + m_el[2].z(); - btScalar temp[4]; - - if (trace > btScalar(0.0)) - { - btScalar s = btSqrt(trace + btScalar(1.0)); - temp[3]=(s * btScalar(0.5)); - s = btScalar(0.5) / s; - - temp[0]=((m_el[2].y() - m_el[1].z()) * s); - temp[1]=((m_el[0].z() - m_el[2].x()) * s); - temp[2]=((m_el[1].x() - m_el[0].y()) * s); - } - else - { - int i = m_el[0].x() < m_el[1].y() ? - (m_el[1].y() < m_el[2].z() ? 2 : 1) : - (m_el[0].x() < m_el[2].z() ? 2 : 0); - 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; - - temp[3] = (m_el[k][j] - m_el[j][k]) * s; - temp[j] = (m_el[j][i] + m_el[i][j]) * s; - temp[k] = (m_el[k][i] + m_el[i][k]) * s; - } - q.setValue(temp[0],temp[1],temp[2],temp[3]); + if (yaw>0) + yaw-=SIMD_PI; + else + yaw+=SIMD_PI; + + if (roll>0) + roll-=SIMD_PI; + else + roll+=SIMD_PI; } + }; - /**@brief Get the matrix represented as euler angles around YXZ, roundtrip with setEulerYPR - * @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 + + /**@brief Get the matrix represented as euler angles around ZYX + * @param yaw Yaw around X axis + * @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 + { + struct Euler { - - // first use the normal calculus - yaw = btScalar(btAtan2(m_el[1].x(), m_el[0].x())); - pitch = btScalar(btAsin(-m_el[2].x())); - roll = btScalar(btAtan2(m_el[2].y(), m_el[2].z())); - - // on pitch = +/-HalfPI - if (btFabs(pitch)==SIMD_HALF_PI) - { - if (yaw>0) - yaw-=SIMD_PI; - else - yaw+=SIMD_PI; - - if (roll>0) - roll-=SIMD_PI; - else - roll+=SIMD_PI; - } + btScalar yaw; + btScalar pitch; + btScalar roll; }; + Euler euler_out; + Euler euler_out2; //second solution + //get the pointer to the raw data - /**@brief Get the matrix represented as euler angles around ZYX - * @param yaw Yaw around X axis - * @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 - { - struct Euler{btScalar yaw, pitch, roll;}; - Euler euler_out; - Euler euler_out2; //second solution - //get the pointer to the raw data - - // Check that pitch is not at a singularity - if (btFabs(m_el[2].x()) >= 1) - { - euler_out.yaw = 0; - euler_out2.yaw = 0; - - // From difference of angles formula - btScalar delta = btAtan2(m_el[0].x(),m_el[0].z()); - if (m_el[2].x() > 0) //gimbal locked up - { - euler_out.pitch = SIMD_PI / btScalar(2.0); - euler_out2.pitch = SIMD_PI / btScalar(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.roll = -euler_out.pitch + delta; - euler_out2.roll = -euler_out.pitch + delta; - } - } - else - { - euler_out.pitch = - btAsin(m_el[2].x()); - euler_out2.pitch = SIMD_PI - euler_out.pitch; - - euler_out.roll = btAtan2(m_el[2].y()/btCos(euler_out.pitch), - m_el[2].z()/btCos(euler_out.pitch)); - euler_out2.roll = btAtan2(m_el[2].y()/btCos(euler_out2.pitch), + // Check that pitch is not at a singularity + if (btFabs(m_el[2].x()) >= 1) + { + euler_out.yaw = 0; + euler_out2.yaw = 0; + + // From difference of angles formula + btScalar delta = btAtan2(m_el[0].x(),m_el[0].z()); + if (m_el[2].x() > 0) //gimbal locked up + { + euler_out.pitch = SIMD_PI / btScalar(2.0); + euler_out2.pitch = SIMD_PI / btScalar(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.roll = -euler_out.pitch + delta; + euler_out2.roll = -euler_out.pitch + delta; + } + } + else + { + euler_out.pitch = - btAsin(m_el[2].x()); + euler_out2.pitch = SIMD_PI - euler_out.pitch; + + euler_out.roll = btAtan2(m_el[2].y()/btCos(euler_out.pitch), + m_el[2].z()/btCos(euler_out.pitch)); + euler_out2.roll = btAtan2(m_el[2].y()/btCos(euler_out2.pitch), m_el[2].z()/btCos(euler_out2.pitch)); - - euler_out.yaw = btAtan2(m_el[1].x()/btCos(euler_out.pitch), - m_el[0].x()/btCos(euler_out.pitch)); - euler_out2.yaw = btAtan2(m_el[1].x()/btCos(euler_out2.pitch), - m_el[0].x()/btCos(euler_out2.pitch)); - } - - if (solution_number == 1) - { - yaw = euler_out.yaw; - pitch = euler_out.pitch; - roll = euler_out.roll; - } - else - { - yaw = euler_out2.yaw; - pitch = euler_out2.pitch; - roll = euler_out2.roll; - } - } - /**@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 - { - return btMatrix3x3(m_el[0].x() * s.x(), m_el[0].y() * s.y(), m_el[0].z() * s.z(), - m_el[1].x() * s.x(), m_el[1].y() * s.y(), m_el[1].z() * s.z(), - m_el[2].x() * s.x(), m_el[2].y() * s.y(), m_el[2].z() * s.z()); + euler_out.yaw = btAtan2(m_el[1].x()/btCos(euler_out.pitch), + m_el[0].x()/btCos(euler_out.pitch)); + euler_out2.yaw = btAtan2(m_el[1].x()/btCos(euler_out2.pitch), + m_el[0].x()/btCos(euler_out2.pitch)); } - /**@brief Return the determinant of the matrix */ - btScalar determinant() const; - /**@brief Return the adjoint of the matrix */ - btMatrix3x3 adjoint() const; - /**@brief Return the matrix with all values non negative */ - btMatrix3x3 absolute() const; - /**@brief Return the transpose of the matrix */ - btMatrix3x3 transpose() const; - /**@brief Return the inverse of the matrix */ - btMatrix3x3 inverse() const; - - btMatrix3x3 transposeTimes(const btMatrix3x3& m) const; - btMatrix3x3 timesTranspose(const btMatrix3x3& m) const; - - SIMD_FORCE_INLINE btScalar tdotx(const btVector3& v) const - { - return m_el[0].x() * v.x() + m_el[1].x() * v.y() + m_el[2].x() * v.z(); + if (solution_number == 1) + { + yaw = euler_out.yaw; + pitch = euler_out.pitch; + roll = euler_out.roll; } - SIMD_FORCE_INLINE btScalar tdoty(const btVector3& v) const - { - return m_el[0].y() * v.x() + m_el[1].y() * v.y() + m_el[2].y() * v.z(); + else + { + yaw = euler_out2.yaw; + pitch = euler_out2.pitch; + roll = euler_out2.roll; } - SIMD_FORCE_INLINE btScalar tdotz(const btVector3& v) const - { - return m_el[0].z() * v.x() + m_el[1].z() * v.y() + m_el[2].z() * v.z(); - } - + } - /**@brief diagonalizes this matrix by the Jacobi method. - * @param rot stores the rotation from the coordinate system in which the matrix is diagonal to the original - * coordinate system, i.e., old_this = rot * new_this * rot^T. - * @param threshold See iteration - * @param iteration The iteration stops when all off-diagonal elements are less than the threshold multiplied - * by the sum of the absolute values of the diagonal, or when maxSteps have been executed. - * - * Note that this matrix is assumed to be symmetric. - */ - void diagonalize(btMatrix3x3& rot, btScalar threshold, int maxSteps) + /**@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 + { + return btMatrix3x3(m_el[0].x() * s.x(), m_el[0].y() * s.y(), m_el[0].z() * s.z(), + m_el[1].x() * s.x(), m_el[1].y() * s.y(), m_el[1].z() * s.z(), + m_el[2].x() * s.x(), m_el[2].y() * s.y(), m_el[2].z() * s.z()); + } + + /**@brief Return the determinant of the matrix */ + btScalar determinant() const; + /**@brief Return the adjoint of the matrix */ + btMatrix3x3 adjoint() const; + /**@brief Return the matrix with all values non negative */ + btMatrix3x3 absolute() const; + /**@brief Return the transpose of the matrix */ + btMatrix3x3 transpose() const; + /**@brief Return the inverse of the matrix */ + btMatrix3x3 inverse() const; + + btMatrix3x3 transposeTimes(const btMatrix3x3& m) const; + btMatrix3x3 timesTranspose(const btMatrix3x3& m) const; + + SIMD_FORCE_INLINE btScalar tdotx(const btVector3& v) const + { + return m_el[0].x() * v.x() + m_el[1].x() * v.y() + m_el[2].x() * v.z(); + } + SIMD_FORCE_INLINE btScalar tdoty(const btVector3& v) const + { + return m_el[0].y() * v.x() + m_el[1].y() * v.y() + m_el[2].y() * v.z(); + } + SIMD_FORCE_INLINE btScalar tdotz(const btVector3& v) const + { + return m_el[0].z() * v.x() + m_el[1].z() * v.y() + m_el[2].z() * v.z(); + } + + + /**@brief diagonalizes this matrix by the Jacobi method. + * @param rot stores the rotation from the coordinate system in which the matrix is diagonal to the original + * coordinate system, i.e., old_this = rot * new_this * rot^T. + * @param threshold See iteration + * @param iteration The iteration stops when all off-diagonal elements are less than the threshold multiplied + * by the sum of the absolute values of the diagonal, or when maxSteps have been executed. + * + * Note that this matrix is assumed to be symmetric. + */ + void diagonalize(btMatrix3x3& rot, btScalar threshold, int maxSteps) + { + rot.setIdentity(); + for (int step = maxSteps; step > 0; step--) { - rot.setIdentity(); - for (int step = maxSteps; step > 0; step--) - { // find off-diagonal element [p][q] with largest magnitude int p = 0; int q = 1; @@ -408,27 +416,27 @@ class btMatrix3x3 { btScalar v = btFabs(m_el[0][2]); if (v > max) { - q = 2; - r = 1; - max = v; + q = 2; + r = 1; + max = v; } v = btFabs(m_el[1][2]); if (v > max) { - p = 1; - q = 2; - r = 0; - max = v; + p = 1; + q = 2; + r = 0; + max = v; } btScalar 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) - { - return; - } - step = 1; + if (max <= SIMD_EPSILON * t) + { + return; + } + step = 1; } // compute Jacobi rotation J which leads to a zero for element [p][q] @@ -439,17 +447,17 @@ class btMatrix3x3 { btScalar sin; if (theta2 * theta2 < btScalar(10 / SIMD_EPSILON)) { - t = (theta >= 0) ? 1 / (theta + btSqrt(1 + theta2)) - : 1 / (theta - btSqrt(1 + theta2)); - cos = 1 / btSqrt(1 + t * t); - sin = cos * t; + t = (theta >= 0) ? 1 / (theta + btSqrt(1 + theta2)) + : 1 / (theta - btSqrt(1 + theta2)); + cos = 1 / btSqrt(1 + t * t); + sin = cos * t; } 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; - sin = cos * t; + // 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; + sin = cos * t; } // apply rotation to matrix (this = J^T * this * J) @@ -464,155 +472,179 @@ class btMatrix3x3 { // apply rotation to rot (rot = rot * J) for (int i = 0; i < 3; i++) { - btVector3& row = rot[i]; - mrp = row[p]; - mrq = row[q]; - row[p] = cos * mrp - sin * mrq; - row[q] = cos * mrq + sin * mrp; + btVector3& row = rot[i]; + mrp = row[p]; + mrq = row[q]; + row[p] = cos * mrp - sin * mrq; + row[q] = cos * mrq + sin * mrp; } - } } - - - - protected: - /**@brief Calculate the matrix cofactor - * @param r1 The first row to use for calculating the cofactor - * @param c1 The first column to use for calculating the cofactor - * @param r1 The second row to use for calculating the cofactor - * @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 - { - return m_el[r1][c1] * m_el[r2][c2] - m_el[r1][c2] * m_el[r2][c1]; - } - ///Data storage for the matrix, each vector is a row of the matrix - btVector3 m_el[3]; - }; - - SIMD_FORCE_INLINE btMatrix3x3& - btMatrix3x3::operator*=(const btMatrix3x3& m) - { - setValue(m.tdotx(m_el[0]), m.tdoty(m_el[0]), m.tdotz(m_el[0]), - m.tdotx(m_el[1]), m.tdoty(m_el[1]), m.tdotz(m_el[1]), - m.tdotx(m_el[2]), m.tdoty(m_el[2]), m.tdotz(m_el[2])); - return *this; - } - - SIMD_FORCE_INLINE btScalar - btMatrix3x3::determinant() const - { - return btTriple((*this)[0], (*this)[1], (*this)[2]); - } - - - SIMD_FORCE_INLINE btMatrix3x3 - btMatrix3x3::absolute() const - { - return btMatrix3x3( - btFabs(m_el[0].x()), btFabs(m_el[0].y()), btFabs(m_el[0].z()), - btFabs(m_el[1].x()), btFabs(m_el[1].y()), btFabs(m_el[1].z()), - btFabs(m_el[2].x()), btFabs(m_el[2].y()), btFabs(m_el[2].z())); } - SIMD_FORCE_INLINE btMatrix3x3 - btMatrix3x3::transpose() const + + + + /**@brief Calculate the matrix cofactor + * @param r1 The first row to use for calculating the cofactor + * @param c1 The first column to use for calculating the cofactor + * @param r1 The second row to use for calculating the cofactor + * @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 { - return btMatrix3x3(m_el[0].x(), m_el[1].x(), m_el[2].x(), - m_el[0].y(), m_el[1].y(), m_el[2].y(), - m_el[0].z(), m_el[1].z(), m_el[2].z()); - } - - SIMD_FORCE_INLINE btMatrix3x3 - btMatrix3x3::adjoint() const - { - return btMatrix3x3(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 - { - 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.x() * s, cofac(0, 2, 2, 1) * s, cofac(0, 1, 1, 2) * s, - co.y() * s, cofac(0, 0, 2, 2) * s, cofac(0, 2, 1, 0) * s, - co.z() * s, cofac(0, 1, 2, 0) * s, cofac(0, 0, 1, 1) * s); - } - - SIMD_FORCE_INLINE btMatrix3x3 - btMatrix3x3::transposeTimes(const btMatrix3x3& m) const - { - return btMatrix3x3( - m_el[0].x() * m[0].x() + m_el[1].x() * m[1].x() + m_el[2].x() * m[2].x(), - m_el[0].x() * m[0].y() + m_el[1].x() * m[1].y() + m_el[2].x() * m[2].y(), - m_el[0].x() * m[0].z() + m_el[1].x() * m[1].z() + m_el[2].x() * m[2].z(), - m_el[0].y() * m[0].x() + m_el[1].y() * m[1].x() + m_el[2].y() * m[2].x(), - m_el[0].y() * m[0].y() + m_el[1].y() * m[1].y() + m_el[2].y() * m[2].y(), - m_el[0].y() * m[0].z() + m_el[1].y() * m[1].z() + m_el[2].y() * m[2].z(), - m_el[0].z() * m[0].x() + m_el[1].z() * m[1].x() + m_el[2].z() * m[2].x(), - m_el[0].z() * m[0].y() + m_el[1].z() * m[1].y() + m_el[2].z() * m[2].y(), - m_el[0].z() * m[0].z() + m_el[1].z() * m[1].z() + m_el[2].z() * m[2].z()); - } - - SIMD_FORCE_INLINE btMatrix3x3 - btMatrix3x3::timesTranspose(const btMatrix3x3& m) const - { - return btMatrix3x3( - 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])); - + return m_el[r1][c1] * m_el[r2][c2] - m_el[r1][c2] * m_el[r2][c1]; } - SIMD_FORCE_INLINE btVector3 - operator*(const btMatrix3x3& m, const btVector3& v) - { - return btVector3(m[0].dot(v), m[1].dot(v), m[2].dot(v)); - } - + virtual void serialize(struct btMatrix3x3Data& dataOut) const; - SIMD_FORCE_INLINE btVector3 - operator*(const btVector3& v, const btMatrix3x3& m) - { - return btVector3(m.tdotx(v), m.tdoty(v), m.tdotz(v)); - } + virtual void deSerialize(const struct btMatrix3x3Data& dataIn); - SIMD_FORCE_INLINE btMatrix3x3 - operator*(const btMatrix3x3& m1, const btMatrix3x3& m2) - { - return btMatrix3x3( - 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])); - } +}; + + +SIMD_FORCE_INLINE btMatrix3x3& +btMatrix3x3::operator*=(const btMatrix3x3& m) +{ + setValue(m.tdotx(m_el[0]), m.tdoty(m_el[0]), m.tdotz(m_el[0]), + m.tdotx(m_el[1]), m.tdoty(m_el[1]), m.tdotz(m_el[1]), + m.tdotx(m_el[2]), m.tdoty(m_el[2]), m.tdotz(m_el[2])); + return *this; +} + +SIMD_FORCE_INLINE btScalar +btMatrix3x3::determinant() const +{ + return btTriple((*this)[0], (*this)[1], (*this)[2]); +} + + +SIMD_FORCE_INLINE btMatrix3x3 +btMatrix3x3::absolute() const +{ + return btMatrix3x3( + btFabs(m_el[0].x()), btFabs(m_el[0].y()), btFabs(m_el[0].z()), + btFabs(m_el[1].x()), btFabs(m_el[1].y()), btFabs(m_el[1].z()), + btFabs(m_el[2].x()), btFabs(m_el[2].y()), btFabs(m_el[2].z())); +} + +SIMD_FORCE_INLINE btMatrix3x3 +btMatrix3x3::transpose() const +{ + return btMatrix3x3(m_el[0].x(), m_el[1].x(), m_el[2].x(), + m_el[0].y(), m_el[1].y(), m_el[2].y(), + m_el[0].z(), m_el[1].z(), m_el[2].z()); +} + +SIMD_FORCE_INLINE btMatrix3x3 +btMatrix3x3::adjoint() const +{ + return btMatrix3x3(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 +{ + 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.x() * s, cofac(0, 2, 2, 1) * s, cofac(0, 1, 1, 2) * s, + co.y() * s, cofac(0, 0, 2, 2) * s, cofac(0, 2, 1, 0) * s, + co.z() * s, cofac(0, 1, 2, 0) * s, cofac(0, 0, 1, 1) * s); +} + +SIMD_FORCE_INLINE btMatrix3x3 +btMatrix3x3::transposeTimes(const btMatrix3x3& m) const +{ + return btMatrix3x3( + m_el[0].x() * m[0].x() + m_el[1].x() * m[1].x() + m_el[2].x() * m[2].x(), + m_el[0].x() * m[0].y() + m_el[1].x() * m[1].y() + m_el[2].x() * m[2].y(), + m_el[0].x() * m[0].z() + m_el[1].x() * m[1].z() + m_el[2].x() * m[2].z(), + m_el[0].y() * m[0].x() + m_el[1].y() * m[1].x() + m_el[2].y() * m[2].x(), + m_el[0].y() * m[0].y() + m_el[1].y() * m[1].y() + m_el[2].y() * m[2].y(), + m_el[0].y() * m[0].z() + m_el[1].y() * m[1].z() + m_el[2].y() * m[2].z(), + m_el[0].z() * m[0].x() + m_el[1].z() * m[1].x() + m_el[2].z() * m[2].x(), + m_el[0].z() * m[0].y() + m_el[1].z() * m[1].y() + m_el[2].z() * m[2].y(), + m_el[0].z() * m[0].z() + m_el[1].z() * m[1].z() + m_el[2].z() * m[2].z()); +} + +SIMD_FORCE_INLINE btMatrix3x3 +btMatrix3x3::timesTranspose(const btMatrix3x3& m) const +{ + return btMatrix3x3( + 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])); + +} + +SIMD_FORCE_INLINE btVector3 +operator*(const btMatrix3x3& m, const btVector3& v) +{ + return btVector3(m[0].dot(v), m[1].dot(v), m[2].dot(v)); +} + + +SIMD_FORCE_INLINE btVector3 +operator*(const btVector3& v, const btMatrix3x3& m) +{ + return btVector3(m.tdotx(v), m.tdoty(v), m.tdotz(v)); +} + +SIMD_FORCE_INLINE btMatrix3x3 +operator*(const btMatrix3x3& m1, const btMatrix3x3& m2) +{ + return btMatrix3x3( + 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])); +} /* - SIMD_FORCE_INLINE btMatrix3x3 btMultTransposeLeft(const btMatrix3x3& m1, const btMatrix3x3& m2) { - return btMatrix3x3( - 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], - m1[0][1] * m2[0][0] + m1[1][1] * m2[1][0] + m1[2][1] * m2[2][0], - m1[0][1] * m2[0][1] + m1[1][1] * m2[1][1] + m1[2][1] * m2[2][1], - m1[0][1] * m2[0][2] + m1[1][1] * m2[1][2] + m1[2][1] * m2[2][2], - m1[0][2] * m2[0][0] + m1[1][2] * m2[1][0] + m1[2][2] * m2[2][0], - m1[0][2] * m2[0][1] + m1[1][2] * m2[1][1] + m1[2][2] * m2[2][1], - m1[0][2] * m2[0][2] + m1[1][2] * m2[1][2] + m1[2][2] * m2[2][2]); +SIMD_FORCE_INLINE btMatrix3x3 btMultTransposeLeft(const btMatrix3x3& m1, const btMatrix3x3& m2) { +return btMatrix3x3( +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], +m1[0][1] * m2[0][0] + m1[1][1] * m2[1][0] + m1[2][1] * m2[2][0], +m1[0][1] * m2[0][1] + m1[1][1] * m2[1][1] + m1[2][1] * m2[2][1], +m1[0][1] * m2[0][2] + m1[1][1] * m2[1][2] + m1[2][1] * m2[2][2], +m1[0][2] * m2[0][0] + m1[1][2] * m2[1][0] + m1[2][2] * m2[2][0], +m1[0][2] * m2[0][1] + m1[1][2] * m2[1][1] + m1[2][2] * m2[2][1], +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. */ +* It will test all elements are equal. */ SIMD_FORCE_INLINE bool operator==(const btMatrix3x3& m1, const btMatrix3x3& m2) { - return ( m1[0][0] == m2[0][0] && m1[1][0] == m2[1][0] && m1[2][0] == m2[2][0] && - m1[0][1] == m2[0][1] && m1[1][1] == m2[1][1] && m1[2][1] == m2[2][1] && - m1[0][2] == m2[0][2] && m1[1][2] == m2[1][2] && m1[2][2] == m2[2][2] ); + return ( m1[0][0] == m2[0][0] && m1[1][0] == m2[1][0] && m1[2][0] == m2[2][0] && + m1[0][1] == m2[0][1] && m1[1][1] == m2[1][1] && m1[2][1] == m2[2][1] && + m1[0][2] == m2[0][2] && m1[1][2] == m2[1][2] && m1[2][2] == m2[2][2] ); } -#endif +///for serialization +struct btMatrix3x3Data +{ + btVector3Data m_el[3]; +}; + + +SIMD_FORCE_INLINE void btMatrix3x3::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::deSerialize(const struct btMatrix3x3Data& dataIn) +{ + for (int i=0;i<3;i++) + m_el[i].deSerialize(dataIn.m_el[i]); +} + +#endif //BT_MATRIX3x3_H + diff --git a/src/LinearMath/btSerializer.h b/src/LinearMath/btSerializer.h new file mode 100644 index 000000000..636b8f1dc --- /dev/null +++ b/src/LinearMath/btSerializer.h @@ -0,0 +1,288 @@ +/* +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_SERIALIZER_H +#define BT_SERIALIZER_H + +#include "btScalar.h" // has definitions like SIMD_FORCE_INLINE +#include "btStackAlloc.h" + +class btChunk +{ +public: + int m_chunkCode; + int m_length; + void *m_oldPtr; + int m_dna_nr; + int m_number; +}; + + +///Allow to serialize data in a chunk format +class btSerializer +{ + public: + + virtual ~btSerializer() {} + + virtual btChunk* allocate(size_t size,int numElements) = 0; + +}; + + +#define BT_HEADER_LENGTH 12 + + +class btDefaultSerializer +{ +public: + + btAlignedObjectArray mTypes; + btAlignedObjectArray mStructs; + btAlignedObjectArray mTlens; + btHashMap mStructReverse; + btHashMap mTypeLookup; + + btAlignedObjectArray m_chunkPtrs; + + int m_totalSize; + unsigned char* m_buffer; + int m_bufferPointer; + + + + + void* m_dna; + int m_dnaLength; + + + btDefaultSerializer(int totalSize) + :m_totalSize(totalSize), + m_bufferPointer(0) +// m_dna(0), +// m_dnaLength(0) + { + m_buffer = (unsigned char*)btAlignedAlloc(16,totalSize); + m_bufferPointer += BT_HEADER_LENGTH; + + memcpy(m_buffer, "BULLET ", 7); + int endian= 1; + endian= ((char*)&endian)[0]; + + if (endian) + { + m_buffer[7] = '_'; + } else + { + m_buffer[7] = '-'; + } + if (sizeof(void*)==8) + { + m_buffer[8]='V'; + } else + { + m_buffer[8]='v'; + } + + m_buffer[9] = '2'; + m_buffer[10] = '7'; + m_buffer[11] = '5'; + + + + } + + virtual ~btDefaultSerializer() + { + btAlignedFree(m_buffer); + } + + int getReverseType(const char *type) const + { + + btHashString key(type); + const int* valuePtr = mTypeLookup.find(key); + if (valuePtr) + return *valuePtr; + + return -1; + } + + void writeDNA() + { + unsigned char* dnaTarget = m_buffer+m_bufferPointer; + memcpy(dnaTarget,m_dna,m_dnaLength); + m_bufferPointer += m_dnaLength; + } + + virtual btChunk* allocate(size_t size, int numElements) + { + + unsigned char* ptr = m_buffer+m_bufferPointer; + m_bufferPointer += size*numElements+sizeof(btChunk); + + unsigned char* data = ptr + sizeof(btChunk); + + btChunk* chunk = (btChunk*)ptr; + chunk->m_chunkCode = 0; + chunk->m_oldPtr = data; + chunk->m_length = size; + chunk->m_number = numElements; + + m_chunkPtrs.push_back(chunk); + + return chunk; + } + + + void initDNA(const char* bdna,int dnalen) + { + + m_dna = btAlignedAlloc(dnalen,16); + memcpy(m_dna,bdna,dnalen); + m_dnaLength = dnalen; + + int *intPtr=0;short *shtPtr=0; + char *cp = 0;int dataLen =0;long nr=0; + intPtr = (int*)bdna; + + /* + SDNA (4 bytes) (magic number) + NAME (4 bytes) + (4 bytes) amount of names (int) + + + */ + + if (strncmp((const char*)bdna, "SDNA", 4)==0) + { + // skip ++ NAME + intPtr++; intPtr++; + } + + // Parse names + + dataLen = *intPtr; + intPtr++; + + cp = (char*)intPtr; + for (int i=0; i amount of types (int) + + + */ + + intPtr = (int*)cp; + assert(strncmp(cp, "TYPE", 4)==0); intPtr++; + + dataLen = *intPtr; + intPtr++; + + cp = (char*)intPtr; + for (int i=0; i (short) the lengths of types + + */ + + // Parse type lens + intPtr = (int*)cp; + assert(strncmp(cp, "TLEN", 4)==0); intPtr++; + + dataLen = (int)mTypes.size(); + + shtPtr = (short*)intPtr; + for (int i=0; i amount of structs (int) + + + + + + + */ + + intPtr = (int*)shtPtr; + cp = (char*)intPtr; + assert(strncmp(cp, "STRC", 4)==0); intPtr++; + + dataLen = *intPtr; + intPtr++; + + + shtPtr = (short*)intPtr; + for (int i=0; i