Exemple #1
0
 public DefaultVehicleRaycaster(DynamicsWorld world)
 {
     m_dynamicsWorld = world;
 }
        //----------------------------------------------------------------------------------------------

        public DemoApplication()
        {
            m_dynamicsWorld = null;
            m_pickConstraint = null;
            m_shootBoxShape = null;
            m_cameraDistance = 30f;
            m_pitch =(20f/360f)*MathUtil.SIMD_2_PI;
            m_yaw = 0f;
            m_cameraPosition = IndexedVector3.Zero;
            m_cameraTargetPosition = IndexedVector3.Zero;
            m_scaleBottom = 0.5f;
            m_scaleFactor = 2f;
            m_cameraUp = new IndexedVector3(0, 1, 0);
            m_forwardAxis = 2;
            m_glutScreenWidth = 0;
            m_glutScreenHeight = 0;
            m_ShootBoxInitialSpeed = 40f;
            m_stepping = true;
            m_singleStep = false;
            m_idle = false;
            m_enableshadows = true;
            m_lightPosition = new IndexedVector3(5, 5, 5);
            //m_lightDirection = IndexedVector3.Down;
            m_lightDirection = new IndexedVector3(.5f, -.5f, .5f);
            m_lightDirection.Normalize();

            //#ifndef BT_NO_PROFILE
            //    m_profileIterator = CProfileManager::Get_Iterator();
            //#endif //BT_NO_PROFILE
            
            Content.RootDirectory = "Content";
            m_graphics = new GraphicsDeviceManager(this);
            m_graphics.PreferredBackBufferWidth = 800;
            m_graphics.PreferredBackBufferHeight = 600;


            SetSize(m_graphics.PreferredBackBufferWidth,m_graphics.PreferredBackBufferHeight);
            m_nearClip = 1f;
            m_farClip = 1000f;

            m_aspect = m_glutScreenWidth / m_glutScreenHeight;
            m_perspective = IndexedMatrix.CreatePerspectiveFieldOfView(MathHelper.ToRadians(40.0f), m_aspect, m_nearClip, m_farClip);
        }
        	public RagDoll (RagDollDemo ragDollDemo,DynamicsWorld ownerWorld, ref IndexedVector3 positionOffset,StreamWriter streamWriter)
		
	        {
                m_ownerWorld = ownerWorld;
                m_ragDollDemo = ragDollDemo;
		        // Setup the geometry
                m_shapes[(int)BODYPART.PELVIS] = new CapsuleShape(0.15f, 0.20f);
                m_shapes[(int)BODYPART.SPINE] = new CapsuleShape(0.15f, 0.28f);
                m_shapes[(int)BODYPART.HEAD] = new CapsuleShape(0.10f, 0.05f);
                m_shapes[(int)BODYPART.LEFT_UPPER_LEG] = new CapsuleShape(0.07f, 0.45f);
                m_shapes[(int)BODYPART.LEFT_LOWER_LEG] = new CapsuleShape(0.05f, 0.37f);
                m_shapes[(int)BODYPART.RIGHT_UPPER_LEG] = new CapsuleShape(0.07f, 0.45f);
                m_shapes[(int)BODYPART.RIGHT_LOWER_LEG] = new CapsuleShape(0.05f, 0.37f);
                m_shapes[(int)BODYPART.LEFT_UPPER_ARM] = new CapsuleShape(0.05f, 0.33f);
                m_shapes[(int)BODYPART.LEFT_LOWER_ARM] = new CapsuleShape(0.04f, 0.25f);
                m_shapes[(int)BODYPART.RIGHT_UPPER_ARM] = new CapsuleShape(0.05f, 0.33f);
                m_shapes[(int)BODYPART.RIGHT_LOWER_ARM] = new CapsuleShape(0.04f, 0.25f);

		        // Setup all the rigid bodies
		        IndexedMatrix offset = IndexedMatrix.CreateTranslation(positionOffset);

		        IndexedMatrix transform = IndexedMatrix.CreateTranslation(new IndexedVector3(0,1,0));

				IndexedMatrix adjusted = offset*transform;

                m_bodies[(int)BODYPART.PELVIS] = m_ragDollDemo.LocalCreateRigidBody(1f, adjusted, m_shapes[(int)BODYPART.PELVIS],true);
				m_bodies[(int)BODYPART.PELVIS].SetUserPointer("PELVIS");
                transform = IndexedMatrix.CreateTranslation(new IndexedVector3(0,1.2f,0));
				adjusted = offset * transform;
				m_bodies[(int)BODYPART.SPINE] = m_ragDollDemo.LocalCreateRigidBody(1f, adjusted, m_shapes[(int)BODYPART.SPINE], true);
				m_bodies[(int)BODYPART.SPINE].SetUserPointer("SPINE");

                transform = IndexedMatrix.CreateTranslation(new IndexedVector3(0,1.6f,0));
				adjusted = offset * transform;
				m_bodies[(int)BODYPART.HEAD] = m_ragDollDemo.LocalCreateRigidBody(1f, adjusted, m_shapes[(int)BODYPART.HEAD], true);
				m_bodies[(int)BODYPART.HEAD].SetUserPointer("HEAD");

				transform = IndexedMatrix.CreateTranslation(new IndexedVector3(-0.18f, 0.65f, 0));
				adjusted = offset * transform
                    ;
				m_bodies[(int)BODYPART.LEFT_UPPER_LEG] = m_ragDollDemo.LocalCreateRigidBody(1f, adjusted, m_shapes[(int)BODYPART.LEFT_UPPER_LEG], true);
				m_bodies[(int)BODYPART.LEFT_UPPER_LEG].SetUserPointer("LEFTUPPERLEG");

				transform = IndexedMatrix.CreateTranslation(new IndexedVector3(-0.18f, 0.2f, 0));
				adjusted = offset *transform;
				m_bodies[(int)BODYPART.LEFT_LOWER_LEG] = m_ragDollDemo.LocalCreateRigidBody(1f, adjusted, m_shapes[(int)BODYPART.LEFT_LOWER_LEG], true);
				m_bodies[(int)BODYPART.LEFT_LOWER_LEG].SetUserPointer("LEFTLOWERLEG");

				transform = IndexedMatrix.CreateTranslation(new IndexedVector3(0.18f, 0.65f, 0));
				adjusted = offset * transform;
				m_bodies[(int)BODYPART.RIGHT_UPPER_LEG] = m_ragDollDemo.LocalCreateRigidBody(1f, adjusted, m_shapes[(int)BODYPART.RIGHT_UPPER_LEG], true);
				m_bodies[(int)BODYPART.RIGHT_UPPER_LEG].SetUserPointer("RIGHTUPPERLEG");

				transform = IndexedMatrix.CreateTranslation(new IndexedVector3(0.18f, 0.2f, 0));
				adjusted = offset*transform;
				m_bodies[(int)BODYPART.RIGHT_LOWER_LEG] = m_ragDollDemo.LocalCreateRigidBody(1f, adjusted, m_shapes[(int)BODYPART.RIGHT_LOWER_LEG], true);
				m_bodies[(int)BODYPART.RIGHT_LOWER_LEG].SetUserPointer("RIGHTLOWERLEG");

				transform = MathUtil.SetEulerZYX(0, 0, MathUtil.SIMD_HALF_PI);
				transform._origin = new IndexedVector3(-0.35f, 1.45f, 0);
				adjusted = offset *transform;
				m_bodies[(int)BODYPART.LEFT_UPPER_ARM] = m_ragDollDemo.LocalCreateRigidBody(1f, adjusted, m_shapes[(int)BODYPART.LEFT_UPPER_ARM], true);
				m_bodies[(int)BODYPART.LEFT_UPPER_ARM].SetUserPointer("LEFTUPPERARM");

				transform = MathUtil.SetEulerZYX(0, 0, MathUtil.SIMD_HALF_PI);
				transform._origin = new IndexedVector3(-0.7f, 1.45f, 0);
				adjusted = offset * transform;
				m_bodies[(int)BODYPART.LEFT_LOWER_ARM] = m_ragDollDemo.LocalCreateRigidBody(1f, adjusted, m_shapes[(int)BODYPART.LEFT_LOWER_ARM], true);
				m_bodies[(int)BODYPART.LEFT_LOWER_ARM].SetUserPointer("LEFTLOWERARM");

				transform = MathUtil.SetEulerZYX(0, 0, -MathUtil.SIMD_HALF_PI);
				transform._origin = new IndexedVector3(0.35f, 1.45f, 0);
				adjusted = offset * transform;
				m_bodies[(int)BODYPART.RIGHT_UPPER_ARM] = m_ragDollDemo.LocalCreateRigidBody(1f, adjusted, m_shapes[(int)BODYPART.RIGHT_UPPER_ARM], true);
				m_bodies[(int)BODYPART.RIGHT_UPPER_ARM].SetUserPointer("RIGHTUPPERARM");

				transform = MathUtil.SetEulerZYX(0, 0, -MathUtil.SIMD_HALF_PI);
				transform._origin = new IndexedVector3(0.7f, 1.45f, 0);
				adjusted = offset * transform;
				m_bodies[(int)BODYPART.RIGHT_LOWER_ARM] = m_ragDollDemo.LocalCreateRigidBody(1f, adjusted, m_shapes[(int)BODYPART.RIGHT_LOWER_ARM], true);
				m_bodies[(int)BODYPART.RIGHT_LOWER_ARM].SetUserPointer("RIGHTLOWERARM");

		        // Setup some damping on the m_bodies
		        for (int i = 0; i < (int)BODYPART.COUNT; ++i)
		        {
					if (m_bodies[i] != null)
					{
                        //m_bodies[i].SetDamping(0.05f, 0.85f);
                        m_bodies[i].SetDamping(0.5f, 0.85f);
						m_bodies[i].SetDeactivationTime(0.8f);
						m_bodies[i].SetSleepingThresholds(1.6f, 2.5f);
					}
                }

		        // Now setup the constraints
		        HingeConstraint hingeC;
		        ConeTwistConstraint coneC;

		        IndexedMatrix localA = IndexedMatrix.Identity;
                IndexedMatrix localB = IndexedMatrix.Identity;

                localA = MathUtil.SetEulerZYX(0, MathUtil.SIMD_HALF_PI, 0);
                localA._origin = new IndexedVector3(0.0f, 0.15f, 0.0f);
                localB = MathUtil.SetEulerZYX(0, MathUtil.SIMD_HALF_PI, 0);
                localB._origin = new IndexedVector3(0.0f, -0.15f, 0.0f);
                hingeC = new HingeConstraint(m_bodies[(int)BODYPART.PELVIS], m_bodies[(int)BODYPART.SPINE], ref localA, ref localB);
		        hingeC.SetLimit(-MathUtil.SIMD_QUARTER_PI, MathUtil.SIMD_HALF_PI);
                m_joints[(int)JOINT.PELVIS_SPINE] = hingeC;
                m_joints[(int)JOINT.PELVIS_SPINE].m_debugName = "PELVIS_SPINE";
		        hingeC.SetDbgDrawSize(CONSTRAINT_DEBUG_SIZE);

                m_ownerWorld.AddConstraint(m_joints[(int)JOINT.PELVIS_SPINE], true);


                localA = MathUtil.SetEulerZYX(0, 0, MathUtil.SIMD_HALF_PI);
                localA._origin = new IndexedVector3(0.0f, 0.30f, 0.0f);
                localB = MathUtil.SetEulerZYX(0, 0, MathUtil.SIMD_HALF_PI);
                localB._origin = new IndexedVector3(0.0f, -0.14f, 0.0f);
                coneC = new ConeTwistConstraint(m_bodies[(int)BODYPART.SPINE], m_bodies[(int)BODYPART.HEAD], ref localA, ref localB);
		        coneC.SetLimit(MathUtil.SIMD_QUARTER_PI, MathUtil.SIMD_QUARTER_PI, MathUtil.SIMD_HALF_PI);
                m_joints[(int)JOINT.SPINE_HEAD] = coneC;
                m_joints[(int)JOINT.SPINE_HEAD].m_debugName = "SPINE_HEAD";

		        coneC.SetDbgDrawSize(CONSTRAINT_DEBUG_SIZE);

                m_ownerWorld.AddConstraint(m_joints[(int)JOINT.SPINE_HEAD], true);


				localA = IndexedMatrix.Identity;
				localB = IndexedMatrix.Identity;
				localA = MathUtil.SetEulerZYX(0, 0, -MathUtil.SIMD_QUARTER_PI * 5);
				localA._origin = new IndexedVector3(-0.18f, -0.10f, 0.0f);
				localB = MathUtil.SetEulerZYX(0, 0, -MathUtil.SIMD_QUARTER_PI * 5);
				localB._origin = new IndexedVector3(0.0f, 0.225f, 0.0f);
				coneC = new ConeTwistConstraint(m_bodies[(int)BODYPART.PELVIS], m_bodies[(int)BODYPART.LEFT_UPPER_LEG], ref localA, ref localB);
				coneC.SetLimit(MathUtil.SIMD_QUARTER_PI, MathUtil.SIMD_QUARTER_PI, 0);
				m_joints[(int)JOINT.LEFT_HIP] = coneC;
                m_joints[(int)JOINT.LEFT_HIP].m_debugName = "LEFT_HIP";

				coneC.SetDbgDrawSize(CONSTRAINT_DEBUG_SIZE);

				m_ownerWorld.AddConstraint(m_joints[(int)JOINT.LEFT_HIP], true);

				localA = MathUtil.SetEulerZYX(0f, MathUtil.SIMD_HALF_PI, 0f);
				localA._origin = new IndexedVector3(0.0f, -0.225f, 0.0f);
				localB = MathUtil.SetEulerZYX(0, MathUtil.SIMD_HALF_PI, 0);
				localB._origin = new IndexedVector3(0.0f, 0.185f, 0.0f);
				hingeC = new HingeConstraint(m_bodies[(int)BODYPART.LEFT_UPPER_LEG], m_bodies[(int)BODYPART.LEFT_LOWER_LEG], ref localA, ref localB);
				hingeC.SetLimit(0, MathUtil.SIMD_HALF_PI);
				m_joints[(int)JOINT.LEFT_KNEE] = hingeC;
                m_joints[(int)JOINT.LEFT_KNEE].m_debugName = "LEFT_KNEE";

				hingeC.SetDbgDrawSize(CONSTRAINT_DEBUG_SIZE);

				m_ownerWorld.AddConstraint(m_joints[(int)JOINT.LEFT_KNEE], true);


				localA = MathUtil.SetEulerZYX(0, 0, MathUtil.SIMD_QUARTER_PI);
				localA._origin = new IndexedVector3(0.18f, -0.10f, 0.0f);
				localB = MathUtil.SetEulerZYX(0, 0, MathUtil.SIMD_QUARTER_PI);
				localB._origin = new IndexedVector3(0.0f, 0.225f, 0.0f);
				coneC = new ConeTwistConstraint(m_bodies[(int)BODYPART.PELVIS], m_bodies[(int)BODYPART.RIGHT_UPPER_LEG], ref localA, ref localB);
				coneC.SetLimit(MathUtil.SIMD_QUARTER_PI, MathUtil.SIMD_QUARTER_PI, 0);
				m_joints[(int)JOINT.RIGHT_HIP] = coneC;
                m_joints[(int)JOINT.RIGHT_HIP].m_debugName = "RIGHT_HIP";

				coneC.SetDbgDrawSize(CONSTRAINT_DEBUG_SIZE);

				m_ownerWorld.AddConstraint(m_joints[(int)JOINT.RIGHT_HIP], true);

				localA = MathUtil.SetEulerZYX(0, MathUtil.SIMD_HALF_PI, 0);
				localA._origin = new IndexedVector3(0.0f, -0.225f, 0.0f);
				localB = MathUtil.SetEulerZYX(0, MathUtil.SIMD_HALF_PI, 0);
				localB._origin = new IndexedVector3(0.0f, 0.185f, 0.0f);
				hingeC = new HingeConstraint(m_bodies[(int)BODYPART.RIGHT_UPPER_LEG], m_bodies[(int)BODYPART.RIGHT_LOWER_LEG], ref localA, ref localB);
				hingeC.SetLimit(0, MathUtil.SIMD_HALF_PI);
				m_joints[(int)JOINT.RIGHT_KNEE] = hingeC;
                m_joints[(int)JOINT.RIGHT_KNEE].m_debugName = "RIGHT_KNEE";

				hingeC.SetDbgDrawSize(CONSTRAINT_DEBUG_SIZE);

				m_ownerWorld.AddConstraint(m_joints[(int)JOINT.RIGHT_KNEE], true);


				localA = MathUtil.SetEulerZYX(0, 0, MathUtil.SIMD_PI);
				localA._origin = new IndexedVector3(-0.2f, 0.15f, 0.0f);
				localB = MathUtil.SetEulerZYX(0, 0, MathUtil.SIMD_HALF_PI);
				localB._origin = new IndexedVector3(0.0f, -0.18f, 0.0f);
				coneC = new ConeTwistConstraint(m_bodies[(int)BODYPART.SPINE], m_bodies[(int)BODYPART.LEFT_UPPER_ARM], ref localA, ref localB);
				coneC.SetLimit(MathUtil.SIMD_HALF_PI, MathUtil.SIMD_HALF_PI, 0);
				coneC.SetDbgDrawSize(CONSTRAINT_DEBUG_SIZE);

				m_joints[(int)JOINT.LEFT_SHOULDER] = coneC;
                m_joints[(int)JOINT.LEFT_SHOULDER].m_debugName = "LEFT_SHOULDER";

				m_ownerWorld.AddConstraint(m_joints[(int)JOINT.LEFT_SHOULDER], true);

				localA = MathUtil.SetEulerZYX(0, MathUtil.SIMD_HALF_PI, 0);
				localA._origin = new IndexedVector3(0.0f, 0.18f, 0.0f);
				localB = MathUtil.SetEulerZYX(0, MathUtil.SIMD_HALF_PI, 0);
				localB._origin = new IndexedVector3(0.0f, -0.14f, 0.0f);
				hingeC = new HingeConstraint(m_bodies[(int)BODYPART.LEFT_UPPER_ARM], m_bodies[(int)BODYPART.LEFT_LOWER_ARM], ref localA, ref localB);
				//		hingeC.setLimit(-MathUtil.SIMD_HALF_PI), 0));
				hingeC.SetLimit(0, MathUtil.SIMD_HALF_PI);
				m_joints[(int)JOINT.LEFT_ELBOW] = hingeC;
                m_joints[(int)JOINT.LEFT_ELBOW].m_debugName = "LEFT_ELBOW";

				hingeC.SetDbgDrawSize(CONSTRAINT_DEBUG_SIZE);

				m_ownerWorld.AddConstraint(m_joints[(int)JOINT.LEFT_ELBOW], true);

				localA = MathUtil.SetEulerZYX(0, 0, 0);
				localA._origin = new IndexedVector3(0.2f, 0.15f, 0.0f);
				localB = MathUtil.SetEulerZYX(0, 0, MathUtil.SIMD_HALF_PI);
				localB._origin = new IndexedVector3(0.0f, -0.18f, 0.0f);
				coneC = new ConeTwistConstraint(m_bodies[(int)BODYPART.SPINE], m_bodies[(int)BODYPART.RIGHT_UPPER_ARM], ref localA, ref localB);
				coneC.SetLimit(MathUtil.SIMD_HALF_PI, MathUtil.SIMD_HALF_PI, 0);
				m_joints[(int)JOINT.RIGHT_SHOULDER] = coneC;
                m_joints[(int)JOINT.RIGHT_SHOULDER].m_debugName = "RIGHT_SHOULDER";

				coneC.SetDbgDrawSize(CONSTRAINT_DEBUG_SIZE);

				m_ownerWorld.AddConstraint(m_joints[(int)JOINT.RIGHT_SHOULDER], true);

				localA = MathUtil.SetEulerZYX(0, MathUtil.SIMD_HALF_PI, 0);
				localA._origin = new IndexedVector3(0.0f, 0.18f, 0.0f);
				localB = MathUtil.SetEulerZYX(0, MathUtil.SIMD_HALF_PI, 0);
				localB._origin = new IndexedVector3(0.0f, -0.14f, 0.0f);
				hingeC = new HingeConstraint(m_bodies[(int)BODYPART.RIGHT_UPPER_ARM], m_bodies[(int)BODYPART.RIGHT_LOWER_ARM], ref localA, ref localB);
				//		hingeC.setLimit(-MathUtil.SIMD_HALF_PI), 0));
				hingeC.SetLimit(0, MathUtil.SIMD_HALF_PI);
				m_joints[(int)JOINT.RIGHT_ELBOW] = hingeC;
                m_joints[(int)JOINT.RIGHT_ELBOW].m_debugName = "RIGHT_ELBOW";

				hingeC.SetDbgDrawSize(CONSTRAINT_DEBUG_SIZE);

                m_ownerWorld.AddConstraint(m_joints[(int)JOINT.RIGHT_ELBOW], true); 
				
				
	        }
	    public DefaultVehicleRaycaster(DynamicsWorld world)
	    {
            m_dynamicsWorld = world;
	    }
		public TestRig (DemoApplication demoApplication,DynamicsWorld ownerWorld, ref IndexedVector3 positionOffset, bool bFixed)
		{
			m_dynamicsWorld = ownerWorld;
            IndexedVector3 vUp = new IndexedVector3(0, 1, 0);

			//
			// Setup geometry
			//
			float fBodySize  = 0.25f;
			float fLegLength = 0.45f;
			float fForeLegLength = 0.75f;
			m_shapes[0] = new CapsuleShape(fBodySize, 0.10f);
			for (int i=0; i<NUM_LEGS; i++)
			{
				m_shapes[1 + 2*i] = new CapsuleShape(0.10f, fLegLength);
				m_shapes[2 + 2*i] = new CapsuleShape(0.08f, fForeLegLength);
			}

			//
			// Setup rigid bodies
			//
			float fHeight = 0.5f;
			IndexedMatrix offset = IndexedMatrix.Identity;
			offset._origin = positionOffset;		

			// root
			IndexedVector3 vRoot = new IndexedVector3(0,fHeight,0);
			IndexedMatrix transform = IndexedMatrix.Identity;
			transform._origin = vRoot;

			if (bFixed)
			{
				m_bodies[0] = demoApplication.LocalCreateRigidBody(0.0f, offset * transform, m_shapes[0]);
			} else
			{
				m_bodies[0] = demoApplication.LocalCreateRigidBody(1.0f, offset * transform, m_shapes[0]);
			}
			// legs
			for (int i=0; i<NUM_LEGS; i++)
			{
				float fAngle = MathUtil.SIMD_2_PI * i / NUM_LEGS;
				float fSin = (float)Math.Sin(fAngle);
				float fCos = (float)Math.Cos(fAngle);

				transform = IndexedMatrix.Identity;
				IndexedVector3 vBoneOrigin = new IndexedVector3(fCos*(fBodySize+0.5f*fLegLength), fHeight, fSin*(fBodySize+0.5f*fLegLength));
				transform._origin = vBoneOrigin;

				// thigh
				IndexedVector3 vToBone = (vBoneOrigin - vRoot);
				vToBone.Normalize();

				IndexedVector3 vAxis = IndexedVector3.Cross(vToBone,vUp);	
				transform._basis = new IndexedBasisMatrix(Quaternion.CreateFromAxisAngle(vAxis.ToVector3(), MathUtil.SIMD_HALF_PI));
				transform._origin = vBoneOrigin;
				m_bodies[1+2*i] = demoApplication.LocalCreateRigidBody(1.0f, offset * transform, m_shapes[1+2*i]);

				// shin
				transform = IndexedMatrix.Identity;
				transform._origin = new IndexedVector3(fCos*(fBodySize+fLegLength), fHeight-0.5f*fForeLegLength, fSin*(fBodySize+fLegLength));
				m_bodies[2+2*i] = demoApplication.LocalCreateRigidBody(1.0f, offset * transform, m_shapes[2+2*i]);
			}

			// Setup some damping on the m_bodies
			for (int i = 0; i < BODYPART_COUNT; ++i)
			{
				m_bodies[i].SetDamping(0.05f, 0.85f);
				m_bodies[i].SetDeactivationTime(0.8f);
				//m_bodies[i].setSleepingThresholds(1.6, 2.5);
				m_bodies[i].SetSleepingThresholds(0.5f, 0.5f);
			}


			//
			// Setup the constraints
			//
			HingeConstraint hingeC;

			IndexedMatrix localA, localB, localC;

			for (int i=0; i<NUM_LEGS; i++)
			{
				float fAngle = MathUtil.SIMD_2_PI * i / NUM_LEGS;
				float fSin = (float)Math.Sin(fAngle);
				float fCos = (float)Math.Cos(fAngle);

				// hip joints
				localA = IndexedMatrix.Identity; 
				localB= IndexedMatrix.Identity;

				localA = MathUtil.SetEulerZYX(0f,-fAngle,0f);	
				localA._origin = new IndexedVector3(fCos*fBodySize, 0.0f, fSin*fBodySize);
                localB = m_bodies[1 + 2 * i].GetWorldTransform().Inverse() * m_bodies[0].GetWorldTransform() * localA;


                if (BulletGlobals.g_streamWriter != null && false)
				{
					MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "Hip LocalA", localA);
					MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "Hip LocalB", localB);
				}

				hingeC = new HingeConstraint(m_bodies[0], m_bodies[1+2*i], ref localA, ref localB);
				hingeC.SetLimit(-0.75f * MathUtil.SIMD_QUARTER_PI, MathUtil.SIMD_QUARTER_PI/2f);
				m_joints[2*i] = hingeC;
				m_dynamicsWorld.AddConstraint(m_joints[2*i], true);

				// knee joints
				localA = IndexedMatrix.Identity; 
				localB= IndexedMatrix.Identity;
				localC = IndexedMatrix.Identity;

				localA = MathUtil.SetEulerZYX(0f,-fAngle,0f);	
				localA._origin = new IndexedVector3(fCos*(fBodySize+fLegLength), 0.0f, fSin*(fBodySize+fLegLength));

                localB = m_bodies[1 + 2 * i].GetWorldTransform().Inverse() * m_bodies[0].GetWorldTransform() * localA;
                localC = m_bodies[2 + 2 * i].GetWorldTransform().Inverse() * m_bodies[0].GetWorldTransform() * localA;


				if (BulletGlobals.g_streamWriter != null && false)
				{
					MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "Knee LocalA", localA);
					MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "Knee LocalB", localB);
					MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "Knee LocalC", localC);
				}

				hingeC = new HingeConstraint(m_bodies[1 + 2 * i], m_bodies[2 + 2 * i], ref localB, ref localC);
				//hingeC.setLimit(float(-0.01), float(0.01));
				hingeC.SetLimit(-MathUtil.SIMD_QUARTER_PI/2f, 0.2f);
				m_joints[1+2*i] = hingeC;
				m_dynamicsWorld.AddConstraint(m_joints[1+2*i], true);
			}
		}
		public void InternalTickCallback(DynamicsWorld world, float timeStep)
		{
			MotorDemo motorDemo = (MotorDemo)world.GetWorldUserInfo();
			motorDemo.SetMotorTargets(timeStep);
		}