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

Add 1D and 2D support for Bullet: using rigidbody->angularFactor(const btVector3& factor) and body->setLinearFactor(const btVector3& linearFactor);

Browse Source 
For example, to only allow linear motion in the X-Z plane, and only rotation around Y axis use:
	body->setLinearFactor(btVector3(1,0,1));
	body->setAngularFactor(btVector3(0,1,0));
Fix build issues with CodeBlocks, when generating projectfiles using CMake 2.6:
${OPENGL_glU_LIBRARY} should be ${OPENGL_glu_LIBRARY}
Fix build issue with CodeBlocks, comment out xmlfree in Extras/COLLADA_DOM/src/modules/LIBXMLPlugin/daeLIBXMLPlugin.cpp (will leak memory)
This commit is contained in:
erwin.coumans
2009-03-08 04:14:17 +00:00
parent 3317f17270
commit 700db838b1
8 changed files with 91 additions and 77 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
Demos/ForkLiftDemo/CMakeLists.txt
View File

@@ -52,7 +52,7 @@ ${BULLET_PHYSICS_SOURCE_DIR}/src ${BULLET_PHYSICS_SOURCE_DIR}/Demos/OpenGL }
) )
LINK_LIBRARIES( LINK_LIBRARIES(
OpenGLSupport BulletDynamics BulletCollision LinearMath ${GLUT_glut_LIBRARY} ${OPENGL_gl_LIBRARY} ${OPENGL_glU_LIBRARY} OpenGLSupport BulletDynamics BulletCollision LinearMath ${GLUT_glut_LIBRARY} ${OPENGL_gl_LIBRARY} ${OPENGL_glu_LIBRARY}
) )
ADD_EXECUTABLE(AppForkLiftDemo ADD_EXECUTABLE(AppForkLiftDemo

7
Demos/OpenGL/DemoApplication.cpp
View File

@@ -71,7 +71,7 @@ m_cameraUp(0,1,0),
m_forwardAxis(2), m_forwardAxis(2),
m_glutScreenWidth(0), m_glutScreenWidth(0),
m_glutScreenHeight(0), m_glutScreenHeight(0),
m_ShootBoxInitialSpeed(40.f), m_ShootBoxInitialSpeed(4.f),
m_stepping(true), m_stepping(true),
m_singleStep(false), m_singleStep(false),
m_idle(false), m_idle(false),
@@ -544,7 +544,7 @@ void DemoApplication::setShootBoxShape ()
btConvexShape* childShape = new btBoxShape(btVector3(1.f,1.f,1.f)); btConvexShape* childShape = new btBoxShape(btVector3(1.f,1.f,1.f));
m_shootBoxShape = new btUniformScalingShape(childShape,0.5f); m_shootBoxShape = new btUniformScalingShape(childShape,0.5f);
#else #else
m_shootBoxShape = new btSphereShape(1.f);//BoxShape(btVector3(1.f,1.f,1.f)); m_shootBoxShape = new btSphereShape(.1f);//BoxShape(btVector3(1.f,1.f,1.f));
#endif// #endif//
} }
} }
@@ -554,7 +554,7 @@ void DemoApplication::shootBox(const btVector3& destination)
if (m_dynamicsWorld) if (m_dynamicsWorld)
{ {
float mass = 10.f; float mass = 0.1f;
btTransform startTransform; btTransform startTransform;
startTransform.setIdentity(); startTransform.setIdentity();
btVector3 camPos = getCameraPosition(); btVector3 camPos = getCameraPosition();
@@ -563,6 +563,7 @@ void DemoApplication::shootBox(const btVector3& destination)
setShootBoxShape (); setShootBoxShape ();
btRigidBody* body = this->localCreateRigidBody(mass, startTransform,m_shootBoxShape); btRigidBody* body = this->localCreateRigidBody(mass, startTransform,m_shootBoxShape);
body->setLinearFactor(btVector3(1,1,1));
btVector3 linVel(destination[0]-camPos[0],destination[1]-camPos[1],destination[2]-camPos[2]); btVector3 linVel(destination[0]-camPos[0],destination[1]-camPos[1],destination[2]-camPos[2]);
linVel.normalize(); linVel.normalize();

6
Extras/COLLADA_DOM/src/modules/LIBXMLPlugin/daeLIBXMLPlugin.cpp
View File

@@ -377,7 +377,8 @@ void daeLIBXMLPlugin::readAttributes( daeElement *element, xmlTextReaderPtr read
} }
} }
} }
xmlFree(value); //rather leak than break compilers (CodeBlocks/MinGW)
//xmlFree(value);
} }
} }
} }
@@ -425,7 +426,8 @@ void daeLIBXMLPlugin::readValue( daeElement *element, xmlTextReaderPtr reader )
// eat the characters we just read (would be nice if set returned characters used. // eat the characters we just read (would be nice if set returned characters used.
} }
} }
xmlFree(value); //rather leak than break compilers (CodeBlocks/MinGW)
//xmlFree(value);
} }
int ret = xmlTextReaderRead(reader); int ret = xmlTextReaderRead(reader);
assert(ret==1); assert(ret==1);

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

