Constraint demo has been changed - debug visualization turned on by default

2009-01-16 03:06:28 +00:00
parent 21afccdb29
commit 4bcd016c98
4 changed files with 603 additions and 390 deletions
--- a/Demos/ConstraintDemo/ConstraintDemo.cpp
+++ b/Demos/ConstraintDemo/ConstraintDemo.cpp
@@ -34,6 +34,7 @@ const int numObjects = 3;

 #define M_PI 3.1415926f
 #define M_PI_2 ((M_PI)*0.5f)
+#define M_PI_4 ((M_PI)*0.25f)

 btTransform sliderTransform;
 btVector3 lowerSliderLimit = btVector3(-10,0,0);
@@ -79,6 +80,16 @@ void	ConstraintDemo::initPhysics()
 	m_constraintSolver = new btSequentialImpulseConstraintSolver();
 	m_dynamicsWorld = new btDiscreteDynamicsWorld(m_dispatcher,m_overlappingPairCache,m_constraintSolver,m_collisionConfiguration);

+
+	btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(50.),btScalar(40.),btScalar(50.)));
+	m_collisionShapes.push_back(groundShape);
+	btTransform groundTransform;
+	groundTransform.setIdentity();
+	groundTransform.setOrigin(btVector3(0,-56,0));
+	btRigidBody* groundBody = localCreateRigidBody(0, groundTransform, groundShape);
+
+
+
 	btCollisionShape* shape = new btBoxShape(btVector3(CUBE_HALF_EXTENTS,CUBE_HALF_EXTENTS,CUBE_HALF_EXTENTS));
 	m_collisionShapes.push_back(shape);
 	btTransform trans;
@@ -86,6 +97,7 @@ void	ConstraintDemo::initPhysics()
 	trans.setOrigin(btVector3(0,20,0));

 	float mass = 1.f;
+#if 1
 	//point to point constraint (ball socket)
 	{
 		btRigidBody* body0 = localCreateRigidBody( mass,trans,shape);
@@ -93,6 +105,7 @@ void	ConstraintDemo::initPhysics()

 		mass = 1.f;
 		btRigidBody* body1 = 0;//localCreateRigidBody( mass,trans,shape);
+//		btRigidBody* body1 = localCreateRigidBody( 0.0,trans,0);
 		//body1->setActivationState(DISABLE_DEACTIVATION);
 		//body1->setDamping(0.3,0.3);

@@ -116,10 +129,12 @@ void	ConstraintDemo::initPhysics()
 		hinge->enableAngularMotor(true,targetVelocity,maxMotorImpulse);

 		m_dynamicsWorld->addConstraint(hinge);//p2p);
+//		m_dynamicsWorld->addConstraint(p2p);

 	}
+#endif

-	
+#if 1	
 	//create a slider, using the generic D6 constraint
 	{
 		mass = 1.f;
@@ -141,36 +156,143 @@ void	ConstraintDemo::initPhysics()
 		frameInA = btTransform::getIdentity();
 		frameInB = btTransform::getIdentity();

-		bool useLinearReferenceFrameA = false;//use fixed frame B for linear limits
-		btGeneric6DofConstraint* slider = new btGeneric6DofConstraint(*d6body0,*fixedBody1,frameInA,frameInB,useLinearReferenceFrameA);
+//		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);

 		//range should be small, otherwise singularities will 'explode' the constraint
-		slider->setAngularLowerLimit(btVector3(20,0,0));
-		slider->setAngularUpperLimit(btVector3(0,0,0));
+		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->getTranslationalLimitMotor()->m_enableMotor[0] = true;
+		slider->getTranslationalLimitMotor()->m_targetVelocity[0] = -5.0f;
+		slider->getTranslationalLimitMotor()->m_maxMotorForce[0] = 0.1f;
+

 		m_dynamicsWorld->addConstraint(slider);

 	}
-
+#endif
+#if 1
 	{ // 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);
 		btTransform doorTrans;
 		doorTrans.setIdentity();
-		doorTrans.setOrigin(btVector3(-5.0f, 0.0f, 0.0f));
+		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, 0.0f, 0.0f ); // right next to the door slightly outside
+		const btVector3 btPivotA( 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 );

 		spDoorHinge->setLimit( 0.0f, M_PI_2 );
 		m_dynamicsWorld->addConstraint(spDoorHinge);
