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

align btQuadWord on 16byte boundary

Browse Source 
slightly improved friction model
This commit is contained in:
ejcoumans
2007-09-25 06:58:53 +00:00
parent a38de566c6
commit 811c105c24
13 changed files with 224 additions and 175 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
Extras/BulletMultiThreaded/SpuCollisionTaskProcess.h
View File

@@ -23,7 +23,7 @@ subject to the following restrictions:
#include "PlatformDefinitions.h"
#include "LinearMath/btAlignedObjectArray.h"
#define DEBUG_SpuCollisionTaskProcess 1
//#define DEBUG_SpuCollisionTaskProcess 1
#define CMD_GATHER_AND_PROCESS_PAIRLIST 1

2
Extras/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuLocalSupport.h
View File

@@ -36,7 +36,7 @@ struct SpuConvexPolyhedronVertexData
};
inline btPoint3 localGetSupportingVertexWithoutMargin(int shapeType, void* shape, btVector3 localDir,struct SpuConvexPolyhedronVertexData* convexVertexData)//, int *featureIndex)
inline btPoint3 localGetSupportingVertexWithoutMargin(int shapeType, void* shape, btVector3& localDir,struct SpuConvexPolyhedronVertexData* convexVertexData)//, int *featureIndex)
{
switch (shapeType)
{

2
Extras/quickstep/OdeTypedJoint.cpp
View File

@@ -169,7 +169,7 @@ void OdeP2PJoint::GetInfo2(Info2 *info)
int bt_get_limit_motor_info2(
btRotationalLimitMotor * limot,
btRigidBody * body0, btRigidBody * body1,
BU_Joint::Info2 *info, int row, btVector3 ax1, int rotational)
BU_Joint::Info2 *info, int row, btVector3& ax1, int rotational)
{

2
Extras/quickstep/OdeTypedJoint.h
View File

@@ -105,7 +105,7 @@ public:
int bt_get_limit_motor_info2(
btRotationalLimitMotor * limot,
btRigidBody * body0, btRigidBody * body1,
BU_Joint::Info2 *info, int row, btVector3 ax1, int rotational);
BU_Joint::Info2 *info, int row, btVector3& ax1, int rotational);
#endif //CONTACT_JOINT_H

2
src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h
View File

@@ -40,7 +40,7 @@ struct btContactSolverInfo : public btContactSolverInfoData
m_friction = btScalar(0.3);
m_restitution = btScalar(0.);
m_maxErrorReduction = btScalar(20.);
m_numIterations = 10;
m_numIterations = 4;
m_erp = btScalar(0.4);
m_sor = btScalar(1.3);
}

2
src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp
View File

@@ -112,7 +112,7 @@ int btRotationalLimitMotor::testLimitValue(btScalar test_value)
btScalar btRotationalLimitMotor::solveAngularLimits(
btScalar timeStep,btVector3 axis,btScalar jacDiagABInv,
btScalar timeStep,btVector3& axis,btScalar jacDiagABInv,
btRigidBody * body0, btRigidBody * body1)
{
if (needApplyTorques()==false) return 0.0f;

2
src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h
View File

@@ -110,7 +110,7 @@ public:
int testLimitValue(btScalar test_value);
//! apply the correction impulses for two bodies
btScalar solveAngularLimits(btScalar timeStep,btVector3 axis, btScalar jacDiagABInv,btRigidBody * body0, btRigidBody * body1);
btScalar solveAngularLimits(btScalar timeStep,btVector3& axis, btScalar jacDiagABInv,btRigidBody * body0, btRigidBody * body1);
};

208
src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp
View File