@@ -76,8 +76,8 @@ void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD(
__m128 upperMinApplied = _mm_sub_ps(upperLimit1,cpAppliedImp); __m128 upperMinApplied = _mm_sub_ps(upperLimit1,cpAppliedImp);
deltaImpulse = _mm_or_ps( _mm_and_ps(resultUpperLess, deltaImpulse), _mm_andnot_ps(resultUpperLess, upperMinApplied) ); deltaImpulse = _mm_or_ps( _mm_and_ps(resultUpperLess, deltaImpulse), _mm_andnot_ps(resultUpperLess, upperMinApplied) );
c.m_appliedImpulse = _mm_or_ps( _mm_and_ps(resultUpperLess, c.m_appliedImpulse), _mm_andnot_ps(resultUpperLess, upperLimit1) ); c.m_appliedImpulse = _mm_or_ps( _mm_and_ps(resultUpperLess, c.m_appliedImpulse), _mm_andnot_ps(resultUpperLess, upperLimit1) );
__m128 linearComponentA = _mm_mul_ps(c.m_contactNormal.mVec128,_mm_set1_ps(body1.m_invMass)); __m128 linearComponentA = _mm_mul_ps(c.m_contactNormal.mVec128,body1.m_invMass.mVec128);
__m128 linearComponentB = _mm_mul_ps((c.m_contactNormal).mVec128,_mm_set1_ps(body2.m_invMass)); __m128 linearComponentB = _mm_mul_ps((c.m_contactNormal).mVec128,body2.m_invMass.mVec128);
__m128 impulseMagnitude = deltaImpulse; __m128 impulseMagnitude = deltaImpulse;
body1.m_deltaLinearVelocity.mVec128 = _mm_add_ps(body1.m_deltaLinearVelocity.mVec128,_mm_mul_ps(linearComponentA,impulseMagnitude)); body1.m_deltaLinearVelocity.mVec128 = _mm_add_ps(body1.m_deltaLinearVelocity.mVec128,_mm_mul_ps(linearComponentA,impulseMagnitude));
body1.m_deltaAngularVelocity.mVec128 = _mm_add_ps(body1.m_deltaAngularVelocity.mVec128 ,_mm_mul_ps(c.m_angularComponentA.mVec128,impulseMagnitude)); body1.m_deltaAngularVelocity.mVec128 = _mm_add_ps(body1.m_deltaAngularVelocity.mVec128 ,_mm_mul_ps(c.m_angularComponentA.mVec128,impulseMagnitude));
@@ -114,9 +114,7 @@ void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD(
{ {
c.m_appliedImpulse = sum; c.m_appliedImpulse = sum;
} }
if (body1.m_invMass)
body1.applyImpulse(c.m_contactNormal*body1.m_invMass,c.m_angularComponentA,deltaImpulse); body1.applyImpulse(c.m_contactNormal*body1.m_invMass,c.m_angularComponentA,deltaImpulse);
if (body2.m_invMass)
body2.applyImpulse(-c.m_contactNormal*body2.m_invMass,c.m_angularComponentB,deltaImpulse); body2.applyImpulse(-c.m_contactNormal*body2.m_invMass,c.m_angularComponentB,deltaImpulse);
} }
@@ -138,8 +136,8 @@ void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD(
__m128 lowMinApplied = _mm_sub_ps(lowerLimit1,cpAppliedImp); __m128 lowMinApplied = _mm_sub_ps(lowerLimit1,cpAppliedImp);
deltaImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowMinApplied), _mm_andnot_ps(resultLowerLess, deltaImpulse) ); deltaImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowMinApplied), _mm_andnot_ps(resultLowerLess, deltaImpulse) );
c.m_appliedImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowerLimit1), _mm_andnot_ps(resultLowerLess, sum) ); c.m_appliedImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowerLimit1), _mm_andnot_ps(resultLowerLess, sum) );
__m128 linearComponentA = _mm_mul_ps(c.m_contactNormal.mVec128,_mm_set1_ps(body1.m_invMass)); __m128 linearComponentA = _mm_mul_ps(c.m_contactNormal.mVec128,body1.m_invMass.mVec128);
__m128 linearComponentB = _mm_mul_ps((c.m_contactNormal).mVec128,_mm_set1_ps(body2.m_invMass)); __m128 linearComponentB = _mm_mul_ps((c.m_contactNormal).mVec128,body2.m_invMass.mVec128);
__m128 impulseMagnitude = deltaImpulse; __m128 impulseMagnitude = deltaImpulse;
body1.m_deltaLinearVelocity.mVec128 = _mm_add_ps(body1.m_deltaLinearVelocity.mVec128,_mm_mul_ps(linearComponentA,impulseMagnitude)); body1.m_deltaLinearVelocity.mVec128 = _mm_add_ps(body1.m_deltaLinearVelocity.mVec128,_mm_mul_ps(linearComponentA,impulseMagnitude));
body1.m_deltaAngularVelocity.mVec128 = _mm_add_ps(body1.m_deltaAngularVelocity.mVec128 ,_mm_mul_ps(c.m_angularComponentA.mVec128,impulseMagnitude)); body1.m_deltaAngularVelocity.mVec128 = _mm_add_ps(body1.m_deltaAngularVelocity.mVec128 ,_mm_mul_ps(c.m_angularComponentA.mVec128,impulseMagnitude));
@@ -169,9 +167,7 @@ void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD(
{ {
c.m_appliedImpulse = sum; c.m_appliedImpulse = sum;
} }
if (body1.m_invMass)
body1.applyImpulse(c.m_contactNormal*body1.m_invMass,c.m_angularComponentA,deltaImpulse); body1.applyImpulse(c.m_contactNormal*body1.m_invMass,c.m_angularComponentA,deltaImpulse);
if (body2.m_invMass)
body2.applyImpulse(-c.m_contactNormal*body2.m_invMass,c.m_angularComponentB,deltaImpulse); body2.applyImpulse(-c.m_contactNormal*body2.m_invMass,c.m_angularComponentB,deltaImpulse);
} }
@@ -224,14 +220,14 @@ void btSequentialImpulseConstraintSolver::initSolverBody(btSolverBody* solverBod
if (rb) if (rb)
{ {
solverBody->m_invMass = rb->getInvMass(); solverBody->m_invMass = btVector3(rb->getInvMass(),rb->getInvMass(),rb->getInvMass())*rb->getLinearFactor();
solverBody->m_originalBody = rb; solverBody->m_originalBody = rb;
solverBody->m_angularFactor = rb->getAngularFactor(); solverBody->m_angularFactor = rb->getAngularFactor();
} else } else
{ {
solverBody->m_invMass = 0.f; solverBody->m_invMass.setValue(0,0,0);
solverBody->m_originalBody = 0; solverBody->m_originalBody = 0;
solverBody->m_angularFactor = 1.f; solverBody->m_angularFactor.setValue(1,1,1);
} }
} }
@@ -494,13 +490,13 @@ void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m
///warm starting (or zero if disabled) ///warm starting (or zero if disabled)
if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING) if (0)//infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
{ {
solverConstraint.m_appliedImpulse = cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor; solverConstraint.m_appliedImpulse = cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor;
if (rb0) if (rb0)
m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA].applyImpulse(solverConstraint.m_contactNormal*rb0->getInvMass(),solverConstraint.m_angularComponentA,solverConstraint.m_appliedImpulse); m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA].applyImpulse(solverConstraint.m_contactNormal*rb0->getInvMass()*rb0->getLinearFactor(),solverConstraint.m_angularComponentA,solverConstraint.m_appliedImpulse);
if (rb1) if (rb1)
m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB].applyImpulse(solverConstraint.m_contactNormal*rb1->getInvMass(),-solverConstraint.m_angularComponentB,-solverConstraint.m_appliedImpulse); m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB].applyImpulse(solverConstraint.m_contactNormal*rb1->getInvMass()*rb1->getLinearFactor(),-solverConstraint.m_angularComponentB,-solverConstraint.m_appliedImpulse);
} else } else
{ {
solverConstraint.m_appliedImpulse = 0.f; solverConstraint.m_appliedImpulse = 0.f;
@@ -587,9 +583,9 @@ void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m
{ {
frictionConstraint1.m_appliedImpulse = cp.m_appliedImpulseLateral1 * infoGlobal.m_warmstartingFactor; frictionConstraint1.m_appliedImpulse = cp.m_appliedImpulseLateral1 * infoGlobal.m_warmstartingFactor;
if (rb0) if (rb0)
m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA].applyImpulse(frictionConstraint1.m_contactNormal*rb0->getInvMass(),frictionConstraint1.m_angularComponentA,frictionConstraint1.m_appliedImpulse); m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA].applyImpulse(frictionConstraint1.m_contactNormal*rb0->getInvMass()*rb0->getLinearFactor(),frictionConstraint1.m_angularComponentA,frictionConstraint1.m_appliedImpulse);
if (rb1) if (rb1)
m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB].applyImpulse(frictionConstraint1.m_contactNormal*rb1->getInvMass(),-frictionConstraint1.m_angularComponentB,-frictionConstraint1.m_appliedImpulse); m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB].applyImpulse(frictionConstraint1.m_contactNormal*rb1->getInvMass()*rb1->getLinearFactor(),-frictionConstraint1.m_angularComponentB,-frictionConstraint1.m_appliedImpulse);
} else } else
{ {
frictionConstraint1.m_appliedImpulse = 0.f; frictionConstraint1.m_appliedImpulse = 0.f;

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

@@ -110,8 +110,8 @@ ATTRIBUTE_ALIGNED16 (struct) btSolverBody
BT_DECLARE_ALIGNED_ALLOCATOR(); BT_DECLARE_ALIGNED_ALLOCATOR();
btVector3 m_deltaLinearVelocity; btVector3 m_deltaLinearVelocity;
btVector3 m_deltaAngularVelocity; btVector3 m_deltaAngularVelocity;
btScalar m_angularFactor; btVector3 m_angularFactor;
btScalar m_invMass; btVector3 m_invMass;
btScalar m_friction; btScalar m_friction;
btRigidBody* m_originalBody; btRigidBody* m_originalBody;
btVector3 m_pushVelocity; btVector3 m_pushVelocity;
@@ -162,7 +162,7 @@ ATTRIBUTE_ALIGNED16 (struct) btSolverBody
void writebackVelocity(btScalar timeStep=0) void writebackVelocity(btScalar timeStep=0)
{ {
if (m_invMass) if (m_originalBody)
{ {
m_originalBody->setLinearVelocity(m_originalBody->getLinearVelocity()+m_deltaLinearVelocity); m_originalBody->setLinearVelocity(m_originalBody->getLinearVelocity()+m_deltaLinearVelocity);
m_originalBody->setAngularVelocity(m_originalBody->getAngularVelocity()+m_deltaAngularVelocity); m_originalBody->setAngularVelocity(m_originalBody->getAngularVelocity()+m_deltaAngularVelocity);

3
src/BulletDynamics/Dynamics/btRigidBody.cpp
View File

@@ -44,7 +44,8 @@ void btRigidBody::setupRigidBody(const btRigidBody::btRigidBodyConstructionInfo&
m_linearVelocity.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0)); m_linearVelocity.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0));
m_angularVelocity.setValue(btScalar(0.),btScalar(0.),btScalar(0.)); m_angularVelocity.setValue(btScalar(0.),btScalar(0.),btScalar(0.));
m_angularFactor = btScalar(1.); m_angularFactor.setValue(1,1,1);
m_linearFactor.setValue(1,1,1);
m_gravity.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0)); m_gravity.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0));
m_gravity_acceleration.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0)); m_gravity_acceleration.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0));
m_totalForce.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0)); m_totalForce.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0));

