private void PickRigidBody(RigidBody body, ref Vector3 pickPosition)
{
if (body.IsStaticObject || body.IsKinematicObject)
{
return;
}
_pickedBody = body;
_pickedBody.ActivationState = ActivationState.DisableDeactivation;
DynamicsWorld world = _demo.World;
Vector3 localPivot = Vector3.TransformCoordinate(pickPosition, Matrix.Invert(body.CenterOfMassTransform));
if (_demo.Input.KeysDown.Contains(Keys.ShiftKey))
{
var dof6 = new Generic6DofConstraint(body, Matrix.Translation(localPivot), false)
{
LinearLowerLimit = Vector3.Zero,
LinearUpperLimit = Vector3.Zero,
AngularLowerLimit = Vector3.Zero,
AngularUpperLimit = Vector3.Zero
};
world.AddConstraint(dof6);
_rigidBodyPickConstraint = dof6;
dof6.SetParam(ConstraintParam.StopCfm, 0.8f, 0);
dof6.SetParam(ConstraintParam.StopCfm, 0.8f, 1);
dof6.SetParam(ConstraintParam.StopCfm, 0.8f, 2);
dof6.SetParam(ConstraintParam.StopCfm, 0.8f, 3);
dof6.SetParam(ConstraintParam.StopCfm, 0.8f, 4);
dof6.SetParam(ConstraintParam.StopCfm, 0.8f, 5);
dof6.SetParam(ConstraintParam.StopErp, 0.1f, 0);
dof6.SetParam(ConstraintParam.StopErp, 0.1f, 1);
dof6.SetParam(ConstraintParam.StopErp, 0.1f, 2);
dof6.SetParam(ConstraintParam.StopErp, 0.1f, 3);
dof6.SetParam(ConstraintParam.StopErp, 0.1f, 4);
dof6.SetParam(ConstraintParam.StopErp, 0.1f, 5);
}
else
{
var p2p = new Point2PointConstraint(body, localPivot);
world.AddConstraint(p2p);
_rigidBodyPickConstraint = p2p;
p2p.Setting.ImpulseClamp = 30;
//very weak constraint for picking
p2p.Setting.Tau = 0.001f;
/*
* p2p.SetParam(ConstraintParams.Cfm, 0.8f, 0);
* p2p.SetParam(ConstraintParams.Cfm, 0.8f, 1);
* p2p.SetParam(ConstraintParams.Cfm, 0.8f, 2);
* p2p.SetParam(ConstraintParams.Erp, 0.1f, 0);
* p2p.SetParam(ConstraintParams.Erp, 0.1f, 1);
* p2p.SetParam(ConstraintParams.Erp, 0.1f, 2);
*/
}
}
private void CreateLegs(Matrix rootTransform)
{
_thighShape = new CapsuleShape(0.10f, ThighLength);
_shinShape = new CapsuleShape(0.08f, ShinLength);
const float PI_2 = (float)(0.5f * Math.PI);
Vector3 up = new Vector3(0, 1, 0);
for (int i = 0; i < NumLegs; i++)
{
float direction = (float)(2 * Math.PI * i / NumLegs);
Vector3 thighOrigin = ToCartesian(direction, BodyRadius + 0.5f * ThighLength);
Vector3 toBone = thighOrigin.Normalized;
Vector3 vAxis = Vector3.Cross(toBone, up);
Matrix transform = Matrix.RotationQuaternion(Quaternion.RotationAxis(vAxis, PI_2)) * Matrix.Translation(thighOrigin);
RigidBody thigh = LocalCreateRigidBody(1, transform * rootTransform, _thighShape);
SetDamping(thigh);
Vector3 translation = ToCartesian(direction, BodyRadius + ThighLength);
translation.Y = -0.5f * ShinLength;
transform = Matrix.Translation(translation);
RigidBody shin = LocalCreateRigidBody(1, transform * rootTransform, _shinShape);
SetDamping(shin);
Joints[2 * i] = CreateHipJoint(_rootBody, thigh, direction);
_ownerWorld.AddConstraint(Joints[2 * i], true);
Joints[2 * i + 1] = CreateKneeJoint(_rootBody, thigh, shin, direction);
_ownerWorld.AddConstraint(Joints[2 * i + 1], true);
_jointBodies.Add(thigh);
_jointBodies.Add(shin);
}
}
private void SetupConstraints()
{
Matrix localA, localB, localC;
for (int i = 0; i < Legs.Length; i++)
{
Leg leg = Legs[i];
float legAngle = (float)(2 * Math.PI * i / Legs.Length);
float fSin = (float)Math.Sin(legAngle);
float fCos = (float)Math.Cos(legAngle);
localA = Matrix.RotationYawPitchRoll(-legAngle, 0, 0) * Matrix.Translation(fCos * BodyRadius, 0, fSin * BodyRadius);
localB = localA * _bodyObject.WorldTransform * Matrix.Invert(leg.Thigh.WorldTransform);
leg.Hip = new HingeConstraint(_bodyObject, leg.Thigh, localA, localB);
leg.Hip.SetLimit(-0.75f * PI_4, PI_8);
//leg.Hip.SetLimit(-0.1f, 0.1f);
_world.AddConstraint(leg.Hip, true);
localA = Matrix.RotationYawPitchRoll(-legAngle, 0, 0) * Matrix.Translation(fCos * (BodyRadius + ThighLength), 0, fSin * (BodyRadius + ThighLength));
localB = localA * _bodyObject.WorldTransform * Matrix.Invert(leg.Thigh.WorldTransform);
localC = localA * _bodyObject.WorldTransform * Matrix.Invert(leg.Shin.WorldTransform);
leg.Knee = new HingeConstraint(leg.Thigh, leg.Shin, localB, localC);
//leg.Knee.SetLimit(-0.01f, 0.01f);
leg.Knee.SetLimit(-PI_8, 0.2f);
_world.AddConstraint(leg.Knee, true);
}
}
public override void AddConstraint(Physic.Constraints.IPhysicConstraint ctn)
{
if (ctn is BulletConstraint)
{
BulletConstraint BulletConstraint = ctn as BulletConstraint;
world.AddConstraint(BulletConstraint.Constraint, BulletConstraint.DisableCollisionsBetweenLinkedBodies);
}
ctns.Add(ctn);
}
/// <summary>
/// add to default world
/// </summary>
private void AddToWorld()
{
for (int i = 0; i < RigidBodies.Length; i++)
{
World.AddRigidBody(RigidBodies[i]);
}
for (int i = 0; i < Constraints.Length; i++)
{
World.AddConstraint(Constraints[i], true);
}
}
public bool AddConstraint(BTypedConstraint c)
{
if (!_isDisposed)
{
Debug.LogFormat("Adding constraint {0} to world", c);
if (c._BuildConstraint())
{
World.AddConstraint(c.GetConstraint(), c.disableCollisionsBetweenConstrainedBodies);
}
return(true);
}
return(false);
}
public TestRig(DynamicsWorld ownerWorld, Vector3 positionOffset, bool bFixed)
{
this.ownerWorld = ownerWorld;
Vector3 vUp = new Vector3(0, 1, 0);
//
// Setup geometry
//
const float fBodySize = 0.25f;
const float fLegLength = 0.45f;
const float fForeLegLength = 0.75f;
const float PI_2 = (float)(0.5f * Math.PI);
const float PI_4 = (float)(0.25f * Math.PI);
const float PI_8 = (float)(0.125f * Math.PI);
shapes[0] = new CapsuleShape(fBodySize, 0.10f);
int i;
for (i = 0; i < NumLegs; i++)
{
shapes[1 + 2 * i] = new CapsuleShape(0.10f, fLegLength);
shapes[2 + 2 * i] = new CapsuleShape(0.08f, fForeLegLength);
}
//
// Setup rigid bodies
//
const float fHeight = 0.5f;
Matrix offset = Matrix.Translation(positionOffset);
// root
Vector3 vRoot = new Vector3(0, fHeight, 0);
Matrix transform = Matrix.Translation(vRoot);
if (bFixed)
{
bodies[0] = LocalCreateRigidBody(0, transform * offset, shapes[0]);
}
else
{
bodies[0] = LocalCreateRigidBody(1, transform * offset, shapes[0]);
}
// legs
for (i = 0; i < NumLegs; i++)
{
float fAngle = (float)(2 * Math.PI * i / NumLegs);
float fSin = (float)Math.Sin(fAngle);
float fCos = (float)Math.Cos(fAngle);
Vector3 vBoneOrigin = new Vector3(fCos * (fBodySize + 0.5f * fLegLength), fHeight, fSin * (fBodySize + 0.5f * fLegLength));
// thigh
Vector3 vToBone = (vBoneOrigin - vRoot);
vToBone.Normalize();
Vector3 vAxis = Vector3.Cross(vToBone, vUp);
transform = Matrix.RotationQuaternion(Quaternion.RotationAxis(vAxis, PI_2)) * Matrix.Translation(vBoneOrigin);
bodies[1 + 2 * i] = LocalCreateRigidBody(1, transform * offset, shapes[1 + 2 * i]);
// shin
transform = Matrix.Translation(fCos * (fBodySize + fLegLength), fHeight - 0.5f * fForeLegLength, fSin * (fBodySize + fLegLength));
bodies[2 + 2 * i] = LocalCreateRigidBody(1, transform * offset, shapes[2 + 2 * i]);
}
// Setup some damping on the bodies
for (i = 0; i < BodyPartCount; ++i)
{
bodies[i].SetDamping(0.05f, 0.85f);
bodies[i].DeactivationTime = 0.8f;
//bodies[i].SetSleepingThresholds(1.6f, 2.5f);
bodies[i].SetSleepingThresholds(0.5f, 0.5f);
}
//
// Setup the constraints
//
HingeConstraint hingeC;
//ConeTwistConstraint coneC;
Matrix localA, localB, localC;
for (i = 0; i < NumLegs; i++)
{
float fAngle = (float)(2 * Math.PI * i / NumLegs);
float fSin = (float)Math.Sin(fAngle);
float fCos = (float)Math.Cos(fAngle);
// hip joints
localA = Matrix.RotationYawPitchRoll(-fAngle, 0, 0) * Matrix.Translation(fCos * fBodySize, 0, fSin * fBodySize); // OK
localB = localA * bodies[0].WorldTransform * Matrix.Invert(bodies[1 + 2 * i].WorldTransform);
hingeC = new HingeConstraint(bodies[0], bodies[1 + 2 * i], localA, localB);
hingeC.SetLimit(-0.75f * PI_4, PI_8);
//hingeC.SetLimit(-0.1f, 0.1f);
joints[2 * i] = hingeC;
ownerWorld.AddConstraint(joints[2 * i], true);
// knee joints
localA = Matrix.RotationYawPitchRoll(-fAngle, 0, 0) * Matrix.Translation(fCos * (fBodySize + fLegLength), 0, fSin * (fBodySize + fLegLength));
localB = localA * bodies[0].WorldTransform * Matrix.Invert(bodies[1 + 2 * i].WorldTransform);
localC = localA * bodies[0].WorldTransform * Matrix.Invert(bodies[2 + 2 * i].WorldTransform);
hingeC = new HingeConstraint(bodies[1 + 2 * i], bodies[2 + 2 * i], localB, localC);
//hingeC.SetLimit(-0.01f, 0.01f);
hingeC.SetLimit(-PI_8, 0.2f);
joints[1 + 2 * i] = hingeC;
ownerWorld.AddConstraint(joints[1 + 2 * i], true);
}
}
public virtual void OnHandleInput()
{
if (Input.KeysPressed.Count != 0)
{
switch (Input.KeysPressed[0])
{
case Keys.Escape:
case Keys.Q:
Graphics.Form.Close();
return;
case Keys.F3:
IsDebugDrawEnabled = !IsDebugDrawEnabled;
break;
case Keys.F8:
Input.ClearKeyCache();
GraphicsLibraryManager.ExitWithReload = true;
Graphics.Form.Close();
break;
case Keys.F11:
Graphics.IsFullScreen = !Graphics.IsFullScreen;
break;
case (Keys.Control | Keys.F):
const int maxSerializeBufferSize = 1024 * 1024 * 5;
DefaultSerializer serializer = new DefaultSerializer(maxSerializeBufferSize);
World.Serialize(serializer);
byte[] dataBytes = new byte[serializer.CurrentBufferSize];
Marshal.Copy(serializer.BufferPointer, dataBytes, 0, dataBytes.Length);
System.IO.FileStream file = new System.IO.FileStream("world.bullet", System.IO.FileMode.Create);
file.Write(dataBytes, 0, dataBytes.Length);
file.Dispose();
break;
case Keys.G:
//shadowsEnabled = !shadowsEnabled;
break;
case Keys.Space:
ShootBox(Freelook.Eye, GetRayTo(Input.MousePoint, Freelook.Eye, Freelook.Target, Graphics.FieldOfView));
break;
case Keys.Return:
ClientResetScene();
break;
}
}
if (Input.MousePressed != MouseButtons.None)
{
Vector3 rayTo = GetRayTo(Input.MousePoint, Freelook.Eye, Freelook.Target, Graphics.FieldOfView);
if (Input.MousePressed == MouseButtons.Right)
{
if (_world != null)
{
Vector3 rayFrom = Freelook.Eye;
ClosestRayResultCallback rayCallback = new ClosestRayResultCallback(ref rayFrom, ref rayTo);
_world.RayTestRef(ref rayFrom, ref rayTo, rayCallback);
if (rayCallback.HasHit)
{
RigidBody body = rayCallback.CollisionObject as RigidBody;
if (body != null)
{
if (!(body.IsStaticObject || body.IsKinematicObject))
{
pickedBody = body;
pickedBody.ActivationState = ActivationState.DisableDeactivation;
Vector3 pickPos = rayCallback.HitPointWorld;
Vector3 localPivot = Vector3.TransformCoordinate(pickPos, Matrix.Invert(body.CenterOfMassTransform));
if (Input.KeysDown.Contains(Keys.ShiftKey))
{
Generic6DofConstraint dof6 = new Generic6DofConstraint(body, Matrix.Translation(localPivot), false)
{
LinearLowerLimit = Vector3.Zero,
LinearUpperLimit = Vector3.Zero,
AngularLowerLimit = Vector3.Zero,
AngularUpperLimit = Vector3.Zero
};
_world.AddConstraint(dof6);
pickConstraint = dof6;
dof6.SetParam(ConstraintParam.StopCfm, 0.8f, 0);
dof6.SetParam(ConstraintParam.StopCfm, 0.8f, 1);
dof6.SetParam(ConstraintParam.StopCfm, 0.8f, 2);
dof6.SetParam(ConstraintParam.StopCfm, 0.8f, 3);
dof6.SetParam(ConstraintParam.StopCfm, 0.8f, 4);
dof6.SetParam(ConstraintParam.StopCfm, 0.8f, 5);
dof6.SetParam(ConstraintParam.StopErp, 0.1f, 0);
dof6.SetParam(ConstraintParam.StopErp, 0.1f, 1);
dof6.SetParam(ConstraintParam.StopErp, 0.1f, 2);
dof6.SetParam(ConstraintParam.StopErp, 0.1f, 3);
dof6.SetParam(ConstraintParam.StopErp, 0.1f, 4);
dof6.SetParam(ConstraintParam.StopErp, 0.1f, 5);
}
else
{
Point2PointConstraint p2p = new Point2PointConstraint(body, localPivot);
_world.AddConstraint(p2p);
pickConstraint = p2p;
p2p.Setting.ImpulseClamp = 30;
//very weak constraint for picking
p2p.Setting.Tau = 0.001f;
/*
* p2p.SetParam(ConstraintParams.Cfm, 0.8f, 0);
* p2p.SetParam(ConstraintParams.Cfm, 0.8f, 1);
* p2p.SetParam(ConstraintParams.Cfm, 0.8f, 2);
* p2p.SetParam(ConstraintParams.Erp, 0.1f, 0);
* p2p.SetParam(ConstraintParams.Erp, 0.1f, 1);
* p2p.SetParam(ConstraintParams.Erp, 0.1f, 2);
*/
}
oldPickingDist = (pickPos - rayFrom).Length;
}
}
}
rayCallback.Dispose();
}
}
}
else if (Input.MouseReleased == MouseButtons.Right)
{
RemovePickingConstraint();
}
// Mouse movement
if (Input.MouseDown == MouseButtons.Right)
{
if (pickConstraint != null)
{
Vector3 newRayTo = GetRayTo(Input.MousePoint, Freelook.Eye, Freelook.Target, Graphics.FieldOfView);
if (pickConstraint.ConstraintType == TypedConstraintType.D6)
{
Generic6DofConstraint pickCon = pickConstraint as Generic6DofConstraint;
//keep it at the same picking distance
Vector3 rayFrom = Freelook.Eye;
Vector3 dir = newRayTo - rayFrom;
dir.Normalize();
dir *= oldPickingDist;
Vector3 newPivotB = rayFrom + dir;
Matrix tempFrameOffsetA = pickCon.FrameOffsetA;
tempFrameOffsetA.M41 = newPivotB.X;
tempFrameOffsetA.M42 = newPivotB.Y;
tempFrameOffsetA.M43 = newPivotB.Z;
pickCon.SetFrames(tempFrameOffsetA, pickCon.FrameOffsetB);
}
else
{
Point2PointConstraint pickCon = pickConstraint as Point2PointConstraint;
//keep it at the same picking distance
Vector3 rayFrom = Freelook.Eye;
Vector3 dir = newRayTo - rayFrom;
dir.Normalize();
dir *= oldPickingDist;
pickCon.PivotInB = rayFrom + dir;
}
}
}
}
public Ragdoll(DynamicsWorld ownerWorld, Vector3 positionOffset)
{
this.ownerWorld = ownerWorld;
// Setup the geometry
shapes[(int)BodyPart.Pelvis] = new CapsuleShape(0.15f, 0.20f);
shapes[(int)BodyPart.Spine] = new CapsuleShape(0.15f, 0.28f);
shapes[(int)BodyPart.Head] = new CapsuleShape(0.10f, 0.05f);
shapes[(int)BodyPart.LeftUpperLeg] = new CapsuleShape(0.07f, 0.45f);
shapes[(int)BodyPart.LeftLowerLeg] = new CapsuleShape(0.05f, 0.37f);
shapes[(int)BodyPart.RightUpperLeg] = new CapsuleShape(0.07f, 0.45f);
shapes[(int)BodyPart.RightLowerLeg] = new CapsuleShape(0.05f, 0.37f);
shapes[(int)BodyPart.LeftUpperArm] = new CapsuleShape(0.05f, 0.33f);
shapes[(int)BodyPart.LeftLowerArm] = new CapsuleShape(0.04f, 0.25f);
shapes[(int)BodyPart.RightUpperArm] = new CapsuleShape(0.05f, 0.33f);
shapes[(int)BodyPart.RightLowerArm] = new CapsuleShape(0.04f, 0.25f);
Matrix offset = Matrix.Translation(positionOffset);
Matrix transform;
transform = offset * Matrix.Translation(0, 1, 0);
bodies[(int)BodyPart.Pelvis] = LocalCreateRigidBody(1, transform, shapes[(int)BodyPart.Pelvis]);
transform = offset * Matrix.Translation(0, 1.2f, 0);
bodies[(int)BodyPart.Spine] = LocalCreateRigidBody(1, transform, shapes[(int)BodyPart.Spine]);
transform = offset * Matrix.Translation(0, 1.6f, 0);
bodies[(int)BodyPart.Head] = LocalCreateRigidBody(1, transform, shapes[(int)BodyPart.Head]);
transform = offset * Matrix.Translation(-0.18f, 0.6f, 0);
bodies[(int)BodyPart.LeftUpperLeg] = LocalCreateRigidBody(1, transform, shapes[(int)BodyPart.LeftUpperLeg]);
transform = offset * Matrix.Translation(-0.18f, 0.2f, 0);
bodies[(int)BodyPart.LeftLowerLeg] = LocalCreateRigidBody(1, transform, shapes[(int)BodyPart.LeftLowerLeg]);
transform = offset * Matrix.Translation(0.18f, 0.65f, 0);
bodies[(int)BodyPart.RightUpperLeg] = LocalCreateRigidBody(1, transform, shapes[(int)BodyPart.RightUpperLeg]);
transform = offset * Matrix.Translation(0.18f, 0.2f, 0);
bodies[(int)BodyPart.RightLowerLeg] = LocalCreateRigidBody(1, transform, shapes[(int)BodyPart.RightLowerLeg]);
transform = Matrix.RotationX((float)Math.PI / 2) * offset * Matrix.Translation(-0.35f, 1.45f, 0);
bodies[(int)BodyPart.LeftUpperArm] = LocalCreateRigidBody(1, transform, shapes[(int)BodyPart.LeftUpperArm]);
transform = Matrix.RotationY((float)Math.PI / 2) * offset * Matrix.Translation(-0.7f, 1.45f, 0);
bodies[(int)BodyPart.LeftLowerArm] = LocalCreateRigidBody(1, transform, shapes[(int)BodyPart.LeftLowerArm]);
transform = Matrix.RotationY(-(float)Math.PI / 2) * offset * Matrix.Translation(0.35f, 1.45f, 0);
bodies[(int)BodyPart.RightUpperArm] = LocalCreateRigidBody(1, transform, shapes[(int)BodyPart.RightUpperArm]);
transform = Matrix.RotationY(-(float)Math.PI / 2) * offset * Matrix.Translation(0.7f, 1.45f, 0);
bodies[(int)BodyPart.RightLowerArm] = LocalCreateRigidBody(1, transform, shapes[(int)BodyPart.RightLowerArm]);
// Setup some damping on the m_bodies
foreach (RigidBody body in bodies)
{
body.SetDamping(0.05f, 0.85f);
body.DeactivationTime = 0.8f;
body.SetSleepingThresholds(1.6f, 2.5f);
}
// Now setup the constraints
HingeConstraint hingeC;
ConeTwistConstraint coneC;
Matrix localA, localB;
localA = Matrix.RotationYawPitchRoll(0, (float)Math.PI / 2, 0) * Matrix.Translation(0, 0.15f, 0);
localB = Matrix.RotationYawPitchRoll(0, (float)Math.PI / 2, 0) * Matrix.Translation(0, -0.15f, 0);
hingeC = new HingeConstraint(bodies[(int)BodyPart.Pelvis], bodies[(int)BodyPart.Spine], localA, localB);
hingeC.SetLimit(-(float)Math.PI / 4, -(float)Math.PI / 2);
joints[(int)Joint.PelvisSpine] = hingeC;
hingeC.DebugDrawSize = ConstraintDebugSize;
ownerWorld.AddConstraint(joints[(int)Joint.PelvisSpine], true);
localA = Matrix.RotationYawPitchRoll(0, 0, (float)Math.PI / 2) * Matrix.Translation(0, 0.30f, 0);
localB = Matrix.RotationYawPitchRoll(0, 0, (float)Math.PI / 2) * Matrix.Translation(0, -0.14f, 0);
coneC = new ConeTwistConstraint(bodies[(int)BodyPart.Spine], bodies[(int)BodyPart.Head], localA, localB);
coneC.SetLimit((float)Math.PI / 4, (float)Math.PI / 4, (float)Math.PI / 2);
joints[(int)Joint.SpineHead] = coneC;
coneC.DebugDrawSize = ConstraintDebugSize;
ownerWorld.AddConstraint(joints[(int)Joint.SpineHead], true);
localA = Matrix.RotationYawPitchRoll(0, 0, -5 * (float)Math.PI / 4) * Matrix.Translation(-0.18f, -0.18f, 0);
localB = Matrix.RotationYawPitchRoll(0, 0, -5 * (float)Math.PI / 4) * Matrix.Translation(0, 0.225f, 0);
coneC = new ConeTwistConstraint(bodies[(int)BodyPart.Pelvis], bodies[(int)BodyPart.LeftUpperLeg], localA, localB);
coneC.SetLimit((float)Math.PI / 4, (float)Math.PI / 4, 0);
joints[(int)Joint.LeftHip] = coneC;
coneC.DebugDrawSize = ConstraintDebugSize;
ownerWorld.AddConstraint(joints[(int)Joint.LeftHip], true);
localA = Matrix.RotationYawPitchRoll(0, (float)Math.PI / 2, 0) * Matrix.Translation(0, -0.225f, 0);
localB = Matrix.RotationYawPitchRoll(0, (float)Math.PI / 2, 0) * Matrix.Translation(0, 0.185f, 0);
hingeC = new HingeConstraint(bodies[(int)BodyPart.LeftUpperLeg], bodies[(int)BodyPart.LeftLowerLeg], localA, localB);
hingeC.SetLimit(0, (float)Math.PI / 2);
joints[(int)Joint.LeftKnee] = hingeC;
hingeC.DebugDrawSize = ConstraintDebugSize;
ownerWorld.AddConstraint(joints[(int)Joint.LeftKnee], true);
localA = Matrix.RotationYawPitchRoll(0, 0, (float)Math.PI / 4) * Matrix.Translation(0.18f, -0.10f, 0);
localB = Matrix.RotationYawPitchRoll(0, 0, (float)Math.PI / 4) * Matrix.Translation(0, 0.225f, 0);
coneC = new ConeTwistConstraint(bodies[(int)BodyPart.Pelvis], bodies[(int)BodyPart.RightUpperLeg], localA, localB);
coneC.SetLimit((float)Math.PI / 4, (float)Math.PI / 4, 0);
joints[(int)Joint.RightHip] = coneC;
coneC.DebugDrawSize = ConstraintDebugSize;
ownerWorld.AddConstraint(joints[(int)Joint.RightHip], true);
localA = Matrix.RotationYawPitchRoll(0, (float)Math.PI / 2, 0) * Matrix.Translation(0, -0.225f, 0);
localB = Matrix.RotationYawPitchRoll(0, (float)Math.PI / 2, 0) * Matrix.Translation(0, 0.185f, 0);
hingeC = new HingeConstraint(bodies[(int)BodyPart.RightUpperLeg], bodies[(int)BodyPart.RightLowerLeg], localA, localB);
hingeC.SetLimit(0, (float)Math.PI / 2);
joints[(int)Joint.RightKnee] = hingeC;
hingeC.DebugDrawSize = ConstraintDebugSize;
ownerWorld.AddConstraint(joints[(int)Joint.RightKnee], true);
localA = Matrix.RotationYawPitchRoll(0, 0, (float)Math.PI) * Matrix.Translation(-0.2f, 0.15f, 0);
localB = Matrix.RotationYawPitchRoll(0, 0, (float)Math.PI / 2) * Matrix.Translation(0, -0.18f, 0);
coneC = new ConeTwistConstraint(bodies[(int)BodyPart.Spine], bodies[(int)BodyPart.LeftUpperArm], localA, localB);
coneC.SetLimit((float)Math.PI / 2, (float)Math.PI / 2, 0);
joints[(int)Joint.LeftShoulder] = coneC;
coneC.DebugDrawSize = ConstraintDebugSize;
ownerWorld.AddConstraint(joints[(int)Joint.LeftShoulder], true);
localA = Matrix.RotationYawPitchRoll(0, (float)Math.PI / 2, 0) * Matrix.Translation(0, 0.18f, 0);
localB = Matrix.RotationYawPitchRoll(0, (float)Math.PI / 2, 0) * Matrix.Translation(0, -0.14f, 0);
hingeC = new HingeConstraint(bodies[(int)BodyPart.LeftUpperArm], bodies[(int)BodyPart.LeftLowerArm], localA, localB);
hingeC.SetLimit(0, (float)Math.PI / 2);
joints[(int)Joint.LeftElbow] = hingeC;
hingeC.DebugDrawSize = ConstraintDebugSize;
ownerWorld.AddConstraint(joints[(int)Joint.LeftElbow], true);
localA = Matrix.RotationYawPitchRoll(0, 0, 0) * Matrix.Translation(0.2f, 0.15f, 0);
localB = Matrix.RotationYawPitchRoll(0, 0, (float)Math.PI / 2) * Matrix.Translation(0, -0.18f, 0);
coneC = new ConeTwistConstraint(bodies[(int)BodyPart.Spine], bodies[(int)BodyPart.RightUpperArm], localA, localB);
coneC.SetLimit((float)Math.PI / 2, (float)Math.PI / 2, 0);
joints[(int)Joint.RightShoulder] = coneC;
coneC.DebugDrawSize = ConstraintDebugSize;
ownerWorld.AddConstraint(joints[(int)Joint.RightShoulder], true);
localA = Matrix.RotationYawPitchRoll(0, (float)Math.PI / 2, 0) * Matrix.Translation(0, 0.18f, 0);
localB = Matrix.RotationYawPitchRoll(0, (float)Math.PI / 2, 0) * Matrix.Translation(0, -0.14f, 0);
hingeC = new HingeConstraint(bodies[(int)BodyPart.RightUpperArm], bodies[(int)BodyPart.RightLowerArm], localA, localB);
//hingeC.SetLimit(-(float)Math.PI / 2, 0);
hingeC.SetLimit(0, (float)Math.PI / 2);
joints[(int)Joint.RightElbow] = hingeC;
hingeC.DebugDrawSize = ConstraintDebugSize;
ownerWorld.AddConstraint(joints[(int)Joint.RightElbow], true);
}
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(new IndexedQuaternion(vAxis, 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);
}
}
private void SetupConstraints()
{
HingeConstraint hinge;
ConeTwistConstraint cone;
Matrix localA, localB;
localA = Matrix.RotationYawPitchRoll(PI_2, 0, 0) * Matrix.Translation(0, 0.15f, 0);
localB = Matrix.RotationYawPitchRoll(PI_2, 0, 0) * Matrix.Translation(0, -0.15f, 0);
hinge = new HingeConstraint(_bodies[(int)BodyPart.Pelvis], _bodies[(int)BodyPart.Spine], localA, localB);
hinge.SetLimit(-PI_4, PI_2);
_joints[(int)Joint.PelvisSpine] = hinge;
hinge.DebugDrawSize = ConstraintDebugSize;
_world.AddConstraint(_joints[(int)Joint.PelvisSpine], true);
localA = Matrix.RotationYawPitchRoll(0, 0, PI_2) * Matrix.Translation(0, 0.30f, 0);
localB = Matrix.RotationYawPitchRoll(0, 0, PI_2) * Matrix.Translation(0, -0.14f, 0);
cone = new ConeTwistConstraint(_bodies[(int)BodyPart.Spine], _bodies[(int)BodyPart.Head], localA, localB);
cone.SetLimit(PI_4, PI_4, PI_2);
_joints[(int)Joint.SpineHead] = cone;
cone.DebugDrawSize = ConstraintDebugSize;
_world.AddConstraint(_joints[(int)Joint.SpineHead], true);
localA = Matrix.RotationYawPitchRoll(0, 0, -PI_4 * 5) * Matrix.Translation(-0.18f, -0.18f, 0);
localB = Matrix.RotationYawPitchRoll(0, 0, -PI_4 * 5) * Matrix.Translation(0, 0.225f, 0);
cone = new ConeTwistConstraint(_bodies[(int)BodyPart.Pelvis], _bodies[(int)BodyPart.LeftUpperLeg], localA, localB);
cone.SetLimit(PI_4, PI_4, 0);
_joints[(int)Joint.LeftHip] = cone;
cone.DebugDrawSize = ConstraintDebugSize;
_world.AddConstraint(_joints[(int)Joint.LeftHip], true);
localA = Matrix.RotationYawPitchRoll(PI_2, 0, 0) * Matrix.Translation(0, -0.225f, 0);
localB = Matrix.RotationYawPitchRoll(PI_2, 0, 0) * Matrix.Translation(0, 0.185f, 0);
hinge = new HingeConstraint(_bodies[(int)BodyPart.LeftUpperLeg], _bodies[(int)BodyPart.LeftLowerLeg], localA, localB);
hinge.SetLimit(0, PI_2);
_joints[(int)Joint.LeftKnee] = hinge;
hinge.DebugDrawSize = ConstraintDebugSize;
_world.AddConstraint(_joints[(int)Joint.LeftKnee], true);
localA = Matrix.RotationYawPitchRoll(0, 0, PI_4) * Matrix.Translation(0.18f, -0.10f, 0);
localB = Matrix.RotationYawPitchRoll(0, 0, PI_4) * Matrix.Translation(0, 0.225f, 0);
cone = new ConeTwistConstraint(_bodies[(int)BodyPart.Pelvis], _bodies[(int)BodyPart.RightUpperLeg], localA, localB);
cone.SetLimit(PI_4, PI_4, 0);
_joints[(int)Joint.RightHip] = cone;
cone.DebugDrawSize = ConstraintDebugSize;
_world.AddConstraint(_joints[(int)Joint.RightHip], true);
localA = Matrix.RotationYawPitchRoll(PI_2, 0, 0) * Matrix.Translation(0, -0.225f, 0);
localB = Matrix.RotationYawPitchRoll(PI_2, 0, 0) * Matrix.Translation(0, 0.185f, 0);
hinge = new HingeConstraint(_bodies[(int)BodyPart.RightUpperLeg], _bodies[(int)BodyPart.RightLowerLeg], localA, localB);
hinge.SetLimit(0, PI_2);
_joints[(int)Joint.RightKnee] = hinge;
hinge.DebugDrawSize = ConstraintDebugSize;
_world.AddConstraint(_joints[(int)Joint.RightKnee], true);
localA = Matrix.RotationYawPitchRoll(0, 0, (float)Math.PI) * Matrix.Translation(-0.2f, 0.15f, 0);
localB = Matrix.RotationYawPitchRoll(0, 0, PI_2) * Matrix.Translation(0, -0.18f, 0);
cone = new ConeTwistConstraint(_bodies[(int)BodyPart.Spine], _bodies[(int)BodyPart.LeftUpperArm], localA, localB);
cone.SetLimit(PI_2, PI_2, 0);
_joints[(int)Joint.LeftShoulder] = cone;
cone.DebugDrawSize = ConstraintDebugSize;
_world.AddConstraint(_joints[(int)Joint.LeftShoulder], true);
localA = Matrix.RotationYawPitchRoll(PI_2, 0, 0) * Matrix.Translation(0, 0.18f, 0);
localB = Matrix.RotationYawPitchRoll(PI_2, 0, 0) * Matrix.Translation(0, -0.14f, 0);
hinge = new HingeConstraint(_bodies[(int)BodyPart.LeftUpperArm], _bodies[(int)BodyPart.LeftLowerArm], localA, localB);
hinge.SetLimit(0, PI_2);
_joints[(int)Joint.LeftElbow] = hinge;
hinge.DebugDrawSize = ConstraintDebugSize;
_world.AddConstraint(_joints[(int)Joint.LeftElbow], true);
localA = Matrix.RotationYawPitchRoll(0, 0, 0) * Matrix.Translation(0.2f, 0.15f, 0);
localB = Matrix.RotationYawPitchRoll(0, 0, PI_2) * Matrix.Translation(0, -0.18f, 0);
cone = new ConeTwistConstraint(_bodies[(int)BodyPart.Spine], _bodies[(int)BodyPart.RightUpperArm], localA, localB);
cone.SetLimit(PI_2, PI_2, 0);
_joints[(int)Joint.RightShoulder] = cone;
cone.DebugDrawSize = ConstraintDebugSize;
_world.AddConstraint(_joints[(int)Joint.RightShoulder], true);
localA = Matrix.RotationYawPitchRoll(PI_2, 0, 0) * Matrix.Translation(0, 0.18f, 0);
localB = Matrix.RotationYawPitchRoll(PI_2, 0, 0) * Matrix.Translation(0, -0.14f, 0);
hinge = new HingeConstraint(_bodies[(int)BodyPart.RightUpperArm], _bodies[(int)BodyPart.RightLowerArm], localA, localB);
//hinge.SetLimit(-PI_2, 0);
hinge.SetLimit(0, PI_2);
_joints[(int)Joint.RightElbow] = hinge;
hinge.DebugDrawSize = ConstraintDebugSize;
_world.AddConstraint(_joints[(int)Joint.RightElbow], true);
}
public virtual void OnHandleInput()
{
if (Input.KeysPressed.Count != 0)
{
switch (Input.KeysPressed[0])
{
case Keys.Escape:
case Keys.Q:
Graphics.Form.Close();
return;
case Keys.F3:
IsDebugDrawEnabled = !IsDebugDrawEnabled;
break;
case Keys.F8:
Input.ClearKeyCache();
GraphicsLibraryManager.ExitWithReload = true;
Graphics.Form.Close();
break;
case Keys.F11:
Graphics.IsFullScreen = !Graphics.IsFullScreen;
break;
case Keys.G:
//shadowsEnabled = !shadowsEnabled;
break;
case Keys.Space:
ShootBox(Freelook.Eye, GetRayTo(Input.MousePoint, Freelook.Eye, Freelook.Target, Graphics.FieldOfView));
break;
case Keys.Return:
ClientResetScene();
break;
}
}
if (Input.MousePressed != MouseButtons.None)
{
Vector3 rayTo = GetRayTo(Input.MousePoint, Freelook.Eye, Freelook.Target, Graphics.FieldOfView);
if (Input.MousePressed == MouseButtons.Right)
{
if (_world != null)
{
Vector3 rayFrom = Freelook.Eye;
ClosestRayResultCallback rayCallback = new ClosestRayResultCallback(ref rayFrom, ref rayTo);
_world.RayTest(ref rayFrom, ref rayTo, rayCallback);
if (rayCallback.HasHit)
{
Vector3 pickPos = rayCallback.HitPointWorld;
RigidBody body = rayCallback.CollisionObject as RigidBody;
if (body != null)
{
if (!(body.IsStaticObject || body.IsKinematicObject))
{
pickedBody = body;
pickedBody.ActivationState = ActivationState.DisableDeactivation;
Vector3 localPivot = Vector3.TransformCoordinate(pickPos, Matrix.Invert(body.CenterOfMassTransform));
if (Input.KeysDown.Contains(Keys.ShiftKey))
{
Generic6DofConstraint dof6 = new Generic6DofConstraint(body, Matrix.Translation(localPivot), false)
{
LinearLowerLimit = Vector3.Zero,
LinearUpperLimit = Vector3.Zero,
AngularLowerLimit = Vector3.Zero,
AngularUpperLimit = Vector3.Zero
};
_world.AddConstraint(dof6);
pickConstraint = dof6;
dof6.SetParam(ConstraintParam.StopCfm, 0.8f, 0);
dof6.SetParam(ConstraintParam.StopCfm, 0.8f, 1);
dof6.SetParam(ConstraintParam.StopCfm, 0.8f, 2);
dof6.SetParam(ConstraintParam.StopCfm, 0.8f, 3);
dof6.SetParam(ConstraintParam.StopCfm, 0.8f, 4);
dof6.SetParam(ConstraintParam.StopCfm, 0.8f, 5);
dof6.SetParam(ConstraintParam.StopErp, 0.1f, 0);
dof6.SetParam(ConstraintParam.StopErp, 0.1f, 1);
dof6.SetParam(ConstraintParam.StopErp, 0.1f, 2);
dof6.SetParam(ConstraintParam.StopErp, 0.1f, 3);
dof6.SetParam(ConstraintParam.StopErp, 0.1f, 4);
dof6.SetParam(ConstraintParam.StopErp, 0.1f, 5);
}
else
{
Point2PointConstraint p2p = new Point2PointConstraint(body, localPivot);
_world.AddConstraint(p2p);
pickConstraint = p2p;
p2p.Setting.ImpulseClamp = 30;
//very weak constraint for picking
p2p.Setting.Tau = 0.001f;
/*
* p2p.SetParam(ConstraintParams.Cfm, 0.8f, 0);
* p2p.SetParam(ConstraintParams.Cfm, 0.8f, 1);
* p2p.SetParam(ConstraintParams.Cfm, 0.8f, 2);
* p2p.SetParam(ConstraintParams.Erp, 0.1f, 0);
* p2p.SetParam(ConstraintParams.Erp, 0.1f, 1);
* p2p.SetParam(ConstraintParams.Erp, 0.1f, 2);
*/
}
}
}
else
{
MultiBodyLinkCollider multiCol = rayCallback.CollisionObject as MultiBodyLinkCollider;
if (multiCol != null && multiCol.MultiBody != null)
{
MultiBody mb = multiCol.MultiBody;
prevCanSleep = mb.CanSleep;
mb.CanSleep = false;
Vector3 pivotInA = mb.WorldPosToLocal(multiCol.Link, pickPos);
MultiBodyPoint2Point p2p = new MultiBodyPoint2Point(mb, multiCol.Link, null, pivotInA, pickPos);
p2p.MaxAppliedImpulse = 2;
(World as MultiBodyDynamicsWorld).AddMultiBodyConstraint(p2p);
pickingMultiBodyPoint2Point = p2p;
}
}
oldPickingDist = (pickPos - rayFrom).Length;
}
rayCallback.Dispose();
}
}
}
else if (Input.MouseReleased == MouseButtons.Right)
{
RemovePickingConstraint();
}
// Mouse movement
if (Input.MouseDown == MouseButtons.Right)
{
MovePickedBody();
}
}
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);
}