@@ -13,6 +13,7 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
//#define COMPUTE_IMPULSE_DENOM 1
#include "btSequentialImpulseConstraintSolver.h"
#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
@@ -336,9 +337,57 @@ btScalar resolveSingleFrictionCacheFriendly(
btAlignedObjectArray<btSolverBody> tmpSolverBodyPool;
btAlignedObjectArray<btSolverConstraint> tmpSolverConstraintPool;
btAlignedObjectArray<btSolverConstraint> tmpSolverFrictionConstraintPool;
btAlignedObjectArray<int> gOrderTmpConstraintPool;
btAlignedObjectArray<int> gOrderFrictionConstraintPool;
btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendly(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc)
void addFrictionConstraint(const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btRigidBody* rb0,btRigidBody* rb1, btScalar relaxation)
{
btSolverConstraint& solverConstraint = tmpSolverFrictionConstraintPool.expand();
solverConstraint.m_contactNormal = normalAxis;
solverConstraint.m_solverBodyIdA = solverBodyIdA;
solverConstraint.m_solverBodyIdB = solverBodyIdB;
solverConstraint.m_constraintType = btSolverConstraint::BT_SOLVER_FRICTION_1D;
solverConstraint.m_frictionIndex = frictionIndex;
solverConstraint.m_friction = cp.m_combinedFriction;
solverConstraint.m_appliedImpulse = btScalar(0.);
solverConstraint.m_appliedVelocityImpulse = 0.f;
solverConstraint.m_penetration = 0.f;
{
btVector3 ftorqueAxis1 = rel_pos1.cross(solverConstraint.m_contactNormal);
solverConstraint.m_relpos1CrossNormal = ftorqueAxis1;
solverConstraint.m_angularComponentA = rb0->getInvInertiaTensorWorld()*ftorqueAxis1;
}
{
btVector3 ftorqueAxis1 = rel_pos2.cross(solverConstraint.m_contactNormal);
solverConstraint.m_relpos2CrossNormal = ftorqueAxis1;
solverConstraint.m_angularComponentB = rb1->getInvInertiaTensorWorld()*ftorqueAxis1;
}
#ifdef COMPUTE_IMPULSE_DENOM
btScalar denom0 = rb0->computeImpulseDenominator(pos1,solverConstraint.m_contactNormal);
btScalar denom1 = rb1->computeImpulseDenominator(pos2,solverConstraint.m_contactNormal);
#else
btVector3 vec;
vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1);
btScalar denom0 = rb0->getInvMass() + normalAxis.dot(vec);
vec = ( solverConstraint.m_angularComponentB).cross(rel_pos2);
btScalar denom1 = rb1->getInvMass() + normalAxis.dot(vec);
#endif //COMPUTE_IMPULSE_DENOM
btScalar denom = relaxation/(denom0+denom1);
solverConstraint.m_jacDiagABInv = denom;
}
btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc)
{
(void)stackAlloc;
(void)debugDrawer;
@@ -445,6 +494,10 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendly(btCollisio
}
}
btVector3 rel_pos1;
btVector3 rel_pos2;
btScalar relaxation;
for (int j=0;j<manifold->getNumContacts();j++)
{
@@ -458,11 +511,13 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendly(btCollisio
const btVector3& pos1 = cp.getPositionWorldOnA();
const btVector3& pos2 = cp.getPositionWorldOnB();
btVector3 rel_pos1 = pos1 - rb0->getCenterOfMassPosition();
btVector3 rel_pos2 = pos2 - rb1->getCenterOfMassPosition();
rel_pos1 = pos1 - rb0->getCenterOfMassPosition();
rel_pos2 = pos2 - rb1->getCenterOfMassPosition();
btScalar relaxation = 1.f;
relaxation = 1.f;
btScalar rel_vel;
btVector3 vel;
{
btSolverConstraint& solverConstraint = tmpSolverConstraintPool.expand();
@@ -471,12 +526,22 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendly(btCollisio
solverConstraint.m_solverBodyIdB = solverBodyIdB;
solverConstraint.m_constraintType = btSolverConstraint::BT_SOLVER_CONTACT_1D;
btVector3 torqueAxis0 = rel_pos1.cross(cp.m_normalWorldOnB);
solverConstraint.m_angularComponentA = rb0->getInvInertiaTensorWorld()*torqueAxis0;
btVector3 torqueAxis1 = rel_pos2.cross(cp.m_normalWorldOnB);
solverConstraint.m_angularComponentB = rb1->getInvInertiaTensorWorld()*torqueAxis1;
{
//can be optimized, the cross products are already calculated
#ifdef COMPUTE_IMPULSE_DENOM
btScalar denom0 = rb0->computeImpulseDenominator(pos1,cp.m_normalWorldOnB);
btScalar denom1 = rb1->computeImpulseDenominator(pos2,cp.m_normalWorldOnB);
#else
btVector3 vec;
vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1);
btScalar denom0 = rb0->getInvMass() + cp.m_normalWorldOnB.dot(vec);
vec = ( solverConstraint.m_angularComponentB).cross(rel_pos2);
btScalar denom1 = rb1->getInvMass() + cp.m_normalWorldOnB.dot(vec);
#endif //COMPUTE_IMPULSE_DENOM
btScalar denom = relaxation/(denom0+denom1);
solverConstraint.m_jacDiagABInv = denom;
}
@@ -489,8 +554,8 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendly(btCollisio
btVector3 vel1 = rb0->getVelocityInLocalPoint(rel_pos1);
btVector3 vel2 = rb1->getVelocityInLocalPoint(rel_pos2);
btVector3 vel = vel1 - vel2;
btScalar rel_vel;
vel = vel1 - vel2;
rel_vel = cp.m_normalWorldOnB.dot(vel);
@@ -516,83 +581,26 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendly(btCollisio
solverConstraint.m_appliedVelocityImpulse = 0.f;
btVector3 torqueAxis0 = rel_pos1.cross(cp.m_normalWorldOnB);
solverConstraint.m_angularComponentA = rb0->getInvInertiaTensorWorld()*torqueAxis0;
btVector3 torqueAxis1 = rel_pos2.cross(cp.m_normalWorldOnB);
solverConstraint.m_angularComponentB = rb1->getInvInertiaTensorWorld()*torqueAxis1;
}
//create 2 '1d axis' constraints for 2 tangential friction directions
//re-calculate friction direction every frame, todo: check if this is really needed
btVector3 frictionTangential0a, frictionTangential1b;
btPlaneSpace1(cp.m_normalWorldOnB,frictionTangential0a,frictionTangential1b);
{
btSolverConstraint& solverConstraint = tmpSolverFrictionConstraintPool.expand();
solverConstraint.m_contactNormal = frictionTangential0a;
solverConstraint.m_solverBodyIdA = solverBodyIdA;
solverConstraint.m_solverBodyIdB = solverBodyIdB;
solverConstraint.m_constraintType = btSolverConstraint::BT_SOLVER_FRICTION_1D;
solverConstraint.m_frictionIndex = frictionIndex;
solverConstraint.m_friction = cp.m_combinedFriction;
solverConstraint.m_appliedImpulse = btScalar(0.);
solverConstraint.m_appliedVelocityImpulse = 0.f;
btScalar denom0 = rb0->computeImpulseDenominator(pos1,solverConstraint.m_contactNormal);
btScalar denom1 = rb1->computeImpulseDenominator(pos2,solverConstraint.m_contactNormal);
btScalar denom = relaxation/(denom0+denom1);
solverConstraint.m_jacDiagABInv = denom;
{
btVector3 ftorqueAxis0 = rel_pos1.cross(solverConstraint.m_contactNormal);
solverConstraint.m_relpos1CrossNormal = ftorqueAxis0;
solverConstraint.m_angularComponentA = rb0->getInvInertiaTensorWorld()*ftorqueAxis0;
}
{
btVector3 ftorqueAxis0 = rel_pos2.cross(solverConstraint.m_contactNormal);
solverConstraint.m_relpos2CrossNormal = ftorqueAxis0;
solverConstraint.m_angularComponentB = rb1->getInvInertiaTensorWorld()*ftorqueAxis0;
}
}
{
btSolverConstraint& solverConstraint = tmpSolverFrictionConstraintPool.expand();
solverConstraint.m_contactNormal = frictionTangential1b;
solverConstraint.m_solverBodyIdA = solverBodyIdA;
solverConstraint.m_solverBodyIdB = solverBodyIdB;
solverConstraint.m_constraintType = btSolverConstraint::BT_SOLVER_FRICTION_1D;
solverConstraint.m_frictionIndex = frictionIndex;
solverConstraint.m_friction = cp.m_combinedFriction;
solverConstraint.m_appliedImpulse = btScalar(0.);
solverConstraint.m_appliedVelocityImpulse = 0.f;
btScalar denom0 = rb0->computeImpulseDenominator(pos1,solverConstraint.m_contactNormal);
btScalar denom1 = rb1->computeImpulseDenominator(pos2,solverConstraint.m_contactNormal);
btScalar denom = relaxation/(denom0+denom1);
solverConstraint.m_jacDiagABInv = denom;
}
{
btVector3 lat_vel = vel - cp.m_normalWorldOnB * rel_vel;
btScalar lat_rel_vel = lat_vel.length2();
if (lat_rel_vel > SIMD_EPSILON)//0.0f)
{
btVector3 ftorqueAxis1 = rel_pos1.cross(solverConstraint.m_contactNormal);
solverConstraint.m_relpos1CrossNormal = ftorqueAxis1;
solverConstraint.m_angularComponentA = rb0->getInvInertiaTensorWorld()*ftorqueAxis1;
}
{
btVector3 ftorqueAxis1 = rel_pos2.cross(solverConstraint.m_contactNormal);
solverConstraint.m_relpos2CrossNormal = ftorqueAxis1;
solverConstraint.m_angularComponentB = rb1->getInvInertiaTensorWorld()*ftorqueAxis1;
}
lat_vel /= btSqrt(lat_rel_vel);
addFrictionConstraint(lat_vel,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,rb0,rb1, relaxation);
btVector3 frictionDir2 = lat_vel.cross(cp.m_normalWorldOnB);
frictionDir2.normalize();//??
addFrictionConstraint(frictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,rb0,rb1, relaxation);
}
}
}
}
}
@@ -613,8 +621,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendly(btCollisio
}
}
btAlignedObjectArray<int> gOrderTmpConstraintPool;
btAlignedObjectArray<int> gOrderFrictionConstraintPool;
int numConstraintPool = tmpSolverConstraintPool.size();
int numFrictionPool = tmpSolverFrictionConstraintPool.size();
@@ -638,14 +645,20 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendly(btCollisio
END_PROFILE("prepareConstraints");
return 0.f;
}
BEGIN_PROFILE("solveConstraints");
btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc)
{
BEGIN_PROFILE("solveConstraintsIterations");
int numConstraintPool = tmpSolverConstraintPool.size();
int numFrictionPool = tmpSolverFrictionConstraintPool.size();
//should traverse the contacts random order...
int iteration;
{
for ( iteration = 0;iteration<info.m_numIterations;iteration++)
for ( iteration = 0;iteration<infoGlobal.m_numIterations;iteration++)
{
int j;
@@ -682,7 +695,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendly(btCollisio
tmpSolverBodyPool[constraint->getRigidBodyB().getCompanionId()].writebackVelocity();
}
constraint->solveConstraint(info.m_timeStep);
constraint->solveConstraint(infoGlobal.m_timeStep);
if ((constraint->getRigidBodyA().getIslandTag() >= 0) && (constraint->getRigidBodyA().getCompanionId() >= 0))
{
@@ -701,7 +714,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendly(btCollisio
{
btSolverConstraint& solveManifold = tmpSolverConstraintPool[gOrderTmpConstraintPool[j]];
resolveSingleCollisionCombinedCacheFriendly(tmpSolverBodyPool[solveManifold.m_solverBodyIdA],
tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold,info);
tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold,infoGlobal);
}
}
@@ -713,7 +726,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendly(btCollisio
btScalar appliedNormalImpulse = tmpSolverConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
resolveSingleFrictionCacheFriendly(tmpSolverBodyPool[solveManifold.m_solverBodyIdA],
tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold,info,appliedNormalImpulse);
tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold,infoGlobal,appliedNormalImpulse);
}
}
@@ -721,13 +734,28 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendly(btCollisio
}
}
END_PROFILE("solveConstraintsIterations");
return 0.f;
}
btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendly(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc)
{
int i;
solveGroupCacheFriendlySetup( bodies, numBodies, manifoldPtr, numManifolds,constraints, numConstraints,infoGlobal,debugDrawer, stackAlloc);
solveGroupCacheFriendlyIterations(bodies, numBodies, manifoldPtr, numManifolds,constraints, numConstraints,infoGlobal,debugDrawer, stackAlloc);
BEGIN_PROFILE("solveWriteBackVelocity");
for ( i=0;i<tmpSolverBodyPool.size();i++)
{
tmpSolverBodyPool[i].writebackVelocity();
}
END_PROFILE("solveConstraints");
END_PROFILE("solveWriteBackVelocity");
// printf("tmpSolverConstraintPool.size() = %i\n",tmpSolverConstraintPool.size());

3
src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h
View File

@@ -73,6 +73,9 @@ public:
virtual btScalar solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btStackAlloc* stackAlloc,btDispatcher* dispatcher);
virtual btScalar solveGroupCacheFriendly(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc);
btScalar solveGroupCacheFriendlyIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc);
btScalar solveGroupCacheFriendlySetup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc);
///clear internal cached data and reset random seed
virtual void reset();

