public TestRig(DynamicsWorld ownerWorld, int numLegs, Vector3 position, bool isFixed)
{
_world = ownerWorld;
Legs = new Leg[numLegs];
for (int i = 0; i < numLegs; i++)
{
Legs[i] = new Leg();
}
_bodyShape = new CapsuleShape(BodyRadius, 0.10f);
_thighShape = new CapsuleShape(0.1f, ThighLength);
_shinShape = new CapsuleShape(0.08f, ShinLength);
SetupRigidBodies(position, isFixed);
SetupConstraints();
}
private void SetupRigidBodies(Vector3 position, bool isFixed)
{
const float heightFromGround = 0.5f;
Matrix offset = Matrix.Translation(position);
// body
Vector3 rootPosition = new Vector3(0, heightFromGround, 0);
Matrix transform = Matrix.Translation(rootPosition);
float mass = isFixed ? 0 : 1;
_bodyObject = PhysicsHelper.CreateBody(mass, transform * offset, _bodyShape, _world);
// legs
for (int i = 0; i < Legs.Length; i++)
{
Leg leg = Legs[i];
float fAngle = (float)(2 * Math.PI * i / Legs.Length);
float fSin = (float)Math.Sin(fAngle);
float fCos = (float)Math.Cos(fAngle);
Vector3 boneOrigin = new Vector3(fCos * (BodyRadius + 0.5f * ThighLength), heightFromGround, fSin * (BodyRadius + 0.5f * ThighLength));
Vector3 vToBone = boneOrigin - rootPosition;
vToBone.Normalize();
Vector3 vUp = Vector3.UnitY;
Vector3 vAxis = Vector3.Cross(vToBone, vUp);
transform = Matrix.RotationAxis(vAxis, PI_2) * Matrix.Translation(boneOrigin);
leg.Thigh = PhysicsHelper.CreateBody(1, transform * offset, _thighShape, _world);
transform = Matrix.Translation(fCos * (BodyRadius + ThighLength), heightFromGround - 0.5f * ShinLength, fSin * (BodyRadius + ThighLength));
leg.Shin = PhysicsHelper.CreateBody(1, transform * offset, _shinShape, _world);
SetBodyDamping(leg.Thigh);
SetBodyDamping(leg.Shin);
}
}