-	}

+		//doorTrans.setOrigin(btVector3(-5.0f, 2.0f, 0.0f));
+		//btRigidBody* pDropBody = localCreateRigidBody( 10.0, doorTrans, shape);
+	}
+#endif
+#if 1
+	{ // create a generic 6DOF constraint
+
+		btTransform tr;
+		tr.setIdentity();
+		tr.setOrigin(btVector3(btScalar(10.), btScalar(6.), btScalar(0.)));
+		tr.getBasis().setEulerZYX(0,0,0);
+//		btRigidBody* pBodyA = localCreateRigidBody( mass, tr, shape);
+		btRigidBody* pBodyA = localCreateRigidBody( 0.0, tr, shape);
+//		btRigidBody* pBodyA = localCreateRigidBody( 0.0, tr, 0);
+		pBodyA->setActivationState(DISABLE_DEACTIVATION);
+
+		tr.setIdentity();
+		tr.setOrigin(btVector3(btScalar(0.), btScalar(6.), btScalar(0.)));
+		tr.getBasis().setEulerZYX(0,0,0);
+		btRigidBody* pBodyB = localCreateRigidBody(1.0, tr, shape);
+		pBodyB->setActivationState(DISABLE_DEACTIVATION);
+
+		btTransform frameInA, frameInB;
+		frameInA = btTransform::getIdentity();
+		frameInA.setOrigin(btVector3(btScalar(-5.), btScalar(0.), btScalar(0.)));
+		frameInB = btTransform::getIdentity();
+		frameInB.setOrigin(btVector3(btScalar(5.), btScalar(0.), btScalar(0.)));
+
+		btGeneric6DofConstraint* pGen6DOF = new btGeneric6DofConstraint(*pBodyA, *pBodyB, frameInA, frameInB, true);
+//		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->setLinearUpperLimit(btVector3(10., 0., 0.));
+//		pGen6DOF->setLinearLowerLimit(btVector3(0., 0., 0.));
+//		pGen6DOF->setLinearUpperLimit(btVector3(0., 0., 0.));
+
+//		pGen6DOF->getTranslationalLimitMotor()->m_enableMotor[0] = true;
+//		pGen6DOF->getTranslationalLimitMotor()->m_targetVelocity[0] = 5.0f;
+//		pGen6DOF->getTranslationalLimitMotor()->m_maxMotorForce[0] = 0.1f;
+
+
+//		pGen6DOF->setAngularLowerLimit(btVector3(0., SIMD_HALF_PI*0.9, 0.));
+//		pGen6DOF->setAngularUpperLimit(btVector3(0., -SIMD_HALF_PI*0.9, 0.));
+//		pGen6DOF->setAngularLowerLimit(btVector3(0., 0., -SIMD_HALF_PI));
+//		pGen6DOF->setAngularUpperLimit(btVector3(0., 0., SIMD_HALF_PI));
+		pGen6DOF->setAngularLowerLimit(btVector3(-SIMD_HALF_PI * 0.5f, -0.75, -SIMD_HALF_PI * 0.8f));
+		pGen6DOF->setAngularUpperLimit(btVector3(SIMD_HALF_PI * 0.5f, 0.75, SIMD_HALF_PI * 0.8f));
+//		pGen6DOF->setAngularLowerLimit(btVector3(-0.75,-0.5, -0.5));
+//		pGen6DOF->setAngularUpperLimit(btVector3(0.75,0.5, 0.5));
+//		pGen6DOF->setAngularLowerLimit(btVector3(-0.75,0., 0.));
+//		pGen6DOF->setAngularUpperLimit(btVector3(0.75,0., 0.));
+
+		m_dynamicsWorld->addConstraint(pGen6DOF, true);
+	}
+#endif
+#if 1
+	{ // create a ConeTwist constraint
+
+		btTransform tr;
+		tr.setIdentity();
+		tr.setOrigin(btVector3(btScalar(-10.), btScalar(5.), btScalar(0.)));
+		tr.getBasis().setEulerZYX(0,0,0);
+		btRigidBody* pBodyA = localCreateRigidBody( 0.0, tr, shape);
+		pBodyA->setActivationState(DISABLE_DEACTIVATION);
+
+		tr.setIdentity();
+		tr.setOrigin(btVector3(btScalar(-10.), btScalar(0.), btScalar(0.)));
+		tr.getBasis().setEulerZYX(0,0,0);
+		btRigidBody* pBodyB = localCreateRigidBody(1.0, tr, shape);
+
+		btTransform frameInA, frameInB;
+		frameInA = btTransform::getIdentity();
+		frameInA.getBasis().setEulerZYX(0, 0, M_PI_2);
+		frameInA.setOrigin(btVector3(btScalar(0.), btScalar(-1.), btScalar(0.)));
+		frameInB = btTransform::getIdentity();
+		frameInB.getBasis().setEulerZYX(0,0,  M_PI_2);
+		frameInB.setOrigin(btVector3(btScalar(0.), btScalar(4.), btScalar(0.)));
+
+		btConeTwistConstraint* pCT = new btConeTwistConstraint(*pBodyA, *pBodyB, frameInA, frameInB);
+		pCT->setLimit(btScalar(M_PI_4)*0.5f, btScalar(M_PI_4), btScalar(M_PI * 0.9));
+		m_dynamicsWorld->addConstraint(pCT, true);
+	}
+#endif
+#if 1
+	{ // Hinge connected to the world, with motor (to hinge motor with new and old constraint solver)
+		btTransform tr;
+		tr.setIdentity();
+		tr.setOrigin(btVector3(btScalar(0.), btScalar(0.), btScalar(0.)));
+		btRigidBody* pBody = localCreateRigidBody( 1.0, tr, shape);
+		pBody->setActivationState(DISABLE_DEACTIVATION);
+		const btVector3 btPivotA( 10.0f, 0.0f, 0.0f );
+		btVector3 btAxisA( 0.0f, 0.0f, 1.0f );
+
+		btHingeConstraint* pHinge = new btHingeConstraint( *pBody, btPivotA, btAxisA );
+		pHinge->enableAngularMotor(true, -1.0, 0.165);
+		m_dynamicsWorld->addConstraint(pHinge);
+	}
+#endif
 }

 ConstraintDemo::~ConstraintDemo()
