diff --git a/Demos/ConstraintDemo/ConstraintDemo.cpp b/Demos/ConstraintDemo/ConstraintDemo.cpp
index c05a235d0..205e27635 100644
--- a/Demos/ConstraintDemo/ConstraintDemo.cpp
+++ b/Demos/ConstraintDemo/ConstraintDemo.cpp
@@ -30,6 +30,8 @@ subject to the following restrictions:
const int numObjects = 3;
+#define ENABLE_ALL_DEMOS 1
+
#define CUBE_HALF_EXTENTS 1.f
#define M_PI 3.1415926f
@@ -43,6 +45,8 @@ btVector3 hiSliderLimit = btVector3(10,0,0);
btRigidBody* d6body0 =0;
btHingeConstraint* spDoorHinge = NULL;
+btHingeConstraint* spHingeDynAB = NULL;
+btGeneric6DofConstraint* spSlider6Dof = NULL;
static bool s_bTestConeTwistMotor = false;
@@ -102,7 +106,7 @@ void ConstraintDemo::initPhysics()
trans.setOrigin(btVector3(0,20,0));
float mass = 1.f;
-#if 1
+#if ENABLE_ALL_DEMOS
//point to point constraint (ball socket)
{
btRigidBody* body0 = localCreateRigidBody( mass,trans,shape);
@@ -140,7 +144,7 @@ void ConstraintDemo::initPhysics()
}
#endif
-#if 1
+#if ENABLE_ALL_DEMOS
//create a slider, using the generic D6 constraint
{
mass = 1.f;
@@ -161,32 +165,34 @@ void ConstraintDemo::initPhysics()
btTransform frameInA, frameInB;
frameInA = btTransform::getIdentity();
frameInB = btTransform::getIdentity();
+ frameInA.setOrigin(btVector3(0., 5., 0.));
+ frameInB.setOrigin(btVector3(0., 5., 0.));
// bool useLinearReferenceFrameA = false;//use fixed frame B for linear llimits
bool useLinearReferenceFrameA = true;//use fixed frame A for linear llimits
- btGeneric6DofConstraint* slider = new btGeneric6DofConstraint(*fixedBody1, *d6body0,frameInA,frameInB,useLinearReferenceFrameA);
- slider->setLinearLowerLimit(lowerSliderLimit);
- slider->setLinearUpperLimit(hiSliderLimit);
+ spSlider6Dof = new btGeneric6DofConstraint(*fixedBody1, *d6body0,frameInA,frameInB,useLinearReferenceFrameA);
+ spSlider6Dof->setLinearLowerLimit(lowerSliderLimit);
+ spSlider6Dof->setLinearUpperLimit(hiSliderLimit);
//range should be small, otherwise singularities will 'explode' the constraint
-// slider->setAngularLowerLimit(btVector3(-1.5,0,0));
-// slider->setAngularUpperLimit(btVector3(1.5,0,0));
-// slider->setAngularLowerLimit(btVector3(0,0,0));
-// slider->setAngularUpperLimit(btVector3(0,0,0));
- slider->setAngularLowerLimit(btVector3(-SIMD_PI,0,0));
- slider->setAngularUpperLimit(btVector3(1.5,0,0));
+// spSlider6Dof->setAngularLowerLimit(btVector3(-1.5,0,0));
+// spSlider6Dof->setAngularUpperLimit(btVector3(1.5,0,0));
+// spSlider6Dof->setAngularLowerLimit(btVector3(0,0,0));
+// spSlider6Dof->setAngularUpperLimit(btVector3(0,0,0));
+ spSlider6Dof->setAngularLowerLimit(btVector3(-SIMD_PI,0,0));
+ spSlider6Dof->setAngularUpperLimit(btVector3(1.5,0,0));
- slider->getTranslationalLimitMotor()->m_enableMotor[0] = true;
- slider->getTranslationalLimitMotor()->m_targetVelocity[0] = -5.0f;
- slider->getTranslationalLimitMotor()->m_maxMotorForce[0] = 0.1f;
+ spSlider6Dof->getTranslationalLimitMotor()->m_enableMotor[0] = true;
+ spSlider6Dof->getTranslationalLimitMotor()->m_targetVelocity[0] = -5.0f;
+ spSlider6Dof->getTranslationalLimitMotor()->m_maxMotorForce[0] = 0.1f;
- m_dynamicsWorld->addConstraint(slider);
- slider->setDbgDrawSize(btScalar(5.f));
+ m_dynamicsWorld->addConstraint(spSlider6Dof);
+ spSlider6Dof->setDbgDrawSize(btScalar(5.f));
}
#endif
-#if 1
+#if ENABLE_ALL_DEMOS
{ // create a door using hinge constraint attached to the world
btCollisionShape* pDoorShape = new btBoxShape(btVector3(2.0f, 5.0f, 0.2f));
m_collisionShapes.push_back(pDoorShape);
@@ -195,7 +201,7 @@ void ConstraintDemo::initPhysics()
doorTrans.setOrigin(btVector3(-5.0f, -2.0f, 0.0f));
btRigidBody* pDoorBody = localCreateRigidBody( 1.0, doorTrans, pDoorShape);
pDoorBody->setActivationState(DISABLE_DEACTIVATION);
- const btVector3 btPivotA( 2.1f, -2.0f, 0.0f ); // right next to the door slightly outside
+ const btVector3 btPivotA(10.f + 2.1f, -2.0f, 0.0f ); // right next to the door slightly outside
btVector3 btAxisA( 0.0f, 1.0f, 0.0f ); // pointing upwards, aka Y-axis
spDoorHinge = new btHingeConstraint( *pDoorBody, btPivotA, btAxisA );
@@ -203,7 +209,12 @@ void ConstraintDemo::initPhysics()
// spDoorHinge->setLimit( 0.0f, M_PI_2 );
// test problem values
// spDoorHinge->setLimit( -M_PI, M_PI*0.8f);
- spDoorHinge->setLimit( -M_PI*0.8f, M_PI);
+
+// spDoorHinge->setLimit( 1.f, -1.f);
+// spDoorHinge->setLimit( -M_PI*0.8f, M_PI);
+// spDoorHinge->setLimit( -M_PI*0.8f, M_PI, 0.9f, 0.3f, 0.0f);
+// spDoorHinge->setLimit( -M_PI*0.8f, M_PI, 0.9f, 0.01f, 0.0f); // "sticky limits"
+ spDoorHinge->setLimit( -M_PI * 0.25f, M_PI * 0.25f );
// spDoorHinge->setLimit( 0.0f, 0.0f );
m_dynamicsWorld->addConstraint(spDoorHinge);
spDoorHinge->setDbgDrawSize(btScalar(5.f));
@@ -212,7 +223,7 @@ void ConstraintDemo::initPhysics()
//btRigidBody* pDropBody = localCreateRigidBody( 10.0, doorTrans, shape);
}
#endif
-#if 1
+#if ENABLE_ALL_DEMOS
{ // create a generic 6DOF constraint
btTransform tr;
@@ -227,7 +238,8 @@ void ConstraintDemo::initPhysics()
tr.setIdentity();
tr.setOrigin(btVector3(btScalar(0.), btScalar(6.), btScalar(0.)));
tr.getBasis().setEulerZYX(0,0,0);
- btRigidBody* pBodyB = localCreateRigidBody(1.0, tr, shape);
+ btRigidBody* pBodyB = localCreateRigidBody(mass, tr, shape);
+// btRigidBody* pBodyB = localCreateRigidBody(0.f, tr, shape);
pBodyB->setActivationState(DISABLE_DEACTIVATION);
btTransform frameInA, frameInB;
@@ -240,7 +252,7 @@ void ConstraintDemo::initPhysics()
// btGeneric6DofConstraint* pGen6DOF = new btGeneric6DofConstraint(*pBodyA, *pBodyB, frameInA, frameInB, false);
pGen6DOF->setLinearLowerLimit(btVector3(-10., -2., -1.));
pGen6DOF->setLinearUpperLimit(btVector3(10., 2., 1.));
- pGen6DOF->setLinearLowerLimit(btVector3(-10., 0., 0.));
+// pGen6DOF->setLinearLowerLimit(btVector3(-10., 0., 0.));
// pGen6DOF->setLinearUpperLimit(btVector3(10., 0., 0.));
// pGen6DOF->setLinearLowerLimit(btVector3(0., 0., 0.));
// pGen6DOF->setLinearUpperLimit(btVector3(0., 0., 0.));
@@ -268,12 +280,16 @@ void ConstraintDemo::initPhysics()
// pGen6DOF->setAngularUpperLimit(btVector3(0.75,0.5, 0.5));
// pGen6DOF->setAngularLowerLimit(btVector3(-0.75,0., 0.));
// pGen6DOF->setAngularUpperLimit(btVector3(0.75,0., 0.));
+// pGen6DOF->setAngularLowerLimit(btVector3(0., -0.7,0.));
+// pGen6DOF->setAngularUpperLimit(btVector3(0., 0.7, 0.));
+// pGen6DOF->setAngularLowerLimit(btVector3(-1., 0.,0.));
+// pGen6DOF->setAngularUpperLimit(btVector3(1., 0., 0.));
m_dynamicsWorld->addConstraint(pGen6DOF, true);
pGen6DOF->setDbgDrawSize(btScalar(5.f));
}
#endif
-#if 1
+#if ENABLE_ALL_DEMOS
{ // create a ConeTwist constraint
btTransform tr;
@@ -308,7 +324,7 @@ void ConstraintDemo::initPhysics()
s_bTestConeTwistMotor = false;
}
#endif
-#if 1
+#if ENABLE_ALL_DEMOS
{ // Hinge connected to the world, with motor (to hinge motor with new and old constraint solver)
btTransform tr;
tr.setIdentity();
@@ -326,7 +342,7 @@ void ConstraintDemo::initPhysics()
}
#endif
-#if 1
+#if ENABLE_ALL_DEMOS
{
// create a universal joint using generic 6DOF constraint
// create two rigid bodies
@@ -356,7 +372,7 @@ void ConstraintDemo::initPhysics()
}
#endif
-#if 1
+#if ENABLE_ALL_DEMOS
{ // create a generic 6DOF constraint with springs
btTransform tr;
@@ -397,7 +413,7 @@ void ConstraintDemo::initPhysics()
pGen6DOFSpring->setEquilibriumPoint();
}
#endif
-#if 1
+#if ENABLE_ALL_DEMOS
{
// create a Hinge2 joint
// create two rigid bodies
@@ -425,7 +441,34 @@ void ConstraintDemo::initPhysics()
pHinge2->setDbgDrawSize(btScalar(5.f));
}
#endif
-
+#if ENABLE_ALL_DEMOS
+ {
+ // create a Hinge joint between two dynamic bodies
+ // create two rigid bodies
+ // static bodyA (parent) on top:
+ btTransform tr;
+ tr.setIdentity();
+ tr.setOrigin(btVector3(btScalar(-20.), btScalar(-2.), btScalar(0.)));
+ btRigidBody* pBodyA = localCreateRigidBody( 1.0f, tr, shape);
+ pBodyA->setActivationState(DISABLE_DEACTIVATION);
+ // dynamic bodyB:
+ tr.setIdentity();
+ tr.setOrigin(btVector3(btScalar(-30.), btScalar(-2.), btScalar(0.)));
+ btRigidBody* pBodyB = localCreateRigidBody(10.0, tr, shape);
+ pBodyB->setActivationState(DISABLE_DEACTIVATION);
+ // add some data to build constraint frames
+ btVector3 axisA(0.f, 1.f, 0.f);
+ btVector3 axisB(0.f, 1.f, 0.f);
+ btVector3 pivotA(-5.f, 0.f, 0.f);
+ btVector3 pivotB( 5.f, 0.f, 0.f);
+ spHingeDynAB = new btHingeConstraint(*pBodyA, *pBodyB, pivotA, pivotB, axisA, axisB);
+ spHingeDynAB->setLimit(-SIMD_HALF_PI * 0.5f, SIMD_HALF_PI * 0.5f);
+ // add constraint to world
+ m_dynamicsWorld->addConstraint(spHingeDynAB, true);
+ // draw constraint frames and limits for debugging
+ spHingeDynAB->setDbgDrawSize(btScalar(5.f));
+ }
+#endif
}
ConstraintDemo::~ConstraintDemo()
@@ -570,3 +613,42 @@ void ConstraintDemo::displayCallback(void) {
}
+void ConstraintDemo::keyboardCallback(unsigned char key, int x, int y)
+{
+ (void)x;
+ (void)y;
+ switch (key)
+ {
+ case 'O' :
+ {
+ bool offectOnOff;
+ if(spDoorHinge)
+ {
+ offectOnOff = spDoorHinge->getUseFrameOffset();
+ offectOnOff = !offectOnOff;
+ spDoorHinge->setUseFrameOffset(offectOnOff);
+ printf("DoorHinge %s frame offset\n", offectOnOff ? "uses" : "does not use");
+ }
+ if(spHingeDynAB)
+ {
+ offectOnOff = spHingeDynAB->getUseFrameOffset();
+ offectOnOff = !offectOnOff;
+ spHingeDynAB->setUseFrameOffset(offectOnOff);
+ printf("HingeDynAB %s frame offset\n", offectOnOff ? "uses" : "does not use");
+ }
+ if(spSlider6Dof)
+ {
+ offectOnOff = spSlider6Dof->getUseFrameOffset();
+ offectOnOff = !offectOnOff;
+ spSlider6Dof->setUseFrameOffset(offectOnOff);
+ printf("Slider6Dof %s frame offset\n", offectOnOff ? "uses" : "does not use");
+ }
+ }
+ break;
+ default :
+ {
+ DemoApplication::keyboardCallback(key, x, y);
+ }
+ break;
+ }
+}
diff --git a/Demos/ConstraintDemo/ConstraintDemo.h b/Demos/ConstraintDemo/ConstraintDemo.h
index 18d95507d..b7fde5916 100644
--- a/Demos/ConstraintDemo/ConstraintDemo.h
+++ b/Demos/ConstraintDemo/ConstraintDemo.h
@@ -50,6 +50,8 @@ class ConstraintDemo : public GlutDemoApplication
return demo;
}
+ virtual void keyboardCallback(unsigned char key, int x, int y);
+
// for cone-twist motor driving
float m_Time;
class btConeTwistConstraint* m_ctc;
diff --git a/Demos/OpenGL/DemoApplication.h b/Demos/OpenGL/DemoApplication.h
index 4a7826ad4..7300e5256 100644
--- a/Demos/OpenGL/DemoApplication.h
+++ b/Demos/OpenGL/DemoApplication.h
@@ -151,7 +151,7 @@ public:
m_forwardAxis = axis;
}
- void myinit();
+ virtual void myinit();
void toggleIdle();
diff --git a/Demos/SliderConstraintDemo/SliderConstraintDemo.cpp b/Demos/SliderConstraintDemo/SliderConstraintDemo.cpp
index 345d42706..f7a584fba 100755
--- a/Demos/SliderConstraintDemo/SliderConstraintDemo.cpp
+++ b/Demos/SliderConstraintDemo/SliderConstraintDemo.cpp
@@ -43,6 +43,8 @@ April 24, 2008
#define CUBE_HALF_EXTENTS 1.f
+#define SLIDER_ENABLE_ALL_DEMOS 1
+
// A couple of sliders
@@ -166,7 +168,7 @@ void SliderConstraintDemo::initPhysics()
m_collisionShapes.push_back(groundShape);
btTransform groundTransform;
groundTransform.setIdentity();
- groundTransform.setOrigin(btVector3(0,-56,0));
+ groundTransform.setOrigin(btVector3(0,-76,0));
btRigidBody* groundBody = localCreateRigidBody(0, groundTransform, groundShape);
// add box shape (will be reused for all bodies)
@@ -178,21 +180,25 @@ void SliderConstraintDemo::initPhysics()
// add dynamic rigid body A1
btTransform trans;
trans.setIdentity();
- btVector3 worldPos(0,0,0);
+ btVector3 worldPos(-20,0,0);
trans.setOrigin(worldPos);
- btRigidBody* pRbA1 = localCreateRigidBody(mass, trans, shape);
- pRbA1->setActivationState(DISABLE_DEACTIVATION);
-
- // add dynamic rigid body B1
- worldPos.setValue(-10,0,0);
- trans.setOrigin(worldPos);
- btRigidBody* pRbB1 = localCreateRigidBody(mass, trans, shape);
- pRbB1->setActivationState(DISABLE_DEACTIVATION);
-
- // create slider constraint between A1 and B1 and add it to world
btTransform frameInA, frameInB;
frameInA = btTransform::getIdentity();
frameInB = btTransform::getIdentity();
+
+#if SLIDER_ENABLE_ALL_DEMOS
+ btRigidBody* pRbA1 = localCreateRigidBody(mass, trans, shape);
+// btRigidBody* pRbA1 = localCreateRigidBody(0.f, trans, shape);
+ pRbA1->setActivationState(DISABLE_DEACTIVATION);
+
+ // add dynamic rigid body B1
+ worldPos.setValue(-30,0,0);
+ trans.setOrigin(worldPos);
+ btRigidBody* pRbB1 = localCreateRigidBody(mass, trans, shape);
+// btRigidBody* pRbB1 = localCreateRigidBody(0.f, trans, shape);
+ pRbB1->setActivationState(DISABLE_DEACTIVATION);
+
+ // create slider constraint between A1 and B1 and add it to world
#if SLIDER_DEMO_USE_6DOF
spSlider1 = new btGeneric6DofConstraint(*pRbA1, *pRbB1, frameInA, frameInB, true);
@@ -220,19 +226,39 @@ void SliderConstraintDemo::initPhysics()
m_dynamicsWorld->addConstraint(spSlider1, true);
spSlider1->setDbgDrawSize(btScalar(5.f));
+#endif
+#if SLIDER_ENABLE_ALL_DEMOS
// add kinematic rigid body A2
- worldPos.setValue(0,2,0);
+// worldPos.setValue(20,4,0);
+ worldPos.setValue(5,-20,0);
trans.setOrigin(worldPos);
btRigidBody* pRbA2 = localCreateRigidBody(0., trans, shape);
+// btRigidBody* pRbA2 = localCreateRigidBody(mass, trans, shape);
+// btRigidBody* pRbA2 = localCreateRigidBody(mass * 10000, trans, shape);
pRbA2->setActivationState(DISABLE_DEACTIVATION);
// add dynamic rigid body B2
- worldPos.setValue(-10,2,0);
+// worldPos.setValue(-20,4,0);
+ worldPos.setValue(-5,-20,0);
trans.setOrigin(worldPos);
+// btRigidBody* pRbB2 = localCreateRigidBody(0., trans, shape);
btRigidBody* pRbB2 = localCreateRigidBody(mass, trans, shape);
+// btRigidBody* pRbB2 = localCreateRigidBody(mass * 10000, trans, shape);
pRbB2->setActivationState(DISABLE_DEACTIVATION);
+// frameInA.getBasis().setEulerZYX(1.f, 1.f, 1.f);
+// frameInB.getBasis().setEulerZYX(1.f, 1.f, 1.f);
+// frameInA.getBasis().setEulerZYX(1.f, 1.f, 1.f);
+// frameInB.getBasis().setEulerZYX(1.f, 1.f, 1.f);
+
+
+// frameInA.setOrigin(btVector3(-20., 5., 0));
+// frameInB.setOrigin(btVector3( 20., 5., 0));
+ frameInA.setOrigin(btVector3(-5., 20., 0));
+ frameInB.setOrigin(btVector3( 5., 20., 0));
+
+
// create slider constraint between A2 and B2 and add it to world
#if SLIDER_DEMO_USE_6DOF
spSlider2 = new btGeneric6DofConstraint(*pRbA2, *pRbB2, frameInA, frameInB, true);
@@ -243,8 +269,10 @@ void SliderConstraintDemo::initPhysics()
#else
spSlider2 = new btSliderConstraint(*pRbA2, *pRbB2, frameInA, frameInB, true);
// spSlider2 = new btSliderConstraint(*pRbA2, *pRbB2, frameInA, frameInB, false);
- spSlider2->setLowerLinLimit(-25.0F);
- spSlider2->setUpperLinLimit(-5.0F);
+// spSlider2->setLowerLinLimit(0.0F);
+// spSlider2->setUpperLinLimit(0.0F);
+ spSlider2->setLowerLinLimit(-2.0F);
+ spSlider2->setUpperLinLimit(2.0F);
// spSlider2->setLowerLinLimit(5.0F);
// spSlider2->setUpperLinLimit(25.0F);
// spSlider2->setUpperLinLimit(-5.0F);
@@ -256,12 +284,17 @@ void SliderConstraintDemo::initPhysics()
// spSlider2->setLowerAngLimit(-SIMD_PI / 2.0F);
// spSlider2->setUpperAngLimit(SIMD_PI / 2.0F);
- spSlider2->setLowerAngLimit(-SIMD_PI);
- spSlider2->setUpperAngLimit(SIMD_PI *0.8F);
+
+// spSlider2->setLowerAngLimit(-SIMD_PI);
+// spSlider2->setUpperAngLimit(SIMD_PI *0.8F);
// spSlider2->setLowerAngLimit(-0.01F);
// spSlider2->setUpperAngLimit(0.01F);
+ spSlider2->setLowerAngLimit(-1.570796326F * 0.5f);
+ spSlider2->setUpperAngLimit(1.570796326F * 0.5f);
+// spSlider2->setLowerAngLimit(1.F);
+// spSlider2->setUpperAngLimit(-1.F);
// spSlider2->setDampingLimLin(0.5f);
@@ -293,22 +326,22 @@ void SliderConstraintDemo::initPhysics()
#endif
m_dynamicsWorld->addConstraint(spSlider2, true);
spSlider2->setDbgDrawSize(btScalar(5.f));
+#endif
-
-#if 1
+#if SLIDER_ENABLE_ALL_DEMOS
{
// add dynamic rigid body A1
trans.setIdentity();
worldPos.setValue(20,0,0);
trans.setOrigin(worldPos);
btRigidBody* pRbA3 = localCreateRigidBody(0.0F, trans, shape);
- pRbA1->setActivationState(DISABLE_DEACTIVATION);
+ pRbA3->setActivationState(DISABLE_DEACTIVATION);
// add dynamic rigid body B1
worldPos.setValue(25,0,0);
trans.setOrigin(worldPos);
btRigidBody* pRbB3 = localCreateRigidBody(mass, trans, shape);
- pRbB1->setActivationState(DISABLE_DEACTIVATION);
+ pRbB3->setActivationState(DISABLE_DEACTIVATION);
btVector3 pivA( 2.5, 0., 0.);
btVector3 pivB(-2.5, 0., 0.);
@@ -319,7 +352,7 @@ void SliderConstraintDemo::initPhysics()
}
#endif
-#if 1
+#if 0 // SLIDER_ENABLE_ALL_DEMOS
// add dynamic rigid body A4
trans.setIdentity();
worldPos.setValue(20,10,0);
@@ -449,3 +482,30 @@ void SliderConstraintDemo::displayCallback(void)
} // SliderConstraintDemo::displayCallback()
+void SliderConstraintDemo::keyboardCallback(unsigned char key, int x, int y)
+{
+ (void)x;
+ (void)y;
+ switch (key)
+ {
+ case 'O' :
+ {
+ bool offectOnOff;
+ offectOnOff = spSlider1->getUseFrameOffset();
+ offectOnOff = !offectOnOff;
+ spSlider1->setUseFrameOffset(offectOnOff);
+ printf("Slider1 %s frame offset\n", offectOnOff ? "uses" : "does not use");
+ offectOnOff = spSlider2->getUseFrameOffset();
+ offectOnOff = !offectOnOff;
+ spSlider2->setUseFrameOffset(offectOnOff);
+ printf("Slider2 %s frame offset\n", offectOnOff ? "uses" : "does not use");
+ }
+ break;
+ default :
+ {
+ DemoApplication::keyboardCallback(key, x, y);
+ }
+ break;
+ }
+}
+
diff --git a/Demos/SliderConstraintDemo/SliderConstraintDemo.h b/Demos/SliderConstraintDemo/SliderConstraintDemo.h
index 18fd39975..f0a652a22 100755
--- a/Demos/SliderConstraintDemo/SliderConstraintDemo.h
+++ b/Demos/SliderConstraintDemo/SliderConstraintDemo.h
@@ -64,6 +64,8 @@ class SliderConstraintDemo : public GlutDemoApplication
demo->initPhysics();
return demo;
}
+
+ virtual void keyboardCallback(unsigned char key, int x, int y);
};
diff --git a/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp b/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp
index b24f35615..4c2de0b81 100644
--- a/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp
+++ b/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp
@@ -28,11 +28,13 @@ http://gimpact.sf.net
#define D6_USE_OBSOLETE_METHOD false
+#define D6_USE_FRAME_OFFSET true
btGeneric6DofConstraint::btGeneric6DofConstraint()
:btTypedConstraint(D6_CONSTRAINT_TYPE),
m_useLinearReferenceFrameA(true),
+m_useOffsetForConstraintFrame(D6_USE_FRAME_OFFSET),
m_useSolveConstraintObsolete(D6_USE_OBSOLETE_METHOD)
{
}
@@ -44,6 +46,7 @@ btGeneric6DofConstraint::btGeneric6DofConstraint(btRigidBody& rbA, btRigidBody&
, m_frameInA(frameInA)
, m_frameInB(frameInB),
m_useLinearReferenceFrameA(useLinearReferenceFrameA),
+m_useOffsetForConstraintFrame(D6_USE_FRAME_OFFSET),
m_useSolveConstraintObsolete(D6_USE_OBSOLETE_METHOD)
{
@@ -384,6 +387,22 @@ void btGeneric6DofConstraint::calculateTransforms(const btTransform& transA,cons
m_calculatedTransformB = transB * m_frameInB;
calculateLinearInfo();
calculateAngleInfo();
+ if(m_useOffsetForConstraintFrame)
+ { // get weight factors depending on masses
+ btScalar miA = getRigidBodyA().getInvMass();
+ btScalar miB = getRigidBodyB().getInvMass();
+ m_hasStaticBody = (miA < SIMD_EPSILON) || (miB < SIMD_EPSILON);
+ btScalar miS = miA + miB;
+ if(miS > btScalar(0.f))
+ {
+ m_factA = miB / miS;
+ }
+ else
+ {
+ m_factA = btScalar(0.5f);
+ }
+ m_factB = btScalar(1.0f) - m_factA;
+ }
}
@@ -550,37 +569,25 @@ void btGeneric6DofConstraint::getInfo2 (btConstraintInfo2* info)
void btGeneric6DofConstraint::getInfo2NonVirtual (btConstraintInfo2* info, const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB)
{
btAssert(!m_useSolveConstraintObsolete);
-
//prepare constraint
calculateTransforms(transA,transB);
-
- int i;
- //test linear limits
- for(i = 0; i < 3; i++)
- {
- if(m_linearLimits.needApplyForce(i))
- {
-
- }
+ if(m_useOffsetForConstraintFrame)
+ { // for stability better to solve angular limits first
+ int row = setAngularLimits(info, 0,transA,transB,linVelA,linVelB,angVelA,angVelB);
+ setLinearLimits(info, row, transA,transB,linVelA,linVelB,angVelA,angVelB);
}
- //test angular limits
- for (i=0;i<3 ;i++ )
- {
- if(testAngularLimitMotor(i))
- {
-
- }
+ else
+ { // leave old version for compatibility
+ int row = setLinearLimits(info, 0, transA,transB,linVelA,linVelB,angVelA,angVelB);
+ setAngularLimits(info, row,transA,transB,linVelA,linVelB,angVelA,angVelB);
}
-
- int row = setLinearLimits(info,transA,transB,linVelA,linVelB,angVelA,angVelB);
- setAngularLimits(info, row,transA,transB,linVelA,linVelB,angVelA,angVelB);
}
-int btGeneric6DofConstraint::setLinearLimits(btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB)
+int btGeneric6DofConstraint::setLinearLimits(btConstraintInfo2* info, int row, const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB)
{
- int row = 0;
+// int row = 0;
//solve linear limits
btRotationalLimitMotor limot;
for (int i=0;i<3 ;i++ )
@@ -601,9 +608,21 @@ int btGeneric6DofConstraint::setLinearLimits(btConstraintInfo2* info,const btTra
limot.m_maxMotorForce = m_linearLimits.m_maxMotorForce[i];
limot.m_targetVelocity = m_linearLimits.m_targetVelocity[i];
btVector3 axis = m_calculatedTransformA.getBasis().getColumn(i);
- row += get_limit_motor_info2(&limot,
- transA,transB,linVelA,linVelB,angVelA,angVelB
- , info, row, axis, 0);
+ if(m_useOffsetForConstraintFrame)
+ {
+ int indx1 = (i + 1) % 3;
+ int indx2 = (i + 2) % 3;
+ int rotAllowed = 1; // rotations around orthos to current axis
+ if(m_angularLimits[indx1].m_currentLimit && m_angularLimits[indx2].m_currentLimit)
+ {
+ rotAllowed = 0;
+ }
+ row += get_limit_motor_info2UsingFrameOffset(&limot, transA,transB,linVelA,linVelB,angVelA,angVelB, info, row, axis, 0, rotAllowed);
+ }
+ else
+ {
+ row += get_limit_motor_info2(&limot, transA,transB,linVelA,linVelB,angVelA,angVelB, info, row, axis, 0);
+ }
}
}
return row;
@@ -621,10 +640,8 @@ int btGeneric6DofConstraint::setAngularLimits(btConstraintInfo2 *info, int row_o
if(d6constraint->getRotationalLimitMotor(i)->needApplyTorques())
{
btVector3 axis = d6constraint->getAxis(i);
- row += get_limit_motor_info2(
- d6constraint->getRotationalLimitMotor(i),
- transA,transB,linVelA,linVelB,angVelA,angVelB,
- info,row,axis,1);
+ row += get_limit_motor_info2(d6constraint->getRotationalLimitMotor(i),
+ transA,transB,linVelA,linVelB,angVelA,angVelB, info,row,axis,1);
}
}
@@ -890,4 +907,151 @@ int btGeneric6DofConstraint::get_limit_motor_info2(
+int btGeneric6DofConstraint::get_limit_motor_info2UsingFrameOffset( btRotationalLimitMotor * limot,
+ const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB,
+ btConstraintInfo2 *info, int row, btVector3& ax1, int rotational, int rotAllowed)
+{
+ int srow = row * info->rowskip;
+ int powered = limot->m_enableMotor;
+ int limit = limot->m_currentLimit;
+ if (powered || limit)
+ { // if the joint is powered, or has joint limits, add in the extra row
+ btScalar *J1 = rotational ? info->m_J1angularAxis : info->m_J1linearAxis;
+ btScalar *J2 = rotational ? info->m_J2angularAxis : 0;
+ J1[srow+0] = ax1[0];
+ J1[srow+1] = ax1[1];
+ J1[srow+2] = ax1[2];
+ if(rotational)
+ {
+ J2[srow+0] = -ax1[0];
+ J2[srow+1] = -ax1[1];
+ J2[srow+2] = -ax1[2];
+ }
+ if((!rotational))
+ {
+ btVector3 tmpA, tmpB, relA, relB;
+ // get vector from bodyB to frameB in WCS
+ relB = m_calculatedTransformB.getOrigin() - transB.getOrigin();
+ // get its projection to constraint axis
+ btVector3 projB = ax1 * relB.dot(ax1);
+ // get vector directed from bodyB to constraint axis (and orthogonal to it)
+ btVector3 orthoB = relB - projB;
+ // same for bodyA
+ relA = m_calculatedTransformA.getOrigin() - transA.getOrigin();
+ btVector3 projA = ax1 * relA.dot(ax1);
+ btVector3 orthoA = relA - projA;
+ // get desired offset between frames A and B along constraint axis
+ btScalar desiredOffs = limot->m_currentPosition - limot->m_currentLimitError;
+ // desired vector from projection of center of bodyA to projection of center of bodyB to constraint axis
+ btVector3 totalDist = projA + ax1 * desiredOffs - projB;
+ // get offset vectors relA and relB
+ relA = orthoA + totalDist * m_factA;
+ relB = orthoB - totalDist * m_factB;
+ tmpA = relA.cross(ax1);
+ tmpB = relB.cross(ax1);
+ if(m_hasStaticBody && (!rotAllowed))
+ {
+ tmpA *= m_factA;
+ tmpB *= m_factB;
+ }
+ int i;
+ for (i=0; i<3; i++) info->m_J1angularAxis[srow+i] = tmpA[i];
+ for (i=0; i<3; i++) info->m_J2angularAxis[srow+i] = -tmpB[i];
+ }
+ // if we're limited low and high simultaneously, the joint motor is
+ // ineffective
+ if (limit && (limot->m_loLimit == limot->m_hiLimit)) powered = 0;
+ info->m_constraintError[srow] = btScalar(0.f);
+ if (powered)
+ {
+ info->cfm[srow] = 0.0f;
+ if(!limit)
+ {
+ btScalar tag_vel = rotational ? limot->m_targetVelocity : -limot->m_targetVelocity;
+
+ btScalar mot_fact = getMotorFactor( limot->m_currentPosition,
+ limot->m_loLimit,
+ limot->m_hiLimit,
+ tag_vel,
+ info->fps * info->erp);
+ info->m_constraintError[srow] += mot_fact * limot->m_targetVelocity;
+ info->m_lowerLimit[srow] = -limot->m_maxMotorForce;
+ info->m_upperLimit[srow] = limot->m_maxMotorForce;
+ }
+ }
+ if(limit)
+ {
+ btScalar k = info->fps * limot->m_ERP;
+ if(!rotational)
+ {
+ info->m_constraintError[srow] += k * limot->m_currentLimitError;
+ }
+ else
+ {
+ info->m_constraintError[srow] += -k * limot->m_currentLimitError;
+ }
+ info->cfm[srow] = 0.0f;
+ if (limot->m_loLimit == limot->m_hiLimit)
+ { // limited low and high simultaneously
+ info->m_lowerLimit[srow] = -SIMD_INFINITY;
+ info->m_upperLimit[srow] = SIMD_INFINITY;
+ }
+ else
+ {
+ if (limit == 1)
+ {
+ info->m_lowerLimit[srow] = 0;
+ info->m_upperLimit[srow] = SIMD_INFINITY;
+ }
+ else
+ {
+ info->m_lowerLimit[srow] = -SIMD_INFINITY;
+ info->m_upperLimit[srow] = 0;
+ }
+ // deal with bounce
+ if (limot->m_bounce > 0)
+ {
+ // calculate joint velocity
+ btScalar vel;
+ if (rotational)
+ {
+ vel = angVelA.dot(ax1);
+//make sure that if no body -> angVelB == zero vec
+// if (body1)
+ vel -= angVelB.dot(ax1);
+ }
+ else
+ {
+ vel = linVelA.dot(ax1);
+//make sure that if no body -> angVelB == zero vec
+// if (body1)
+ vel -= linVelB.dot(ax1);
+ }
+ // only apply bounce if the velocity is incoming, and if the
+ // resulting c[] exceeds what we already have.
+ if (limit == 1)
+ {
+ if (vel < 0)
+ {
+ btScalar newc = -limot->m_bounce* vel;
+ if (newc > info->m_constraintError[srow])
+ info->m_constraintError[srow] = newc;
+ }
+ }
+ else
+ {
+ if (vel > 0)
+ {
+ btScalar newc = -limot->m_bounce * vel;
+ if (newc < info->m_constraintError[srow])
+ info->m_constraintError[srow] = newc;
+ }
+ }
+ }
+ }
+ }
+ return 1;
+ }
+ else return 0;
+}
diff --git a/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h b/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h
index 3d1936da3..67edd04b7 100644
--- a/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h
+++ b/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h
@@ -221,20 +221,22 @@ This brings support for limit parameters and motors.
Angulars limits have these possible ranges:
-
| AXIS |
MIN ANGLE |
MAX ANGLE |
+
| X |
- -PI |
- PI |
+ -PI |
+ PI |
+
| Y |
- -PI/2 |
- PI/2 |
+ -PI/2 |
+ PI/2 |
+
| Z |
- -PI/2 |
- PI/2 |
+ -PI |
+ PI |
@@ -278,10 +280,14 @@ protected:
btVector3 m_calculatedAxisAngleDiff;
btVector3 m_calculatedAxis[3];
btVector3 m_calculatedLinearDiff;
+ btScalar m_factA;
+ btScalar m_factB;
+ bool m_hasStaticBody;
btVector3 m_AnchorPos; // point betwen pivots of bodies A and B to solve linear axes
bool m_useLinearReferenceFrameA;
+ bool m_useOffsetForConstraintFrame;
//!@}
@@ -295,7 +301,7 @@ protected:
int setAngularLimits(btConstraintInfo2 *info, int row_offset,const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB);
- int setLinearLimits(btConstraintInfo2 *info,const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB);
+ int setLinearLimits(btConstraintInfo2 *info, int row, const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB);
void buildLinearJacobian(
btJacobianEntry & jacLinear,const btVector3 & normalWorld,
@@ -485,7 +491,13 @@ public:
const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB,
btConstraintInfo2 *info, int row, btVector3& ax1, int rotational);
+ int get_limit_motor_info2UsingFrameOffset( btRotationalLimitMotor * limot,
+ const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB,
+ btConstraintInfo2 *info, int row, btVector3& ax1, int rotational, int rotAllowed);
+ // access for UseFrameOffset
+ bool getUseFrameOffset() { return m_useOffsetForConstraintFrame; }
+ void setUseFrameOffset(bool frameOffsetOnOff) { m_useOffsetForConstraintFrame = frameOffsetOnOff; }
};
diff --git a/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp b/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp
index 2cd7b1c41..d7b59610e 100644
--- a/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp
+++ b/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp
@@ -23,8 +23,10 @@ subject to the following restrictions:
+//#define HINGE_USE_OBSOLETE_SOLVER false
#define HINGE_USE_OBSOLETE_SOLVER false
+#define HINGE_USE_FRAME_OFFSET true
#ifndef __SPU__
@@ -32,6 +34,7 @@ btHingeConstraint::btHingeConstraint()
: btTypedConstraint (HINGE_CONSTRAINT_TYPE),
m_enableAngularMotor(false),
m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
+m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET),
m_useReferenceFrameA(false)
{
m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f);
@@ -45,6 +48,7 @@ btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const bt
m_angularOnly(false),
m_enableAngularMotor(false),
m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
+ m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET),
m_useReferenceFrameA(useReferenceFrameA)
{
m_rbAFrame.getOrigin() = pivotInA;
@@ -93,6 +97,7 @@ btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const bt
btHingeConstraint::btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,btVector3& axisInA, bool useReferenceFrameA)
:btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA), m_angularOnly(false), m_enableAngularMotor(false),
m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
+m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET),
m_useReferenceFrameA(useReferenceFrameA)
{
@@ -136,6 +141,7 @@ btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB,
m_angularOnly(false),
m_enableAngularMotor(false),
m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
+m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET),
m_useReferenceFrameA(useReferenceFrameA)
{
//start with free
@@ -155,6 +161,7 @@ btHingeConstraint::btHingeConstraint(btRigidBody& rbA, const btTransform& rbAFra
m_angularOnly(false),
m_enableAngularMotor(false),
m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
+m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET),
m_useReferenceFrameA(useReferenceFrameA)
{
///not providing rigidbody B means implicitly using worldspace for body B
@@ -460,7 +467,14 @@ void btHingeConstraint::getInfo1NonVirtual(btConstraintInfo1* info)
void btHingeConstraint::getInfo2 (btConstraintInfo2* info)
{
- getInfo2Internal(info, m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(),m_rbA.getAngularVelocity(),m_rbB.getAngularVelocity());
+ if(m_useOffsetForConstraintFrame)
+ {
+ getInfo2InternalUsingFrameOffset(info, m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(),m_rbA.getAngularVelocity(),m_rbB.getAngularVelocity());
+ }
+ else
+ {
+ getInfo2Internal(info, m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(),m_rbA.getAngularVelocity(),m_rbB.getAngularVelocity());
+ }
}
@@ -810,3 +824,253 @@ void btHingeConstraint::setMotorTarget(btScalar targetAngle, btScalar dt)
}
+
+void btHingeConstraint::getInfo2InternalUsingFrameOffset(btConstraintInfo2* info, const btTransform& transA,const btTransform& transB,const btVector3& angVelA,const btVector3& angVelB)
+{
+ btAssert(!m_useSolveConstraintObsolete);
+ int i, s = info->rowskip;
+ // transforms in world space
+ btTransform trA = transA*m_rbAFrame;
+ btTransform trB = transB*m_rbBFrame;
+ // pivot point
+ btVector3 pivotAInW = trA.getOrigin();
+ btVector3 pivotBInW = trB.getOrigin();
+#if 1
+ // difference between frames in WCS
+ btVector3 ofs = trB.getOrigin() - trA.getOrigin();
+ // now get weight factors depending on masses
+ btScalar miA = getRigidBodyA().getInvMass();
+ btScalar miB = getRigidBodyB().getInvMass();
+ bool hasStaticBody = (miA < SIMD_EPSILON) || (miB < SIMD_EPSILON);
+ btScalar miS = miA + miB;
+ btScalar factA, factB;
+ if(miS > btScalar(0.f))
+ {
+ factA = miB / miS;
+ }
+ else
+ {
+ factA = btScalar(0.5f);
+ }
+ factB = btScalar(1.0f) - factA;
+ // get the desired direction of hinge axis
+ // as weighted sum of Z-orthos of frameA and frameB in WCS
+ btVector3 ax1A = trA.getBasis().getColumn(2);
+ btVector3 ax1B = trB.getBasis().getColumn(2);
+ btVector3 ax1 = ax1A * factA + ax1B * factB;
+ ax1.normalize();
+ // fill first 3 rows
+ // we want: velA + wA x relA == velB + wB x relB
+ btTransform bodyA_trans = transA;
+ btTransform bodyB_trans = transB;
+ int s0 = 0;
+ int s1 = s;
+ int s2 = s * 2;
+ int nrow = 2; // last filled row
+ btVector3 tmpA, tmpB, relA, relB, p, q;
+ // get vector from bodyB to frameB in WCS
+ relB = trB.getOrigin() - bodyB_trans.getOrigin();
+ // get its projection to hinge axis
+ btVector3 projB = ax1 * relB.dot(ax1);
+ // get vector directed from bodyB to hinge axis (and orthogonal to it)
+ btVector3 orthoB = relB - projB;
+ // same for bodyA
+ relA = trA.getOrigin() - bodyA_trans.getOrigin();
+ btVector3 projA = ax1 * relA.dot(ax1);
+ btVector3 orthoA = relA - projA;
+ btVector3 totalDist = projA - projB;
+ // get offset vectors relA and relB
+ relA = orthoA + totalDist * factA;
+ relB = orthoB - totalDist * factB;
+ // now choose average ortho to hinge axis
+ p = orthoB * factA + orthoA * factB;
+ btScalar len2 = p.length2();
+ if(len2 > SIMD_EPSILON)
+ {
+ p /= btSqrt(len2);
+ }
+ else
+ {
+ p = trA.getBasis().getColumn(1);
+ }
+ // make one more ortho
+ q = ax1.cross(p);
+ // fill three rows
+ tmpA = relA.cross(p);
+ tmpB = relB.cross(p);
+ for (i=0; i<3; i++) info->m_J1angularAxis[s0+i] = tmpA[i];
+ for (i=0; i<3; i++) info->m_J2angularAxis[s0+i] = -tmpB[i];
+ tmpA = relA.cross(q);
+ tmpB = relB.cross(q);
+ if(hasStaticBody && getSolveLimit())
+ { // to make constraint between static and dynamic objects more rigid
+ // remove wA (or wB) from equation if angular limit is hit
+ tmpB *= factB;
+ tmpA *= factA;
+ }
+ for (i=0; i<3; i++) info->m_J1angularAxis[s1+i] = tmpA[i];
+ for (i=0; i<3; i++) info->m_J2angularAxis[s1+i] = -tmpB[i];
+ tmpA = relA.cross(ax1);
+ tmpB = relB.cross(ax1);
+ if(hasStaticBody)
+ { // to make constraint between static and dynamic objects more rigid
+ // remove wA (or wB) from equation
+ tmpB *= factB;
+ tmpA *= factA;
+ }
+ for (i=0; i<3; i++) info->m_J1angularAxis[s2+i] = tmpA[i];
+ for (i=0; i<3; i++) info->m_J2angularAxis[s2+i] = -tmpB[i];
+
+ for (i=0; i<3; i++) info->m_J1linearAxis[s0+i] = p[i];
+ for (i=0; i<3; i++) info->m_J1linearAxis[s1+i] = q[i];
+ for (i=0; i<3; i++) info->m_J1linearAxis[s2+i] = ax1[i];
+ // compute three elements of right hand side
+ btScalar k = info->fps * info->erp;
+ btScalar rhs = k * p.dot(ofs);
+ info->m_constraintError[s0] = rhs;
+ rhs = k * q.dot(ofs);
+ info->m_constraintError[s1] = rhs;
+ rhs = k * ax1.dot(ofs);
+ info->m_constraintError[s2] = rhs;
+ // the hinge axis should be the only unconstrained
+ // rotational axis, the angular velocity of the two bodies perpendicular to
+ // the hinge axis should be equal. thus the constraint equations are
+ // p*w1 - p*w2 = 0
+ // q*w1 - q*w2 = 0
+ // where p and q are unit vectors normal to the hinge axis, and w1 and w2
+ // are the angular velocity vectors of the two bodies.
+ int s3 = 3 * s;
+ int s4 = 4 * s;
+ info->m_J1angularAxis[s3 + 0] = p[0];
+ info->m_J1angularAxis[s3 + 1] = p[1];
+ info->m_J1angularAxis[s3 + 2] = p[2];
+ info->m_J1angularAxis[s4 + 0] = q[0];
+ info->m_J1angularAxis[s4 + 1] = q[1];
+ info->m_J1angularAxis[s4 + 2] = q[2];
+
+ info->m_J2angularAxis[s3 + 0] = -p[0];
+ info->m_J2angularAxis[s3 + 1] = -p[1];
+ info->m_J2angularAxis[s3 + 2] = -p[2];
+ info->m_J2angularAxis[s4 + 0] = -q[0];
+ info->m_J2angularAxis[s4 + 1] = -q[1];
+ info->m_J2angularAxis[s4 + 2] = -q[2];
+ // compute the right hand side of the constraint equation. set relative
+ // body velocities along p and q to bring the hinge back into alignment.
+ // if ax1A,ax1B are the unit length hinge axes as computed from bodyA and
+ // bodyB, we need to rotate both bodies along the axis u = (ax1 x ax2).
+ // if "theta" is the angle between ax1 and ax2, we need an angular velocity
+ // along u to cover angle erp*theta in one step :
+ // |angular_velocity| = angle/time = erp*theta / stepsize
+ // = (erp*fps) * theta
+ // angular_velocity = |angular_velocity| * (ax1 x ax2) / |ax1 x ax2|
+ // = (erp*fps) * theta * (ax1 x ax2) / sin(theta)
+ // ...as ax1 and ax2 are unit length. if theta is smallish,
+ // theta ~= sin(theta), so
+ // angular_velocity = (erp*fps) * (ax1 x ax2)
+ // ax1 x ax2 is in the plane space of ax1, so we project the angular
+ // velocity to p and q to find the right hand side.
+ k = info->fps * info->erp;
+ btVector3 u = ax1A.cross(ax1B);
+ info->m_constraintError[s3] = k * u.dot(p);
+ info->m_constraintError[s4] = k * u.dot(q);
+#endif
+ // check angular limits
+ nrow = 4; // last filled row
+ int srow;
+ btScalar limit_err = btScalar(0.0);
+ int limit = 0;
+ if(getSolveLimit())
+ {
+ limit_err = m_correction * m_referenceSign;
+ limit = (limit_err > btScalar(0.0)) ? 1 : 2;
+ }
+ // if the hinge has joint limits or motor, add in the extra row
+ int powered = 0;
+ if(getEnableAngularMotor())
+ {
+ powered = 1;
+ }
+ if(limit || powered)
+ {
+ nrow++;
+ srow = nrow * info->rowskip;
+ info->m_J1angularAxis[srow+0] = ax1[0];
+ info->m_J1angularAxis[srow+1] = ax1[1];
+ info->m_J1angularAxis[srow+2] = ax1[2];
+
+ info->m_J2angularAxis[srow+0] = -ax1[0];
+ info->m_J2angularAxis[srow+1] = -ax1[1];
+ info->m_J2angularAxis[srow+2] = -ax1[2];
+
+ btScalar lostop = getLowerLimit();
+ btScalar histop = getUpperLimit();
+ if(limit && (lostop == histop))
+ { // the joint motor is ineffective
+ powered = 0;
+ }
+ info->m_constraintError[srow] = btScalar(0.0f);
+ if(powered)
+ {
+ info->cfm[srow] = btScalar(0.0);
+ btScalar mot_fact = getMotorFactor(m_hingeAngle, lostop, histop, m_motorTargetVelocity, info->fps * info->erp);
+ info->m_constraintError[srow] += mot_fact * m_motorTargetVelocity * m_referenceSign;
+ info->m_lowerLimit[srow] = - m_maxMotorImpulse;
+ info->m_upperLimit[srow] = m_maxMotorImpulse;
+ }
+ if(limit)
+ {
+ k = info->fps * info->erp;
+ info->m_constraintError[srow] += k * limit_err;
+ info->cfm[srow] = btScalar(0.0);
+ if(lostop == histop)
+ {
+ // limited low and high simultaneously
+ info->m_lowerLimit[srow] = -SIMD_INFINITY;
+ info->m_upperLimit[srow] = SIMD_INFINITY;
+ }
+ else if(limit == 1)
+ { // low limit
+ info->m_lowerLimit[srow] = 0;
+ info->m_upperLimit[srow] = SIMD_INFINITY;
+ }
+ else
+ { // high limit
+ info->m_lowerLimit[srow] = -SIMD_INFINITY;
+ info->m_upperLimit[srow] = 0;
+ }
+ // bounce (we'll use slider parameter abs(1.0 - m_dampingLimAng) for that)
+ btScalar bounce = m_relaxationFactor;
+ if(bounce > btScalar(0.0))
+ {
+ btScalar vel = angVelA.dot(ax1);
+ vel -= angVelB.dot(ax1);
+ // only apply bounce if the velocity is incoming, and if the
+ // resulting c[] exceeds what we already have.
+ if(limit == 1)
+ { // low limit
+ if(vel < 0)
+ {
+ btScalar newc = -bounce * vel;
+ if(newc > info->m_constraintError[srow])
+ {
+ info->m_constraintError[srow] = newc;
+ }
+ }
+ }
+ else
+ { // high limit - all those computations are reversed
+ if(vel > 0)
+ {
+ btScalar newc = -bounce * vel;
+ if(newc < info->m_constraintError[srow])
+ {
+ info->m_constraintError[srow] = newc;
+ }
+ }
+ }
+ }
+ info->m_constraintError[srow] *= m_biasFactor;
+ } // if(limit)
+ } // if angular limit or powered
+}
+
diff --git a/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h b/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h
index 270f3f85f..a89718c23 100644
--- a/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h
+++ b/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h
@@ -60,6 +60,7 @@ public:
bool m_enableAngularMotor;
bool m_solveLimit;
bool m_useSolveConstraintObsolete;
+ bool m_useOffsetForConstraintFrame;
bool m_useReferenceFrameA;
btScalar m_accMotorImpulse;
@@ -88,6 +89,7 @@ public:
void getInfo2NonVirtual(btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btVector3& angVelA,const btVector3& angVelB);
void getInfo2Internal(btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btVector3& angVelA,const btVector3& angVelB);
+ void getInfo2InternalUsingFrameOffset(btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btVector3& angVelA,const btVector3& angVelB);
virtual void solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep);
@@ -217,7 +219,9 @@ public:
{
return m_maxMotorImpulse;
}
-
+ // access for UseFrameOffset
+ bool getUseFrameOffset() { return m_useOffsetForConstraintFrame; }
+ void setUseFrameOffset(bool frameOffsetOnOff) { m_useOffsetForConstraintFrame = frameOffsetOnOff; }
};
#endif //HINGECONSTRAINT_H
diff --git a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp
index 10bbea891..7e25451b0 100644
--- a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp
+++ b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp
@@ -819,6 +819,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
for ( j=0;jinternalGetAppliedImpulse();
+ sum += solverConstr.m_appliedImpulse;
+ constr->internalSetAppliedImpulse(sum);
+ }
+
+
if (infoGlobal.m_splitImpulse)
{
for ( i=0;i
-
+#define USE_OFFSET_FOR_CONSTANT_FRAME true
void btSliderConstraint::initParams()
{
@@ -62,6 +62,9 @@ void btSliderConstraint::initParams()
m_maxAngMotorForce = btScalar(0.);
m_accumulatedAngMotorImpulse = btScalar(0.0);
+ m_useLinearReferenceFrameA = USE_OFFSET_FOR_CONSTANT_FRAME;
+
+
}
@@ -80,8 +83,7 @@ btSliderConstraint::btSliderConstraint(btRigidBody& rbA, btRigidBody& rbB, const
: btTypedConstraint(SLIDER_CONSTRAINT_TYPE, rbA, rbB),
m_useSolveConstraintObsolete(false),
m_frameInA(frameInA),
- m_frameInB(frameInB),
- m_useLinearReferenceFrameA(useLinearReferenceFrameA)
+ m_frameInB(frameInB)
{
initParams();
}
@@ -175,7 +177,6 @@ void btSliderConstraint::buildJacobianInt(btRigidBody& rbA, btRigidBody& rbB, co
#endif //__SPU__
}
-
void btSliderConstraint::getInfo1(btConstraintInfo1* info)
{
if (m_useSolveConstraintObsolete)
@@ -189,13 +190,13 @@ void btSliderConstraint::getInfo1(btConstraintInfo1* info)
info->nub = 2;
//prepare constraint
calculateTransforms(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
+ testAngLimits();
testLinLimits();
if(getSolveLinLimit() || getPoweredLinMotor())
{
info->m_numConstraintRows++; // limit 3rd linear as well
info->nub--;
}
- testAngLimits();
if(getSolveAngLimit() || getPoweredAngMotor())
{
info->m_numConstraintRows++; // limit 3rd angular as well
@@ -213,7 +214,14 @@ void btSliderConstraint::getInfo1NonVirtual(btConstraintInfo1* info)
void btSliderConstraint::getInfo2(btConstraintInfo2* info)
{
- getInfo2NonVirtual(info,m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(), m_rbA.getLinearVelocity(),m_rbB.getLinearVelocity(), m_rbA.getInvMass(),m_rbB.getInvMass());
+ if(m_useOffsetForConstraintFrame)
+ {
+ getInfo2NonVirtualUsingFrameOffset(info,m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(), m_rbA.getLinearVelocity(),m_rbB.getLinearVelocity(), m_rbA.getInvMass(),m_rbB.getInvMass());
+ }
+ else
+ {
+ getInfo2NonVirtual(info,m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(), m_rbA.getLinearVelocity(),m_rbB.getLinearVelocity(), m_rbA.getInvMass(),m_rbB.getInvMass());
+ }
}
void btSliderConstraint::getInfo2NonVirtual(btConstraintInfo2* info, const btTransform& transA,const btTransform& transB, const btVector3& linVelA,const btVector3& linVelB, btScalar rbAinvMass,btScalar rbBinvMass )
@@ -532,7 +540,6 @@ void btSliderConstraint::getInfo2NonVirtual(btConstraintInfo2* info, const btTra
}
-
void btSliderConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep)
{
if (m_useSolveConstraintObsolete)
@@ -830,8 +837,6 @@ void btSliderConstraint::testAngLimits(void)
}
}
}
-
-
btVector3 btSliderConstraint::getAncorInA(void)
{
@@ -849,3 +854,362 @@ btVector3 btSliderConstraint::getAncorInB(void)
ancorInB = m_frameInB.getOrigin();
return ancorInB;
}
+
+
+void btSliderConstraint::getInfo2NonVirtualUsingFrameOffset(btConstraintInfo2* info, const btTransform& transA,const btTransform& transB, const btVector3& linVelA,const btVector3& linVelB, btScalar rbAinvMass,btScalar rbBinvMass )
+{
+ const btTransform& trA = getCalculatedTransformA();
+ const btTransform& trB = getCalculatedTransformB();
+
+ btAssert(!m_useSolveConstraintObsolete);
+ int i, s = info->rowskip;
+
+ btScalar signFact = m_useLinearReferenceFrameA ? btScalar(1.0f) : btScalar(-1.0f);
+
+ // difference between frames in WCS
+ btVector3 ofs = trB.getOrigin() - trA.getOrigin();
+ // now get weight factors depending on masses
+ btScalar miA = rbAinvMass;
+ btScalar miB = rbBinvMass;
+ bool hasStaticBody = (miA < SIMD_EPSILON) || (miB < SIMD_EPSILON);
+ btScalar miS = miA + miB;
+ btScalar factA, factB;
+ if(miS > btScalar(0.f))
+ {
+ factA = miB / miS;
+ }
+ else
+ {
+ factA = btScalar(0.5f);
+ }
+ factB = btScalar(1.0f) - factA;
+ // get the desired direction of slider axis
+ // as weighted sum of X-orthos of frameA and frameB in WCS
+ btVector3 ax1A = trA.getBasis().getColumn(0);
+ btVector3 ax1B = trB.getBasis().getColumn(0);
+ btVector3 ax1 = ax1A * factA + ax1B * factB;
+ ax1.normalize();
+ // construct two orthos to slider axis
+ btVector3 p, q;
+ btPlaneSpace1 (ax1, p, q);
+ // make rotations around these orthos equal
+ // the slider axis should be the only unconstrained
+ // rotational axis, the angular velocity of the two bodies perpendicular to
+ // the slider axis should be equal. thus the constraint equations are
+ // p*w1 - p*w2 = 0
+ // q*w1 - q*w2 = 0
+ // where p and q are unit vectors normal to the slider axis, and w1 and w2
+ // are the angular velocity vectors of the two bodies.
+ info->m_J1angularAxis[0] = p[0];
+ info->m_J1angularAxis[1] = p[1];
+ info->m_J1angularAxis[2] = p[2];
+ info->m_J1angularAxis[s+0] = q[0];
+ info->m_J1angularAxis[s+1] = q[1];
+ info->m_J1angularAxis[s+2] = q[2];
+
+ info->m_J2angularAxis[0] = -p[0];
+ info->m_J2angularAxis[1] = -p[1];
+ info->m_J2angularAxis[2] = -p[2];
+ info->m_J2angularAxis[s+0] = -q[0];
+ info->m_J2angularAxis[s+1] = -q[1];
+ info->m_J2angularAxis[s+2] = -q[2];
+ // compute the right hand side of the constraint equation. set relative
+ // body velocities along p and q to bring the slider back into alignment.
+ // if ax1A,ax1B are the unit length slider axes as computed from bodyA and
+ // bodyB, we need to rotate both bodies along the axis u = (ax1 x ax2).
+ // if "theta" is the angle between ax1 and ax2, we need an angular velocity
+ // along u to cover angle erp*theta in one step :
+ // |angular_velocity| = angle/time = erp*theta / stepsize
+ // = (erp*fps) * theta
+ // angular_velocity = |angular_velocity| * (ax1 x ax2) / |ax1 x ax2|
+ // = (erp*fps) * theta * (ax1 x ax2) / sin(theta)
+ // ...as ax1 and ax2 are unit length. if theta is smallish,
+ // theta ~= sin(theta), so
+ // angular_velocity = (erp*fps) * (ax1 x ax2)
+ // ax1 x ax2 is in the plane space of ax1, so we project the angular
+ // velocity to p and q to find the right hand side.
+ btScalar k = info->fps * info->erp * getSoftnessOrthoAng();
+ btVector3 u = ax1A.cross(ax1B);
+ info->m_constraintError[0] = k * u.dot(p);
+ info->m_constraintError[s] = k * u.dot(q);
+
+ int nrow = 1; // last filled row
+ int srow;
+ btScalar limit_err;
+ int limit;
+ int powered;
+
+ // next two rows.
+ // we want: velA + wA x relA == velB + wB x relB ... but this would
+ // result in three equations, so we project along two orthos to the slider axis
+
+ btTransform bodyA_trans = transA;
+ btTransform bodyB_trans = transB;
+ nrow++;
+ int s2 = nrow * s;
+ nrow++;
+ int s3 = nrow * s;
+ btVector3 tmpA, tmpB, relA, relB;
+ // get vector from bodyB to frameB in WCS
+ relB = trB.getOrigin() - bodyB_trans.getOrigin();
+ // get its projection to slider axis
+ btVector3 projB = ax1 * relB.dot(ax1);
+ // get vector directed from bodyB to slider axis (and orthogonal to it)
+ btVector3 orthoB = relB - projB;
+ // same for bodyA
+ relA = trA.getOrigin() - bodyA_trans.getOrigin();
+ btVector3 projA = ax1 * relA.dot(ax1);
+ btVector3 orthoA = relA - projA;
+ // get desired offset between frames A and B along slider axis
+ btScalar sliderOffs = m_linPos - m_depth[0];
+ // desired vector from projection of center of bodyA to projection of center of bodyB to slider axis
+ btVector3 totalDist = projA + ax1 * sliderOffs - projB;
+ // get offset vectors relA and relB
+ relA = orthoA + totalDist * factA;
+ relB = orthoB - totalDist * factB;
+ // now choose average ortho to slider axis
+ p = orthoB * factA + orthoA * factB;
+ btScalar len2 = p.length2();
+ if(len2 > SIMD_EPSILON)
+ {
+ p /= btSqrt(len2);
+ }
+ else
+ {
+ p = trA.getBasis().getColumn(1);
+ }
+ // make one more ortho
+ q = ax1.cross(p);
+ // fill two rows
+ tmpA = relA.cross(p);
+ tmpB = relB.cross(p);
+ for (i=0; i<3; i++) info->m_J1angularAxis[s2+i] = tmpA[i];
+ for (i=0; i<3; i++) info->m_J2angularAxis[s2+i] = -tmpB[i];
+ tmpA = relA.cross(q);
+ tmpB = relB.cross(q);
+ if(hasStaticBody && getSolveAngLimit())
+ { // to make constraint between static and dynamic objects more rigid
+ // remove wA (or wB) from equation if angular limit is hit
+ tmpB *= factB;
+ tmpA *= factA;
+ }
+ for (i=0; i<3; i++) info->m_J1angularAxis[s3+i] = tmpA[i];
+ for (i=0; i<3; i++) info->m_J2angularAxis[s3+i] = -tmpB[i];
+ for (i=0; i<3; i++) info->m_J1linearAxis[s2+i] = p[i];
+ for (i=0; i<3; i++) info->m_J1linearAxis[s3+i] = q[i];
+ // compute two elements of right hand side
+ k = info->fps * info->erp * getSoftnessOrthoLin();
+ btScalar rhs = k * p.dot(ofs);
+ info->m_constraintError[s2] = rhs;
+ rhs = k * q.dot(ofs);
+ info->m_constraintError[s3] = rhs;
+ // check linear limits
+ limit_err = btScalar(0.0);
+ limit = 0;
+ if(getSolveLinLimit())
+ {
+ limit_err = getLinDepth() * signFact;
+ limit = (limit_err > btScalar(0.0)) ? 2 : 1;
+ }
+ powered = 0;
+ if(getPoweredLinMotor())
+ {
+ powered = 1;
+ }
+ // if the slider has joint limits or motor, add in the extra row
+ if (limit || powered)
+ {
+ nrow++;
+ srow = nrow * info->rowskip;
+ info->m_J1linearAxis[srow+0] = ax1[0];
+ info->m_J1linearAxis[srow+1] = ax1[1];
+ info->m_J1linearAxis[srow+2] = ax1[2];
+ // linear torque decoupling step:
+ //
+ // we have to be careful that the linear constraint forces (+/- ax1) applied to the two bodies
+ // do not create a torque couple. in other words, the points that the
+ // constraint force is applied at must lie along the same ax1 axis.
+ // a torque couple will result in limited slider-jointed free
+ // bodies from gaining angular momentum.
+ // this is needed only when bodyA and bodyB are both dynamic.
+ if(!hasStaticBody)
+ {
+ tmpA = relA.cross(ax1);
+ tmpB = relB.cross(ax1);
+ info->m_J1angularAxis[srow+0] = tmpA[0];
+ info->m_J1angularAxis[srow+1] = tmpA[1];
+ info->m_J1angularAxis[srow+2] = tmpA[2];
+ info->m_J2angularAxis[srow+0] = -tmpB[0];
+ info->m_J2angularAxis[srow+1] = -tmpB[1];
+ info->m_J2angularAxis[srow+2] = -tmpB[2];
+ }
+ // right-hand part
+ btScalar lostop = getLowerLinLimit();
+ btScalar histop = getUpperLinLimit();
+ if(limit && (lostop == histop))
+ { // the joint motor is ineffective
+ powered = 0;
+ }
+ info->m_constraintError[srow] = 0.;
+ info->m_lowerLimit[srow] = 0.;
+ info->m_upperLimit[srow] = 0.;
+ if(powered)
+ {
+ info->cfm[nrow] = btScalar(0.0);
+ btScalar tag_vel = getTargetLinMotorVelocity();
+ btScalar mot_fact = getMotorFactor(m_linPos, m_lowerLinLimit, m_upperLinLimit, tag_vel, info->fps * info->erp);
+ info->m_constraintError[srow] -= signFact * mot_fact * getTargetLinMotorVelocity();
+ info->m_lowerLimit[srow] += -getMaxLinMotorForce() * info->fps;
+ info->m_upperLimit[srow] += getMaxLinMotorForce() * info->fps;
+ }
+ if(limit)
+ {
+ k = info->fps * info->erp;
+ info->m_constraintError[srow] += k * limit_err;
+ info->cfm[srow] = btScalar(0.0); // stop_cfm;
+ if(lostop == histop)
+ { // limited low and high simultaneously
+ info->m_lowerLimit[srow] = -SIMD_INFINITY;
+ info->m_upperLimit[srow] = SIMD_INFINITY;
+ }
+ else if(limit == 1)
+ { // low limit
+ info->m_lowerLimit[srow] = -SIMD_INFINITY;
+ info->m_upperLimit[srow] = 0;
+ }
+ else
+ { // high limit
+ info->m_lowerLimit[srow] = 0;
+ info->m_upperLimit[srow] = SIMD_INFINITY;
+ }
+ // bounce (we'll use slider parameter abs(1.0 - m_dampingLimLin) for that)
+ btScalar bounce = btFabs(btScalar(1.0) - getDampingLimLin());
+ if(bounce > btScalar(0.0))
+ {
+ btScalar vel = linVelA.dot(ax1);
+ vel -= linVelB.dot(ax1);
+ vel *= signFact;
+ // only apply bounce if the velocity is incoming, and if the
+ // resulting c[] exceeds what we already have.
+ if(limit == 1)
+ { // low limit
+ if(vel < 0)
+ {
+ btScalar newc = -bounce * vel;
+ if (newc > info->m_constraintError[srow])
+ {
+ info->m_constraintError[srow] = newc;
+ }
+ }
+ }
+ else
+ { // high limit - all those computations are reversed
+ if(vel > 0)
+ {
+ btScalar newc = -bounce * vel;
+ if(newc < info->m_constraintError[srow])
+ {
+ info->m_constraintError[srow] = newc;
+ }
+ }
+ }
+ }
+ info->m_constraintError[srow] *= getSoftnessLimLin();
+ } // if(limit)
+ } // if linear limit
+ // check angular limits
+ limit_err = btScalar(0.0);
+ limit = 0;
+ if(getSolveAngLimit())
+ {
+ limit_err = getAngDepth();
+ limit = (limit_err > btScalar(0.0)) ? 1 : 2;
+ }
+ // if the slider has joint limits, add in the extra row
+ powered = 0;
+ if(getPoweredAngMotor())
+ {
+ powered = 1;
+ }
+ if(limit || powered)
+ {
+ nrow++;
+ srow = nrow * info->rowskip;
+ info->m_J1angularAxis[srow+0] = ax1[0];
+ info->m_J1angularAxis[srow+1] = ax1[1];
+ info->m_J1angularAxis[srow+2] = ax1[2];
+
+ info->m_J2angularAxis[srow+0] = -ax1[0];
+ info->m_J2angularAxis[srow+1] = -ax1[1];
+ info->m_J2angularAxis[srow+2] = -ax1[2];
+
+ btScalar lostop = getLowerAngLimit();
+ btScalar histop = getUpperAngLimit();
+ if(limit && (lostop == histop))
+ { // the joint motor is ineffective
+ powered = 0;
+ }
+ if(powered)
+ {
+ info->cfm[srow] = btScalar(0.0);
+ btScalar mot_fact = getMotorFactor(m_angPos, m_lowerAngLimit, m_upperAngLimit, getTargetAngMotorVelocity(), info->fps * info->erp);
+ info->m_constraintError[srow] = mot_fact * getTargetAngMotorVelocity();
+ info->m_lowerLimit[srow] = -getMaxAngMotorForce() * info->fps;
+ info->m_upperLimit[srow] = getMaxAngMotorForce() * info->fps;
+ }
+ if(limit)
+ {
+ k = info->fps * info->erp;
+ info->m_constraintError[srow] += k * limit_err;
+ info->cfm[srow] = btScalar(0.0); // stop_cfm;
+ if(lostop == histop)
+ {
+ // limited low and high simultaneously
+ info->m_lowerLimit[srow] = -SIMD_INFINITY;
+ info->m_upperLimit[srow] = SIMD_INFINITY;
+ }
+ else if(limit == 1)
+ { // low limit
+ info->m_lowerLimit[srow] = 0;
+ info->m_upperLimit[srow] = SIMD_INFINITY;
+ }
+ else
+ { // high limit
+ info->m_lowerLimit[srow] = -SIMD_INFINITY;
+ info->m_upperLimit[srow] = 0;
+ }
+ // bounce (we'll use slider parameter abs(1.0 - m_dampingLimAng) for that)
+ btScalar bounce = btFabs(btScalar(1.0) - getDampingLimAng());
+ if(bounce > btScalar(0.0))
+ {
+ btScalar vel = m_rbA.getAngularVelocity().dot(ax1);
+ vel -= m_rbB.getAngularVelocity().dot(ax1);
+ // only apply bounce if the velocity is incoming, and if the
+ // resulting c[] exceeds what we already have.
+ if(limit == 1)
+ { // low limit
+ if(vel < 0)
+ {
+ btScalar newc = -bounce * vel;
+ if(newc > info->m_constraintError[srow])
+ {
+ info->m_constraintError[srow] = newc;
+ }
+ }
+ }
+ else
+ { // high limit - all those computations are reversed
+ if(vel > 0)
+ {
+ btScalar newc = -bounce * vel;
+ if(newc < info->m_constraintError[srow])
+ {
+ info->m_constraintError[srow] = newc;
+ }
+ }
+ }
+ }
+ info->m_constraintError[srow] *= getSoftnessLimAng();
+ } // if(limit)
+ } // if angular limit or powered
+}
diff --git a/src/BulletDynamics/ConstraintSolver/btSliderConstraint.h b/src/BulletDynamics/ConstraintSolver/btSliderConstraint.h
index 57b0ed062..c8f76b483 100755
--- a/src/BulletDynamics/ConstraintSolver/btSliderConstraint.h
+++ b/src/BulletDynamics/ConstraintSolver/btSliderConstraint.h
@@ -48,6 +48,7 @@ class btSliderConstraint : public btTypedConstraint
protected:
///for backwards compatibility during the transition to 'getInfo/getInfo2'
bool m_useSolveConstraintObsolete;
+ bool m_useOffsetForConstraintFrame;
btTransform m_frameInA;
btTransform m_frameInB;
// use frameA fo define limits, if true
@@ -137,6 +138,7 @@ public:
virtual void getInfo2 (btConstraintInfo2* info);
void getInfo2NonVirtual(btConstraintInfo2* info, const btTransform& transA, const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB, btScalar rbAinvMass,btScalar rbBinvMass);
+ void getInfo2NonVirtualUsingFrameOffset(btConstraintInfo2* info, const btTransform& transA, const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB, btScalar rbAinvMass,btScalar rbBinvMass);
virtual void solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep);
@@ -221,11 +223,13 @@ public:
// shared code used by ODE solver
void calculateTransforms(const btTransform& transA,const btTransform& transB);
void testLinLimits();
- void testLinLimits2(btConstraintInfo2* info);
void testAngLimits();
// access for PE Solver
btVector3 getAncorInA();
btVector3 getAncorInB();
+ // access for UseFrameOffset
+ bool getUseFrameOffset() { return m_useOffsetForConstraintFrame; }
+ void setUseFrameOffset(bool frameOffsetOnOff) { m_useOffsetForConstraintFrame = frameOffsetOnOff; }
};
diff --git a/src/BulletDynamics/ConstraintSolver/btTypedConstraint.h b/src/BulletDynamics/ConstraintSolver/btTypedConstraint.h
index 430e16285..583937f86 100644
--- a/src/BulletDynamics/ConstraintSolver/btTypedConstraint.h
+++ b/src/BulletDynamics/ConstraintSolver/btTypedConstraint.h
@@ -117,6 +117,11 @@ public:
{
m_appliedImpulse = appliedImpulse;
}
+ ///internal method used by the constraint solver, don't use them directly
+ btScalar internalGetAppliedImpulse()
+ {
+ return m_appliedImpulse;
+ }
///internal method used by the constraint solver, don't use them directly
virtual void solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep) = 0;
diff --git a/src/BulletMultiThreaded/btGpuDefines.h b/src/BulletMultiThreaded/btGpuDefines.h
index 3b5eac028..f9315ab64 100644
--- a/src/BulletMultiThreaded/btGpuDefines.h
+++ b/src/BulletMultiThreaded/btGpuDefines.h
@@ -195,7 +195,7 @@ inline float3 operator-(const float3& v)
#define BT_GPU_PREF(func) btGpu_##func
#define BT_GPU_SAFE_CALL(func) func
#define BT_GPU_Memset memset
-#define BT_GPU_MemcpyToSymbol(a, b, c) memcpy(a, b, c)
+#define BT_GPU_MemcpyToSymbol(a, b, c) memcpy(&a, b, c)
#define BT_GPU_BindTexture(a, b, c, d)
#define BT_GPU_UnbindTexture(a)
diff --git a/src/BulletMultiThreaded/btGpuUtilsSharedDefs.h b/src/BulletMultiThreaded/btGpuUtilsSharedDefs.h
index 92cb251fe..8dd4f8031 100644
--- a/src/BulletMultiThreaded/btGpuUtilsSharedDefs.h
+++ b/src/BulletMultiThreaded/btGpuUtilsSharedDefs.h
@@ -1,6 +1,6 @@
/*
Bullet Continuous Collision Detection and Physics Library, http://bulletphysics.org
-Copyright (C) 2006, 2009 Sony Computer Entertainment Inc.
+Copyright (C) 2006, 2007 Sony Computer Entertainment Inc.
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.
@@ -13,8 +13,6 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
-
-
// Shared definitions for GPU-based utilities
//!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
@@ -24,17 +22,14 @@ subject to the following restrictions:
//!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
-
#ifndef BTGPUUTILSDHAREDDEFS_H
#define BTGPUUTILSDHAREDDEFS_H
-
extern "C"
{
-
//Round a / b to nearest higher integer value
int BT_GPU_PREF(iDivUp)(int a, int b);
@@ -45,15 +40,13 @@ void BT_GPU_PREF(allocateArray)(void** devPtr, unsigned int size);
void BT_GPU_PREF(freeArray)(void* devPtr);
void BT_GPU_PREF(copyArrayFromDevice)(void* host, const void* device, unsigned int size);
void BT_GPU_PREF(copyArrayToDevice)(void* device, const void* host, unsigned int size);
-
-
-
-
+void BT_GPU_PREF(registerGLBufferObject(unsigned int vbo));
+void* BT_GPU_PREF(mapGLBufferObject(unsigned int vbo));
+void BT_GPU_PREF(unmapGLBufferObject(unsigned int vbo));
} // extern "C"
-
#endif // BTGPUUTILSDHAREDDEFS_H