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);
}