@@ -266,7 +388,7 @@ void ConstraintDemo::clientMoveAndDisplay()
 	}
 	renderme();

-	drawLimit();
+//	drawLimit();

    glFlush();
    glutSwapBuffers();
@@ -282,7 +404,7 @@ void ConstraintDemo::displayCallback(void) {
 	if (m_dynamicsWorld)
 		m_dynamicsWorld->debugDrawWorld();

-	drawLimit();
+//	drawLimit();

 	renderme();

--- a/Demos/ConstraintDemo/main.cpp
+++ b/Demos/ConstraintDemo/main.cpp
@@ -1,15 +1,20 @@
 #include "ConstraintDemo.h"
 #include "GlutStuff.h"
+#include "GLDebugDrawer.h"
+#include "btBulletDynamicsCommon.h"

 int main(int argc,char** argv)
 {

-        ConstraintDemo* constraintDemo = new ConstraintDemo();
+	GLDebugDrawer	gDebugDrawer;

-        constraintDemo->initPhysics();
+    ConstraintDemo* constraintDemo = new ConstraintDemo();
 	

+    constraintDemo->initPhysics();
+	constraintDemo->getDynamicsWorld()->setDebugDrawer(&gDebugDrawer);
+	constraintDemo->setDebugMode(btIDebugDraw::DBG_DrawConstraints+btIDebugDraw::DBG_DrawConstraintLimits);
 	
-        return glutmain(argc, argv,640,480,"Constraint Demo. http://www.continuousphysics.com/Bullet/phpBB2/",constraintDemo);
+	return glutmain(argc, argv,640,480,"Constraint Demo. http://www.continuousphysics.com/Bullet/phpBB2/",constraintDemo);
 }

--- a/Demos/OpenGL/DemoApplication.cpp
+++ b/Demos/OpenGL/DemoApplication.cpp
@@ -368,6 +368,18 @@ void DemoApplication::keyboardCallback(unsigned char key, int x, int y)
 		else
 			m_debugMode |= btIDebugDraw::DBG_DrawContactPoints;
 		break;
+	case 'C' : 
+		if (m_debugMode & btIDebugDraw::DBG_DrawConstraints)
+			m_debugMode = m_debugMode & (~btIDebugDraw::DBG_DrawConstraints);
+		else
+			m_debugMode |= btIDebugDraw::DBG_DrawConstraints;
+		break;
+	case 'L' : 
+		if (m_debugMode & btIDebugDraw::DBG_DrawConstraintLimits)
+			m_debugMode = m_debugMode & (~btIDebugDraw::DBG_DrawConstraintLimits);
+		else
+			m_debugMode |= btIDebugDraw::DBG_DrawConstraintLimits;
+		break;

 	case 'd' : 
 		if (m_debugMode & btIDebugDraw::DBG_NoDeactivation)
--- a/Demos/SliderConstraintDemo/SliderConstraintDemo.cpp
+++ b/Demos/SliderConstraintDemo/SliderConstraintDemo.cpp
@@ -52,6 +52,9 @@ April 24, 2008
 	static btSliderConstraint *spSlider1, *spSlider2;
 #endif

+static btPoint2PointConstraint* spP2PConst;
+static btHingeConstraint* spHingeConst;
+
 //-----------------------------------------------------------------------------

 static void draw_axes(const btRigidBody& rb, const btTransform& frame)
@@ -155,6 +158,7 @@ void SliderConstraintDemo::initPhysics()
 	btDiscreteDynamicsWorld* wp = new btDiscreteDynamicsWorld(m_dispatcher,m_overlappingPairCache,m_constraintSolver,m_collisionConfiguration);
 	//	wp->getSolverInfo().m_numIterations = 20; // default is 10
 	m_dynamicsWorld = wp;
+//	wp->getSolverInfo().m_erp = 0.8;

 	// add floor
 	//btCollisionShape* groundShape = new btStaticPlaneShape(btVector3(0,1,0),50);
@@ -202,8 +206,11 @@ void SliderConstraintDemo::initPhysics()
 	spSlider1->setAngularUpperLimit(btVector3(0,0,0));
 #else
 	spSlider1 = new btSliderConstraint(*pRbA1, *pRbB1, frameInA, frameInB, true);
+//	spSlider1 = new btSliderConstraint(*pRbA1, *pRbB1, frameInA, frameInB, false);
 	spSlider1->setLowerLinLimit(-15.0F);
 	spSlider1->setUpperLinLimit(-5.0F);
+//	spSlider1->setLowerLinLimit(5.0F);
+//	spSlider1->setUpperLinLimit(15.0F);
 //	spSlider1->setLowerLinLimit(-10.0F);
 //	spSlider1->setUpperLinLimit(-10.0F);
 	spSlider1->setLowerAngLimit(-SIMD_PI / 3.0F);
@@ -215,7 +222,7 @@ void SliderConstraintDemo::initPhysics()
 	// add kinematic rigid body A2
 	worldPos.setValue(0,2,0);
 	trans.setOrigin(worldPos);
-	btRigidBody* pRbA2 = localCreateRigidBody(0, trans, shape);
+	btRigidBody* pRbA2 = localCreateRigidBody(0., trans, shape);
 	pRbA2->setActivationState(DISABLE_DEACTIVATION);

 	// add dynamic rigid body B2
@@ -233,14 +240,30 @@ void SliderConstraintDemo::initPhysics()
 	spSlider2->setAngularUpperLimit(btVector3(0,0,0));
 #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->setLowerAngLimit(SIMD_PI / 2.0F);
-	spSlider2->setUpperAngLimit(-SIMD_PI / 2.0F);
+//	spSlider2->setLowerLinLimit(5.0F);
+//	spSlider2->setUpperLinLimit(25.0F);
+//	spSlider2->setUpperLinLimit(-5.0F);
+//	spSlider2->setUpperLinLimit(-9.99F);
+
+
+//	spSlider2->setLowerAngLimit(SIMD_PI / 2.0F);
+//	spSlider2->setUpperAngLimit(-SIMD_PI / 2.0F);
+	spSlider2->setLowerAngLimit(-SIMD_PI / 2.0F);
+	spSlider2->setUpperAngLimit(SIMD_PI / 2.0F);
+//	spSlider2->setLowerAngLimit(-0.01F);
+//	spSlider2->setUpperAngLimit(0.01F);
+
+
+//	spSlider2->setDampingLimLin(0.5f);
+
+#if 0
 	// add motors
 	spSlider2->setPoweredLinMotor(true);
 	spSlider2->setMaxLinMotorForce(0.1);
-	spSlider2->setTargetLinMotorVelocity(5.0);
+	spSlider2->setTargetLinMotorVelocity(-5.0);

 	spSlider2->setPoweredAngMotor(true);
 //	spSlider2->setMaxAngMotorForce(0.01);
@@ -251,6 +274,7 @@ void SliderConstraintDemo::initPhysics()

 	spSlider2->setDampingDirLin(0.005F);
 	spSlider2->setRestitutionLimLin(1.1F);
+#endif

 	// various ODE tests
 //	spSlider2->setDampingLimLin(0.1F); // linear bounce factor for ODE == 1.0 - DampingLimLin;
@@ -263,6 +287,56 @@ void SliderConstraintDemo::initPhysics()
 	m_dynamicsWorld->addConstraint(spSlider2, true);


+#if 1
+{
+	// 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);
+
+	// add dynamic rigid body B1
+	worldPos.setValue(25,0,0);
+	trans.setOrigin(worldPos);
+	btRigidBody* pRbB3 = localCreateRigidBody(mass, trans, shape);
+	pRbB1->setActivationState(DISABLE_DEACTIVATION);
+
+	btVector3 pivA( 2.5, 0., 0.);
+	btVector3 pivB(-2.5, 0., 0.);
+	spP2PConst = new btPoint2PointConstraint(*pRbA3, *pRbB3, pivA, pivB);
+	m_dynamicsWorld->addConstraint(spP2PConst, true);
+}
+#endif
+
+#if 1
+	// add dynamic rigid body A4
+	trans.setIdentity();
+	worldPos.setValue(20,10,0);
+	trans.setOrigin(worldPos);
+	btRigidBody* pRbA4 = localCreateRigidBody(0.0F, trans, shape);
+	pRbA4->setActivationState(DISABLE_DEACTIVATION);
+
+	// add dynamic rigid body B1
+	worldPos.setValue(27,10,0);
+	trans.setOrigin(worldPos);
+	btRigidBody* pRbB4 = localCreateRigidBody(mass, trans, shape);
+	pRbB1->setActivationState(DISABLE_DEACTIVATION);
+
+
+	btVector3 pivA( 2., 0., 0.);
+	btVector3 pivB(-5., 0., 0.);
+	btVector3 axisA(0., 0., 1.);
+	btVector3 axisB(0., 0., 1.);
+
+	spHingeConst = new btHingeConstraint(*pRbA4, *pRbB4, pivA, pivB, axisA, axisB);
+//	spHingeConst->setLimit(-1.57, 1.57);
+	spHingeConst->setLimit(1.57, -1.57);
+	spHingeConst->enableAngularMotor(true, 10.0, 0.19);
+
+	m_dynamicsWorld->addConstraint(spHingeConst, true);
+#endif
+
 } // SliderConstraintDemo::initPhysics()

 //-----------------------------------------------------------------------------
@@ -340,8 +414,8 @@ void SliderConstraintDemo::clientMoveAndDisplay()
 		}
 	}
 	renderme();
-	drawSlider(spSlider1);
-	drawSlider(spSlider2);
+//	drawSlider(spSlider1);
+//	drawSlider(spSlider2);
    glFlush();
    glutSwapBuffers();
 } // SliderConstraintDemo::clientMoveAndDisplay()
@@ -355,8 +429,8 @@ void SliderConstraintDemo::displayCallback(void)
 	{
 		m_dynamicsWorld->debugDrawWorld();
 	}
-	drawSlider(spSlider1);
-	drawSlider(spSlider2);
+//	drawSlider(spSlider1);
+//	drawSlider(spSlider2);
 	renderme();
    glFlush();
    glutSwapBuffers();