36
src/BulletDynamics/Dynamics/btRigidBody.h
View File

@@ -45,7 +45,8 @@ class btRigidBody : public btCollisionObject
btVector3 m_linearVelocity; btVector3 m_linearVelocity;
btVector3 m_angularVelocity; btVector3 m_angularVelocity;
btScalar m_inverseMass; btScalar m_inverseMass;
btScalar m_angularFactor; btVector3 m_angularFactor;
btVector3 m_linearFactor;
btVector3 m_gravity; btVector3 m_gravity;
btVector3 m_gravity_acceleration; btVector3 m_gravity_acceleration;
@@ -219,6 +220,14 @@ public:
void setMassProps(btScalar mass, const btVector3& inertia); void setMassProps(btScalar mass, const btVector3& inertia);
const btVector3& getLinearFactor() const
{
return m_linearFactor;
}
void setLinearFactor(const btVector3& linearFactor)
{
m_linearFactor = linearFactor;
}
btScalar getInvMass() const { return m_inverseMass; } btScalar getInvMass() const { return m_inverseMass; }
const btMatrix3x3& getInvInertiaTensorWorld() const { const btMatrix3x3& getInvInertiaTensorWorld() const {
return m_invInertiaTensorWorld; return m_invInertiaTensorWorld;
@@ -230,7 +239,7 @@ public:
void applyCentralForce(const btVector3& force) void applyCentralForce(const btVector3& force)
{ {
m_totalForce += force; m_totalForce += force*m_linearFactor;
} }
const btVector3& getTotalForce() const btVector3& getTotalForce()
@@ -261,23 +270,23 @@ public:
void applyTorque(const btVector3& torque) void applyTorque(const btVector3& torque)
{ {
m_totalTorque += torque; m_totalTorque += torque*m_angularFactor;
} }
void applyForce(const btVector3& force, const btVector3& rel_pos) void applyForce(const btVector3& force, const btVector3& rel_pos)
{ {
applyCentralForce(force); applyCentralForce(force);
applyTorque(rel_pos.cross(force)*m_angularFactor); applyTorque(rel_pos.cross(force*m_linearFactor));
} }
void applyCentralImpulse(const btVector3& impulse) void applyCentralImpulse(const btVector3& impulse)
{ {
m_linearVelocity += impulse * m_inverseMass; m_linearVelocity += impulse *m_linearFactor * m_inverseMass;
} }
void applyTorqueImpulse(const btVector3& torque) void applyTorqueImpulse(const btVector3& torque)
{ {
m_angularVelocity += m_invInertiaTensorWorld * torque; m_angularVelocity += m_invInertiaTensorWorld * torque * m_angularFactor;
} }
void applyImpulse(const btVector3& impulse, const btVector3& rel_pos) void applyImpulse(const btVector3& impulse, const btVector3& rel_pos)
@@ -287,7 +296,7 @@ public:
applyCentralImpulse(impulse); applyCentralImpulse(impulse);
if (m_angularFactor) if (m_angularFactor)
{ {
applyTorqueImpulse(rel_pos.cross(impulse)*m_angularFactor); applyTorqueImpulse(rel_pos.cross(impulse*m_linearFactor));
} }
} }
} }
@@ -297,10 +306,10 @@ public:
{ {
if (m_inverseMass != btScalar(0.)) if (m_inverseMass != btScalar(0.))
{ {
m_linearVelocity += linearComponent*impulseMagnitude; m_linearVelocity += linearComponent*m_linearFactor*impulseMagnitude;
if (m_angularFactor) if (m_angularFactor)
{ {
m_angularVelocity += angularComponent*impulseMagnitude*m_angularFactor; m_angularVelocity += angularComponent*m_angularFactor*impulseMagnitude;
} }
} }
} }
@@ -450,11 +459,16 @@ public:
int m_contactSolverType; int m_contactSolverType;
int m_frictionSolverType; int m_frictionSolverType;
void setAngularFactor(btScalar angFac) void setAngularFactor(const btVector3& angFac)
{ {
m_angularFactor = angFac; m_angularFactor = angFac;
} }
btScalar getAngularFactor() const
void setAngularFactor(btScalar angFac)
{
m_angularFactor.setValue(angFac,angFac,angFac);
}
const btVector3& getAngularFactor() const
{ {
return m_angularFactor; return m_angularFactor;
} }