149
src/BulletDynamics/ConstraintSolver/btSolverBody.h
View File

@@ -1,75 +1,74 @@
/*
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_SOLVER_BODY_H
#define BT_SOLVER_BODY_H
class btRigidBody;
#include "LinearMath/btVector3.h"
#include "LinearMath/btMatrix3x3.h"
#include "BulletDynamics/Dynamics/btRigidBody.h"
#include "LinearMath/btAlignedAllocator.h"
///btSolverBody is an internal datastructure for the constraint solver. Only necessary data is packed to increase cache coherence/performance.
ATTRIBUTE_ALIGNED16 (struct) btSolverBody
{
BT_DECLARE_ALIGNED_ALLOCATOR();
btVector3 m_centerOfMassPosition;
btVector3 m_linearVelocity;
btVector3 m_angularVelocity;
btRigidBody* m_originalBody;
float m_invMass;
float m_friction;
float m_angularFactor;
inline void getVelocityInLocalPoint(const btVector3& rel_pos, btVector3& velocity ) const
{
velocity = m_linearVelocity + m_angularVelocity.cross(rel_pos);
}
//Optimization for the iterative solver: avoid calculating constant terms involving inertia, normal, relative position
inline void internalApplyImpulse(const btVector3& linearComponent, const btVector3& angularComponent,btScalar impulseMagnitude)
{
m_linearVelocity += linearComponent*impulseMagnitude;
m_angularVelocity += angularComponent*impulseMagnitude*m_angularFactor;
}
void writebackVelocity()
{
if (m_invMass)
{
m_originalBody->setLinearVelocity(m_linearVelocity);
m_originalBody->setAngularVelocity(m_angularVelocity);
}
}
void readVelocity()
{
if (m_invMass)
{
m_linearVelocity = m_originalBody->getLinearVelocity();
m_angularVelocity = m_originalBody->getAngularVelocity();
}
}
};
#endif //BT_SOLVER_BODY_H
/*
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_SOLVER_BODY_H
#define BT_SOLVER_BODY_H
class btRigidBody;
#include "LinearMath/btVector3.h"
#include "LinearMath/btMatrix3x3.h"
#include "BulletDynamics/Dynamics/btRigidBody.h"
#include "LinearMath/btAlignedAllocator.h"
///btSolverBody is an internal datastructure for the constraint solver. Only necessary data is packed to increase cache coherence/performance.
ATTRIBUTE_ALIGNED16 (struct) btSolverBody
{
BT_DECLARE_ALIGNED_ALLOCATOR();
btVector3 m_centerOfMassPosition;
btVector3 m_linearVelocity;
btVector3 m_angularVelocity;
btRigidBody* m_originalBody;
float m_invMass;
float m_friction;
float m_angularFactor;
inline void getVelocityInLocalPoint(const btVector3& rel_pos, btVector3& velocity ) const
{
velocity = m_linearVelocity + m_angularVelocity.cross(rel_pos);
}
//Optimization for the iterative solver: avoid calculating constant terms involving inertia, normal, relative position
inline void internalApplyImpulse(const btVector3& linearComponent, const btVector3& angularComponent,btScalar impulseMagnitude)
{
m_linearVelocity += linearComponent*impulseMagnitude;
m_angularVelocity += angularComponent*(impulseMagnitude*m_angularFactor);
}
void writebackVelocity()
{
if (m_invMass)
{
m_originalBody->setLinearVelocity(m_linearVelocity);
m_originalBody->setAngularVelocity(m_angularVelocity);
}
}
void readVelocity()
{
if (m_invMass)
{
m_linearVelocity = m_originalBody->getLinearVelocity();
m_angularVelocity = m_originalBody->getAngularVelocity();
}
}
};
#endif //BT_SOLVER_BODY_H

2
src/LinearMath/btQuadWord.h
View File

@@ -19,7 +19,7 @@ subject to the following restrictions:
#include "btScalar.h"
#include "btSimdMinMax.h"
class btQuadWordStorage
ATTRIBUTE_ALIGNED16(class) btQuadWordStorage
{
protected:
btScalar m_x;

2
src/LinearMath/btQuaternion.h
View File

@@ -308,7 +308,7 @@ shortestArcQuat(const btVector3& v0, const btVector3& v1) // Game Programming Ge
}
SIMD_FORCE_INLINE btQuaternion
shortestArcQuatNormalize(btVector3 v0,btVector3 v1)
shortestArcQuatNormalize2(btVector3& v0,btVector3& v1)
{
v0.normalize();
v1.normalize();

21
src/LinearMath/btScalar.h
View File

@@ -154,7 +154,26 @@ SIMD_FORCE_INLINE btScalar btPow(btScalar x,btScalar y) { return pow(x,y); }
#else
SIMD_FORCE_INLINE btScalar btSqrt(btScalar x) { return sqrtf(x); }
SIMD_FORCE_INLINE btScalar btSqrt(btScalar y)
{
#ifdef USE_APPROXIMATION
double x, z, tempf;
unsigned long *tfptr = ((unsigned long *)&tempf) + 1;
tempf = y;
*tfptr = (0xbfcdd90a - *tfptr)>>1; /* estimate of 1/sqrt(y) */
x = tempf;
z = y*btScalar(0.5); /* hoist out the <20>/2<> */
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);
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); }