10
src/BulletMultiThreaded/SpuSolverTask/SpuParallellSolverTask.cpp
View File

@@ -224,14 +224,14 @@ static void setupSpuBody (btCollisionObject* collisionObject, btSolverBody* solv
if (rb) if (rb)
{ {
solverBody->m_invMass = rb->getInvMass(); solverBody->m_invMass.setValue(rb->getInvMass(),rb->getInvMass(),rb->getInvMass());
solverBody->m_originalBody = rb; solverBody->m_originalBody = rb;
solverBody->m_angularFactor = rb->getAngularFactor(); solverBody->m_angularFactor = rb->getAngularFactor();
} else } else
{ {
solverBody->m_invMass = 0.f; solverBody->m_invMass.setValue(0,0,0);
solverBody->m_originalBody = 0; solverBody->m_originalBody = 0;
solverBody->m_angularFactor = 1.f; solverBody->m_angularFactor.setValue(1,1,1);
} }
} }
@@ -439,8 +439,8 @@ static void SpuResolveSingleConstraintRowGeneric(btSolverBody& body1,btSolverBod
__m128 upperMinApplied = _mm_sub_ps(upperLimit1,cpAppliedImp); __m128 upperMinApplied = _mm_sub_ps(upperLimit1,cpAppliedImp);
deltaImpulse = _mm_or_ps( _mm_and_ps(resultUpperLess, deltaImpulse), _mm_andnot_ps(resultUpperLess, upperMinApplied) ); deltaImpulse = _mm_or_ps( _mm_and_ps(resultUpperLess, deltaImpulse), _mm_andnot_ps(resultUpperLess, upperMinApplied) );
c.m_appliedImpulse = _mm_or_ps( _mm_and_ps(resultUpperLess, c.m_appliedImpulse), _mm_andnot_ps(resultUpperLess, upperLimit1) ); c.m_appliedImpulse = _mm_or_ps( _mm_and_ps(resultUpperLess, c.m_appliedImpulse), _mm_andnot_ps(resultUpperLess, upperLimit1) );
__m128 linearComponentA = _mm_mul_ps(c.m_contactNormal.mVec128,_mm_set1_ps(body1.m_invMass)); __m128 linearComponentA = _mm_mul_ps(c.m_contactNormal.mVec128,body1.m_invMass.mVec128);
__m128 linearComponentB = _mm_mul_ps(c.m_contactNormal.mVec128,_mm_set1_ps(body2.m_invMass)); __m128 linearComponentB = _mm_mul_ps(c.m_contactNormal.mVec128,body2.m_invMass.mVec128);
__m128 impulseMagnitude = deltaImpulse; __m128 impulseMagnitude = deltaImpulse;
body1.m_deltaLinearVelocity.mVec128 = _mm_add_ps(body1.m_deltaLinearVelocity.mVec128,_mm_mul_ps(linearComponentA,impulseMagnitude)); body1.m_deltaLinearVelocity.mVec128 = _mm_add_ps(body1.m_deltaLinearVelocity.mVec128,_mm_mul_ps(linearComponentA,impulseMagnitude));
body1.m_deltaAngularVelocity.mVec128 = _mm_add_ps(body1.m_deltaAngularVelocity.mVec128 ,_mm_mul_ps(c.m_angularComponentA.mVec128,impulseMagnitude)); body1.m_deltaAngularVelocity.mVec128 = _mm_add_ps(body1.m_deltaAngularVelocity.mVec128 ,_mm_mul_ps(c.m_angularComponentA.mVec128,impulseMagnitude));