public static void Update(ref BulletPhysicsComponent bpc)
{
foreach (var entry in _flippers)
{
PhyFlipper phyFlipper = entry.Value;
phyFlipper._FlipperUpdate(ref bpc);
// debug
var dbg = EngineProvider <IDebugUI> .Get();
bool isChanged = false;
float sa = phyFlipper._startAngle * 180.0f / Mathf.PI;
float se = phyFlipper._endAngle * 180.0f / Mathf.PI;
isChanged |= dbg.GetProperty(phyFlipper.dbgPropStartAngle, ref sa);
isChanged |= dbg.GetProperty(phyFlipper.dbgPropEndAngle, ref se);
if (isChanged)
{
phyFlipper._startAngle = sa * Mathf.PI / 180.0f;
phyFlipper._endAngle = se * Mathf.PI / 180.0f;
HingeConstraint hinge = (HingeConstraint)phyFlipper._constraint;
if (phyFlipper.RotationDirection == 1)
{
hinge.SetLimit(phyFlipper._startAngle, phyFlipper._endAngle, 0.0f);
}
else
{
hinge.SetLimit(phyFlipper._endAngle, phyFlipper._startAngle, 0.0f);
}
}
}
}
public void Evaluate(int SpreadMax)
{
IConstraintContainer inputWorld = this.contraintContainer[0];
if (inputWorld != null)
{
this.persister.UpdateWorld(inputWorld);
for (int i = 0; i < SpreadMax; i++)
{
if (FCreate[i])
{
RigidBody body = FBody[i];
if (body != null)
{
HingeConstraint cst = new HingeConstraint(body, this.FPivot1[i].ToBulletVector(), this.FAxis[i].ToBulletVector());
cst.SetLimit(this.FLimitLow[i] * (float)Math.PI * 2.0f, this.FLimitHigh[i] * (float)Math.PI * 2.0f);
cst.DebugDrawSize = 5.0f;
this.persister.Append(cst);
}
}
}
this.persister.Flush();
}
else
{
this.constraintsOutput.SliceCount = 0;
}
}
// Create a door using a hinge constraint attached to the world
private void CreateDoor()
{
const float mass = 1.0f;
var doorShape = new BoxShape(2.0f, 5.0f, 0.2f);
CollisionShapes.Add(doorShape);
RigidBody doorBody = LocalCreateRigidBody(mass, Matrix.Translation(-5.0f, -2.0f, 0.0f), doorShape);
doorBody.ActivationState = ActivationState.DisableDeactivation;
var pivotA = new Vector3(10.0f + 2.1f, -2.0f, 0.0f); // right next to the door slightly outside
var axisA = Vector3.UnitY; // pointing upwards, aka Y-axis
var hinge = new HingeConstraint(doorBody, pivotA, axisA);
hinge.DebugDrawSize = 5;
//hinge.SetLimit(0.0f, (float)Math.PI / 2);
// test problem values
//hinge.SetLimit(-(float)Math.PI, (float)Math.PI * 0.8f);
//hinge.SetLimit(1, -1);
//hinge.SetLimit(-(float)Math.PI * 0.8f, (float)Math.PI);
//hinge.SetLimit(-(float)Math.PI * 0.8f, (float)Math.PI, 0.9f, 0.3f, 0.0f);
//hinge.SetLimit(-(float)Math.PI * 0.8f, (float)Math.PI, 0.9f, 0.01f, 0.0f); // "sticky limits"
hinge.SetLimit(-(float)Math.PI * 0.25f, (float)Math.PI * 0.25f);
//hinge.SetLimit(0, 0);
World.AddConstraint(hinge);
}
// Create a Hinge joint between two dynamic bodies
private void CreateHinge()
{
// static body (parent) on top
RigidBody bodyA = LocalCreateRigidBody(1.0f, Matrix.Translation(-20, -2, 0), cubeShape);
bodyA.ActivationState = ActivationState.DisableDeactivation;
// dynamic body
RigidBody bodyB = LocalCreateRigidBody(10.0f, Matrix.Translation(-30, -2, 0), cubeShape);
bodyB.ActivationState = ActivationState.DisableDeactivation;
// add some data to build constraint frames
var axisA = new Vector3(0, 1, 0);
var axisB = new Vector3(0, 1, 0);
var pivotA = new Vector3(-5, 0, 0);
var pivotB = new Vector3(5, 0, 0);
var hinge = new HingeConstraint(bodyA, bodyB, pivotA, pivotB, axisA, axisB);
hinge.SetLimit(-(float)Math.PI / 4, (float)Math.PI / 4);
// draw constraint frames and limits for debugging
hinge.DebugDrawSize = 5;
World.AddConstraint(hinge, true);
}
/// <summary>
/// Creates the joint data
/// </summary>
public void CreateJoint()
{
if (joint != null || GetSkeletalJoint() == null)
{
return;
}
switch (GetSkeletalJoint().GetJointType())
{
case SkeletalJointType.ROTATIONAL:
RotationalJoint_Base nodeR = (RotationalJoint_Base)GetSkeletalJoint();
CollisionObject parentObject = ((BulletRigidNode)GetParent()).BulletObject;
WheelDriverMeta wheel = GetSkeletalJoint().cDriver.GetInfo <WheelDriverMeta>();
Vector3 pivot = nodeR.basePoint.Convert();
Matrix4 locJ, locP; //Local Joint Pivot, Local Parent Pivot
BulletObject.WorldTransform = parentObject.WorldTransform * Matrix4.CreateTranslation(pivot);
if (debug)
{
Console.WriteLine("Pivot at " + pivot);
}
GetFrames(nodeR.basePoint.Convert(), parentObject.WorldTransform, BulletObject.WorldTransform, out locP, out locJ);
//HingeConstraint temp = new HingeConstraint((RigidBody)parentObject, /*(RigidBody)*/BulletObject, locP, locJ);
HingeConstraint temp = new HingeConstraint((RigidBody)parentObject, BulletObject, pivot, Vector3.Zero, nodeR.axis.Convert(), nodeR.axis.Convert());
joint = temp;
temp.SetAxis(nodeR.axis.Convert());
if (nodeR.hasAngularLimit)
{
temp.SetLimit(nodeR.angularLimitLow, nodeR.angularLimitHigh);
}
//also need to find a less screwy way to do this
Update = (f) => { (/*(RigidBody)*/ BulletObject).ApplyTorque(nodeR.axis.Convert() * f * 25); };
if (debug)
{
Console.WriteLine("{0} joint made", wheel == null ? "Rotational" : "Wheel");
}
break;
default:
if (debug)
{
Console.WriteLine("Received joint of type {0}", GetSkeletalJoint().GetJointType());
}
break;
}
}
private static HingeConstraint CreateHipJoint(RigidBody root, RigidBody thigh, float direction)
{
Vector3 origin = ToCartesian(direction, BodyRadius);
Matrix rootFrame = Matrix.RotationYawPitchRoll(-direction, 0, 0) * Matrix.Translation(origin);
Matrix thighFrame = rootFrame * root.WorldTransform * Matrix.Invert(thigh.WorldTransform);
var hinge = new HingeConstraint(root, thigh, rootFrame, thighFrame);
hinge.SetLimit(-0.75f * PI_4, PI_8);
//hingeC.SetLimit(-0.1f, 0.1f);
return(hinge);
}
private static HingeConstraint CreateKneeJoint(RigidBody root, RigidBody thigh, RigidBody shin, float direction)
{
Vector3 origin = ToCartesian(direction, BodyRadius + ThighLength);
Matrix localFrame = Matrix.RotationYawPitchRoll(-direction, 0, 0) * Matrix.Translation(origin);
Matrix thighFrame = localFrame * root.WorldTransform * Matrix.Invert(thigh.WorldTransform);
Matrix shinFrame = localFrame * root.WorldTransform * Matrix.Invert(shin.WorldTransform);
var hinge = new HingeConstraint(thigh, shin, thighFrame, shinFrame);
//hingeC.SetLimit(-0.01f, 0.01f);
hinge.SetLimit(-PI_8, 0.2f);
return(hinge);
}
TypedConstraint _AddFlipperHinge()
{
float hh = _height * 0.5f;
var hinge = new HingeConstraint(
body,
new Vector3(0, 0, hh),
Vector3.UnitZ,
false);
body.ActivationState = ActivationState.DisableDeactivation;
body.SetSleepingThresholds(float.MaxValue, 0.0f); // no sleep for flippers
if (RotationDirection == 1)
{
hinge.SetLimit(_startAngle, _endAngle, 0.0f);
}
else
{
hinge.SetLimit(_endAngle, _startAngle, 0.0f);
}
return(hinge);
}
HingeConstraint _AddGateHinge()
{
Matrix AFrame = Matrix.RotationX(0) * Matrix.RotationZ(90.0f.ToRad()) * Matrix.RotationY(90.0f.ToRad()) * Matrix.Translation(offset); // rotatione required to make X axis pivot
Matrix BFrame = AFrame * matrix;
//var hinge = new HingeConstraint(
// body,
// Vector3.Zero + base.offset,
// Vector3.UnitX,
// true);
//var ma = hinge.AFrame;
//var mb = hinge.BFrame;
//var oa = hinge.FrameOffsetA;
//var ob = hinge.FrameOffsetB;
//var im = matrix;
//im.Invert();
//var mb2 = mb * im;
//hinge.Dispose();
var hinge = new HingeConstraint(
body,
TypedConstraint.GetFixedBody(),
AFrame,
BFrame,
true) ;
if (_type!= GateType.Spinner)
{
hinge.SetLimit(_startAngle, _endAngle, 0.0f); // revers angles to have same as in VPX behavior
}
//body.ActivationState = ActivationState.DisableDeactivation;
//body.SetSleepingThresholds(float.MaxValue, 0.0f); // no sleep for flippers
return hinge;
}
protected override void OnInitializePhysics()
{
SetupEmptyDynamicsWorld();
CollisionShape groundShape = new BoxShape(50, 1, 50);
//CollisionShape groundShape = new StaticPlaneShape(Vector3.UnitY, 40);
CollisionShapes.Add(groundShape);
RigidBody body = LocalCreateRigidBody(0, Matrix.Translation(0, -16, 0), groundShape);
body.UserObject = "Ground";
CollisionShape shape = new BoxShape(new Vector3(CubeHalfExtents));
CollisionShapes.Add(shape);
const float THETA = (float)Math.PI / 4.0f;
float L_1 = 2 - (float)Math.Tan(THETA);
float L_2 = 1 / (float)Math.Cos(THETA);
float RATIO = L_2 / L_1;
RigidBody bodyA;
RigidBody bodyB;
CollisionShape cylA = new CylinderShape(0.2f, 0.25f, 0.2f);
CollisionShape cylB = new CylinderShape(L_1, 0.025f, L_1);
CompoundShape cyl0 = new CompoundShape();
cyl0.AddChildShape(Matrix.Identity, cylA);
cyl0.AddChildShape(Matrix.Identity, cylB);
float mass = 6.28f;
Vector3 localInertia;
cyl0.CalculateLocalInertia(mass, out localInertia);
RigidBodyConstructionInfo ci = new RigidBodyConstructionInfo(mass, null, cyl0, localInertia);
ci.StartWorldTransform = Matrix.Translation(-8, 1, -8);
body = new RigidBody(ci); //1,0,cyl0,localInertia);
World.AddRigidBody(body);
body.LinearFactor = Vector3.Zero;
body.AngularFactor = new Vector3(0, 1, 0);
bodyA = body;
cylA = new CylinderShape(0.2f, 0.26f, 0.2f);
cylB = new CylinderShape(L_2, 0.025f, L_2);
cyl0 = new CompoundShape();
cyl0.AddChildShape(Matrix.Identity, cylA);
cyl0.AddChildShape(Matrix.Identity, cylB);
mass = 6.28f;
cyl0.CalculateLocalInertia(mass, out localInertia);
ci = new RigidBodyConstructionInfo(mass, null, cyl0, localInertia);
Quaternion orn = Quaternion.RotationAxis(new Vector3(0, 0, 1), -THETA);
ci.StartWorldTransform = Matrix.RotationQuaternion(orn) * Matrix.Translation(-10, 2, -8);
body = new RigidBody(ci);//1,0,cyl0,localInertia);
body.LinearFactor = Vector3.Zero;
HingeConstraint hinge = new HingeConstraint(body, Vector3.Zero, new Vector3(0, 1, 0), true);
World.AddConstraint(hinge);
bodyB = body;
body.AngularVelocity = new Vector3(0, 3, 0);
World.AddRigidBody(body);
Vector3 axisA = new Vector3(0, 1, 0);
Vector3 axisB = new Vector3(0, 1, 0);
orn = Quaternion.RotationAxis(new Vector3(0, 0, 1), -THETA);
Matrix mat = Matrix.RotationQuaternion(orn);
axisB = new Vector3(mat.M21, mat.M22, mat.M23);
GearConstraint gear = new GearConstraint(bodyA, bodyB, axisA, axisB, RATIO);
World.AddConstraint(gear, true);
mass = 1.0f;
RigidBody body0 = LocalCreateRigidBody(mass, Matrix.Translation(0, 20, 0), shape);
RigidBody body1 = null;//LocalCreateRigidBody(mass, Matrix.Translation(2*CUBE_HALF_EXTENTS,20,0), shape);
//RigidBody body1 = LocalCreateRigidBody(0, Matrix.Translation(2*CUBE_HALF_EXTENTS,20,0), null);
//body1.ActivationState = ActivationState.DisableDeactivation;
//body1.SetDamping(0.3f, 0.3f);
Vector3 pivotInA = new Vector3(CubeHalfExtents, -CubeHalfExtents, -CubeHalfExtents);
Vector3 axisInA = new Vector3(0, 0, 1);
Vector3 pivotInB;
if (body1 != null)
{
Matrix transform = Matrix.Invert(body1.CenterOfMassTransform) * body0.CenterOfMassTransform;
pivotInB = Vector3.TransformCoordinate(pivotInA, transform);
}
else
{
pivotInB = pivotInA;
}
Vector3 axisInB;
if (body1 != null)
{
Matrix transform = Matrix.Invert(body1.CenterOfMassTransform) * body1.CenterOfMassTransform;
axisInB = Vector3.TransformCoordinate(axisInA, transform);
}
else
{
axisInB = Vector3.TransformCoordinate(axisInA, body0.CenterOfMassTransform);
}
#if P2P
{
TypedConstraint p2p = new Point2PointConstraint(body0, pivotInA);
//TypedConstraint p2p = new Point2PointConstraint(body0, body1, pivotInA, pivotInB);
//TypedConstraint hinge = new HingeConstraint(body0, body1, pivotInA, pivotInB, axisInA, axisInB);
World.AddConstraint(p2p);
p2p.DebugDrawSize = 5;
}
#else
{
hinge = new HingeConstraint(body0, pivotInA, axisInA);
//use zero targetVelocity and a small maxMotorImpulse to simulate joint friction
//float targetVelocity = 0.f;
//float maxMotorImpulse = 0.01;
const float targetVelocity = 1.0f;
const float maxMotorImpulse = 1.0f;
hinge.EnableAngularMotor(true, targetVelocity, maxMotorImpulse);
World.AddConstraint(hinge);
hinge.DebugDrawSize = 5;
}
#endif
RigidBody pRbA1 = LocalCreateRigidBody(mass, Matrix.Translation(-20, 0, 30), shape);
//RigidBody pRbA1 = LocalCreateRigidBody(0.0f, Matrix.Translation(-20, 0, 30), shape);
pRbA1.ActivationState = ActivationState.DisableDeactivation;
// add dynamic rigid body B1
RigidBody pRbB1 = LocalCreateRigidBody(mass, Matrix.Translation(-20, 0, 30), shape);
//RigidBody pRbB1 = LocalCreateRigidBody(0.0f, Matrix.Translation(-20, 0, 30), shape);
pRbB1.ActivationState = ActivationState.DisableDeactivation;
// create slider constraint between A1 and B1 and add it to world
SliderConstraint spSlider1 = new SliderConstraint(pRbA1, pRbB1, Matrix.Identity, Matrix.Identity, true);
//spSlider1 = new SliderConstraint(pRbA1, pRbB1, Matrix.Identity, Matrix.Identity, false);
spSlider1.LowerLinearLimit = -15.0f;
spSlider1.UpperLinearLimit = -5.0f;
spSlider1.LowerLinearLimit = 5.0f;
spSlider1.UpperLinearLimit = 15.0f;
spSlider1.LowerLinearLimit = -10.0f;
spSlider1.UpperLinearLimit = -10.0f;
spSlider1.LowerAngularLimit = -(float)Math.PI / 3.0f;
spSlider1.UpperAngularLimit = (float)Math.PI / 3.0f;
World.AddConstraint(spSlider1, true);
spSlider1.DebugDrawSize = 5.0f;
//create a slider, using the generic D6 constraint
Vector3 sliderWorldPos = new Vector3(0, 10, 0);
Vector3 sliderAxis = Vector3.UnitX;
const float angle = 0; //SIMD_RADS_PER_DEG * 10.f;
Matrix trans = Matrix.RotationAxis(sliderAxis, angle) * Matrix.Translation(sliderWorldPos);
d6body0 = LocalCreateRigidBody(mass, trans, shape);
d6body0.ActivationState = ActivationState.DisableDeactivation;
RigidBody fixedBody1 = LocalCreateRigidBody(0, trans, null);
World.AddRigidBody(fixedBody1);
Matrix frameInA = Matrix.Translation(0, 5, 0);
Matrix frameInB = Matrix.Translation(0, 5, 0);
//bool useLinearReferenceFrameA = false;//use fixed frame B for linear llimits
const bool useLinearReferenceFrameA = true; //use fixed frame A for linear llimits
spSlider6Dof = new Generic6DofConstraint(fixedBody1, d6body0, frameInA, frameInB, useLinearReferenceFrameA)
{
LinearLowerLimit = lowerSliderLimit,
LinearUpperLimit = hiSliderLimit,
//range should be small, otherwise singularities will 'explode' the constraint
//AngularLowerLimit = new Vector3(-1.5f,0,0),
//AngularUpperLimit = new Vector3(1.5f,0,0),
//AngularLowerLimit = new Vector3(0,0,0),
//AngularUpperLimit = new Vector3(0,0,0),
AngularLowerLimit = new Vector3((float)-Math.PI, 0, 0),
AngularUpperLimit = new Vector3(1.5f, 0, 0)
};
//spSlider6Dof.TranslationalLimitMotor.EnableMotor[0] = true;
spSlider6Dof.TranslationalLimitMotor.TargetVelocity = new Vector3(-5.0f, 0, 0);
spSlider6Dof.TranslationalLimitMotor.MaxMotorForce = new Vector3(0.1f, 0, 0);
World.AddConstraint(spSlider6Dof);
spSlider6Dof.DebugDrawSize = 5;
// create a door using hinge constraint attached to the world
CollisionShape pDoorShape = new BoxShape(2.0f, 5.0f, 0.2f);
CollisionShapes.Add(pDoorShape);
RigidBody pDoorBody = LocalCreateRigidBody(1.0f, Matrix.Translation(-5.0f, -2.0f, 0.0f), pDoorShape);
pDoorBody.ActivationState = ActivationState.DisableDeactivation;
Vector3 btPivotA = new Vector3(10.0f + 2.1f, -2.0f, 0.0f); // right next to the door slightly outside
Vector3 btAxisA = Vector3.UnitY; // pointing upwards, aka Y-axis
spDoorHinge = new HingeConstraint(pDoorBody, btPivotA, btAxisA);
//spDoorHinge.SetLimit(0.0f, (float)Math.PI / 2);
// test problem values
//spDoorHinge.SetLimit(-(float)Math.PI, (float)Math.PI * 0.8f);
//spDoorHinge.SetLimit(1, -1);
//spDoorHinge.SetLimit(-(float)Math.PI * 0.8f, (float)Math.PI);
//spDoorHinge.SetLimit(-(float)Math.PI * 0.8f, (float)Math.PI, 0.9f, 0.3f, 0.0f);
//spDoorHinge.SetLimit(-(float)Math.PI * 0.8f, (float)Math.PI, 0.9f, 0.01f, 0.0f); // "sticky limits"
spDoorHinge.SetLimit(-(float)Math.PI * 0.25f, (float)Math.PI * 0.25f);
//spDoorHinge.SetLimit(0, 0);
World.AddConstraint(spDoorHinge);
spDoorHinge.DebugDrawSize = 5;
RigidBody pDropBody = LocalCreateRigidBody(10.0f, Matrix.Translation(-5.0f, 2.0f, 0.0f), shape);
// create a generic 6DOF constraint
//RigidBody pBodyA = LocalCreateRigidBody(mass, Matrix.Translation(10.0f, 6.0f, 0), shape);
RigidBody pBodyA = LocalCreateRigidBody(0, Matrix.Translation(10, 6, 0), shape);
//RigidBody pBodyA = LocalCreateRigidBody(0, Matrix.Translation(10, 6, 0), null);
pBodyA.ActivationState = ActivationState.DisableDeactivation;
RigidBody pBodyB = LocalCreateRigidBody(mass, Matrix.Translation(0, 6, 0), shape);
//RigidBody pBodyB = LocalCreateRigidBody(0, Matrix.Translation(0, 6, 0), shape);
pBodyB.ActivationState = ActivationState.DisableDeactivation;
frameInA = Matrix.Translation(-5, 0, 0);
frameInB = Matrix.Translation(5, 0, 0);
Generic6DofConstraint pGen6DOF = new Generic6DofConstraint(pBodyA, pBodyB, frameInA, frameInB, true);
//Generic6DofConstraint pGen6DOF = new Generic6DofConstraint(pBodyA, pBodyB, frameInA, frameInB, false);
pGen6DOF.LinearLowerLimit = new Vector3(-10, -2, -1);
pGen6DOF.LinearUpperLimit = new Vector3(10, 2, 1);
//pGen6DOF.LinearLowerLimit = new Vector3(-10, 0, 0);
//pGen6DOF.LinearUpperLimit = new Vector3(10, 0, 0);
//pGen6DOF.LinearLowerLimit = new Vector3(0, 0, 0);
//pGen6DOF.LinearUpperLimit = new Vector3(0, 0, 0);
//pGen6DOF.TranslationalLimitMotor.EnableMotor[0] = true;
//pGen6DOF.TranslationalLimitMotor.TargetVelocity = new Vector3(5, 0, 0);
//pGen6DOF.TranslationalLimitMotor.MaxMotorForce = new Vector3(0.1f, 0, 0);
//pGen6DOF.AngularLowerLimit = new Vector3(0, (float)Math.PI * 0.9f, 0);
//pGen6DOF.AngularUpperLimit = new Vector3(0, -(float)Math.PI * 0.9f, 0);
//pGen6DOF.AngularLowerLimit = new Vector3(0, 0, -(float)Math.PI);
//pGen6DOF.AngularUpperLimit = new Vector3(0, 0, (float)Math.PI);
pGen6DOF.AngularLowerLimit = new Vector3(-(float)Math.PI / 4, -0.75f, -(float)Math.PI * 0.4f);
pGen6DOF.AngularUpperLimit = new Vector3((float)Math.PI / 4, 0.75f, (float)Math.PI * 0.4f);
//pGen6DOF.AngularLowerLimit = new Vector3(0, -0.75f, (float)Math.PI * 0.8f);
//pGen6DOF.AngularUpperLimit = new Vector3(0, 0.75f, -(float)Math.PI * 0.8f);
//pGen6DOF.AngularLowerLimit = new Vector3(0, -(float)Math.PI * 0.8f, (float)Math.PI * 1.98f);
//pGen6DOF.AngularUpperLimit = new Vector3(0, (float)Math.PI * 0.8f, -(float)Math.PI * 1.98f);
//pGen6DOF.AngularLowerLimit = new Vector3(-0.75f, -0.5f, -0.5f);
//pGen6DOF.AngularUpperLimit = new Vector3(0.75f, 0.5f, 0.5f);
//pGen6DOF.AngularLowerLimit = new Vector3(-0.75f, 0, 0);
//pGen6DOF.AngularUpperLimit = new Vector3(0.75f, 0, 0);
//pGen6DOF.AngularLowerLimit = new Vector3(0, -0.7f, 0);
//pGen6DOF.AngularUpperLimit = new Vector3(0, 0.7f, 0);
//pGen6DOF.AngularLowerLimit = new Vector3(-1, 0, 0);
//pGen6DOF.AngularUpperLimit = new Vector3(1, 0, 0);
// create a ConeTwist constraint
pBodyA = LocalCreateRigidBody(1.0f, Matrix.Translation(-10, 5, 0), shape);
//pBodyA = LocalCreateRigidBody(0, Matrix.Translation(-10, 5, 0), shape);
pBodyA.ActivationState = ActivationState.DisableDeactivation;
pBodyB = LocalCreateRigidBody(0, Matrix.Translation(-10, -5, 0), shape);
//pBodyB = LocalCreateRigidBody(1.0f, Matrix.Translation(-10, -5, 0), shape);
frameInA = Matrix.RotationYawPitchRoll(0, 0, (float)Math.PI / 2);
frameInA *= Matrix.Translation(0, -5, 0);
frameInB = Matrix.RotationYawPitchRoll(0, 0, (float)Math.PI / 2);
frameInB *= Matrix.Translation(0, 5, 0);
coneTwist = new ConeTwistConstraint(pBodyA, pBodyB, frameInA, frameInB);
//coneTwist.SetLimit((float)Math.PI / 4, (float)Math.PI / 4, (float)Math.PI * 0.8f);
//coneTwist.SetLimit((((float)Math.PI / 4) * 0.6f), (float)Math.PI / 4, (float)Math.PI * 0.8f, 1.0f); // soft limit == hard limit
coneTwist.SetLimit((((float)Math.PI / 4) * 0.6f), (float)Math.PI / 4, (float)Math.PI * 0.8f, 0.5f);
World.AddConstraint(coneTwist, true);
coneTwist.DebugDrawSize = 5;
// Hinge connected to the world, with motor (to hinge motor with new and old constraint solver)
RigidBody pBody = LocalCreateRigidBody(1.0f, Matrix.Identity, shape);
pBody.ActivationState = ActivationState.DisableDeactivation;
Vector3 pivotA = new Vector3(10.0f, 0.0f, 0.0f);
btAxisA = new Vector3(0.0f, 0.0f, 1.0f);
HingeConstraint pHinge = new HingeConstraint(pBody, pivotA, btAxisA);
//pHinge.EnableAngularMotor(true, -1.0f, 0.165f); // use for the old solver
pHinge.EnableAngularMotor(true, -1.0f, 1.65f); // use for the new SIMD solver
World.AddConstraint(pHinge);
pHinge.DebugDrawSize = 5;
// create a universal joint using generic 6DOF constraint
// create two rigid bodies
// static bodyA (parent) on top:
pBodyA = LocalCreateRigidBody(0, Matrix.Translation(20, 4, 0), shape);
pBodyA.ActivationState = ActivationState.DisableDeactivation;
// dynamic bodyB (child) below it :
pBodyB = LocalCreateRigidBody(1.0f, Matrix.Translation(20, 0, 0), shape);
pBodyB.ActivationState = ActivationState.DisableDeactivation;
// add some (arbitrary) data to build constraint frames
Vector3 parentAxis = new Vector3(1, 0, 0);
Vector3 childAxis = new Vector3(0, 0, 1);
Vector3 anchor = new Vector3(20, 2, 0);
UniversalConstraint pUniv = new UniversalConstraint(pBodyA, pBodyB, anchor, parentAxis, childAxis);
pUniv.SetLowerLimit(-(float)Math.PI / 4, -(float)Math.PI / 4);
pUniv.SetUpperLimit((float)Math.PI / 4, (float)Math.PI / 4);
// add constraint to world
World.AddConstraint(pUniv, true);
// draw constraint frames and limits for debugging
pUniv.DebugDrawSize = 5;
World.AddConstraint(pGen6DOF, true);
pGen6DOF.DebugDrawSize = 5;
// create a generic 6DOF constraint with springs
pBodyA = LocalCreateRigidBody(0, Matrix.Translation(-20, 16, 0), shape);
pBodyA.ActivationState = ActivationState.DisableDeactivation;
pBodyB = LocalCreateRigidBody(1.0f, Matrix.Translation(-10, 16, 0), shape);
pBodyB.ActivationState = ActivationState.DisableDeactivation;
frameInA = Matrix.Translation(10, 0, 0);
frameInB = Matrix.Identity;
Generic6DofSpringConstraint pGen6DOFSpring = new Generic6DofSpringConstraint(pBodyA, pBodyB, frameInA, frameInB, true)
{
LinearUpperLimit = new Vector3(5, 0, 0),
LinearLowerLimit = new Vector3(-5, 0, 0),
AngularLowerLimit = new Vector3(0, 0, -1.5f),
AngularUpperLimit = new Vector3(0, 0, 1.5f),
DebugDrawSize = 5
};
World.AddConstraint(pGen6DOFSpring, true);
pGen6DOFSpring.EnableSpring(0, true);
pGen6DOFSpring.SetStiffness(0, 39.478f);
pGen6DOFSpring.SetDamping(0, 0.5f);
pGen6DOFSpring.EnableSpring(5, true);
pGen6DOFSpring.SetStiffness(5, 39.478f);
pGen6DOFSpring.SetDamping(0, 0.3f);
pGen6DOFSpring.SetEquilibriumPoint();
// create a Hinge2 joint
// create two rigid bodies
// static bodyA (parent) on top:
pBodyA = LocalCreateRigidBody(0, Matrix.Translation(-20, 4, 0), shape);
pBodyA.ActivationState = ActivationState.DisableDeactivation;
// dynamic bodyB (child) below it :
pBodyB = LocalCreateRigidBody(1.0f, Matrix.Translation(-20, 0, 0), shape);
pBodyB.ActivationState = ActivationState.DisableDeactivation;
// add some data to build constraint frames
parentAxis = new Vector3(0, 1, 0);
childAxis = new Vector3(1, 0, 0);
anchor = new Vector3(-20, 0, 0);
Hinge2Constraint pHinge2 = new Hinge2Constraint(pBodyA, pBodyB, anchor, parentAxis, childAxis);
pHinge2.SetLowerLimit(-(float)Math.PI / 4);
pHinge2.SetUpperLimit((float)Math.PI / 4);
// add constraint to world
World.AddConstraint(pHinge2, true);
// draw constraint frames and limits for debugging
pHinge2.DebugDrawSize = 5;
// create a Hinge joint between two dynamic bodies
// create two rigid bodies
// static bodyA (parent) on top:
pBodyA = LocalCreateRigidBody(1.0f, Matrix.Translation(-20, -2, 0), shape);
pBodyA.ActivationState = ActivationState.DisableDeactivation;
// dynamic bodyB:
pBodyB = LocalCreateRigidBody(10.0f, Matrix.Translation(-30, -2, 0), shape);
pBodyB.ActivationState = ActivationState.DisableDeactivation;
// add some data to build constraint frames
axisA = new Vector3(0, 1, 0);
axisB = new Vector3(0, 1, 0);
Vector3 pivotA2 = new Vector3(-5, 0, 0);
Vector3 pivotB = new Vector3(5, 0, 0);
spHingeDynAB = new HingeConstraint(pBodyA, pBodyB, pivotA2, pivotB, axisA, axisB);
spHingeDynAB.SetLimit(-(float)Math.PI / 4, (float)Math.PI / 4);
// add constraint to world
World.AddConstraint(spHingeDynAB, true);
// draw constraint frames and limits for debugging
spHingeDynAB.DebugDrawSize = 5;
}
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 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 static Ragdoll Build(string modelName, BuildOption buildOption)
//{
// switch (buildOption)
// {
// case BuildOption.Default:
// var defultOption = PhyConfiguration.GetValue("BuildOption");
// Debug.LogLine("Default ragdoll build option is [{0}]", defultOption);
// return Build(modelName, (BuildOption)Enum.Parse(buildOption.GetType(),defultOption));
// case BuildOption.FromFile:
// break;
// case BuildOption.Bipped:
// {
// var info = BippedBone.Get(modelName);
// if (info != null)
// return BuildBipped(IEngineStudio.Mod_ForName(modelName,true),info,BoneAccessor.GetCurrent());
// Debug.LogLine("BippedBone missing. Model:{0}", modelName);
// }
// break;
// case BuildOption.Spider:
// break;
// default:
// break;
// }
// //missing inoformation to build for this model
// return null;
//}
// 6 ------------- 5
// / /
// / | / |
// / | / | height/height
// 2 ------------- 1 |
// | |
// | |
// | 7 ------|------ 4
// | / | /
// | / | / depth/width
// / /
// 3 ------------- 0
// width/length
private static unsafe Ragdoll BuildBipped(studiohdr_t *pStudioHeader, BippedBone info, BoneAccessor accessor)
{
Debug.LogLine();
Debug.LogLine("=========================================");
Debug.LogLine("Bipped build begin...");
var listKeybone = new List <int>()
{
info.Head,
//info.Spine,
info.Pelvis,
info.LeftArm,
info.LeftElbow,
//info.LeftHand,
info.LeftHip,
info.LeftKnee,
//info.LeftFoot,
info.RightArm,
info.RightElbow,
//info.RightHand,
info.RightHip,
info.RightKnee,
//info.RightFoot,
};
var listNonKeybone = new List <int>();
for (int i = info.Pelvis; i < accessor.BoneCount; i++)
{
if (!listKeybone.Contains(i))
{
listNonKeybone.Add(i);
}
}
var ragdoll = new Ragdoll(pStudioHeader)
{
RigidBodies = new RigidBody[(int)BodyPart.Count],
Constraints = new TypedConstraint[9],
BoneRelativeTransform = new Matrix[accessor.BoneCount],
EntityId = 0,
RagdollData = new RagdollData()
{
KeyBoneIndeces = listKeybone,
NonKeyBoneIndeces = listNonKeybone,
}
};
//===========Rigidbody=================
Debug.LogLine("Now setup rigidbody...");
var lArm = accessor.Pos(info.LeftArm);
var rArm = accessor.Pos(info.RightArm);
var bodyWidth = (lArm - rArm).Length;
Debug.LogLine("body width:{0}", bodyWidth);
//head
var head = accessor.Pos(info.Head);
var headWidth = bodyWidth / 2.5f; //躯干是头的2.5倍
var headRaito = 5.85f / 8.1f; //头宽高比
var headHeight = headWidth / headRaito;
var headShape = new SphereShape(headHeight / 2);
var rigidOrigin = head;
rigidOrigin.Z += headHeight / 2;
rigidOrigin.X += headWidth / 4.6f;
var headLookAt = head;
headLookAt.X += headWidth / 4.6f;
var headRigid = Matrix.Translation(rigidOrigin);
var headBone = accessor.GetWorldTransform(info.Head);
ragdoll.RigidBodies[(int)BodyPart.Head] = BulletHelper.CreateBoneRigidbody(1, ref headBone, ref headRigid, headShape);
ragdoll.RigidBodies[(int)BodyPart.Head].UserIndex = info.Head;
//lArm
var lArmBone = accessor.GetWorldTransform(info.LeftArm);
var lElbow = accessor.Pos(info.LeftElbow);
ragdoll.RigidBodies[(int)BodyPart.LeftArm] = CreateLimb(ref lArmBone, lElbow, headWidth * 0.55f);
ragdoll.RigidBodies[(int)BodyPart.LeftArm].UserIndex = info.LeftArm;
//rArm
var rArmBone = accessor.GetWorldTransform(info.RightArm);
var rElbow = accessor.Pos(info.RightElbow);
ragdoll.RigidBodies[(int)BodyPart.RightArm] = CreateLimb(ref rArmBone, rElbow, headWidth * 0.55f);
ragdoll.RigidBodies[(int)BodyPart.RightArm].UserIndex = info.RightArm;
//lElbow
var lElbowBone = accessor.GetWorldTransform(info.LeftElbow);
var lHand = accessor.Pos(info.LeftHand);
ragdoll.RigidBodies[(int)BodyPart.LeftElbow] = CreateLimb(ref lElbowBone, lHand, headWidth * 0.5f);
ragdoll.RigidBodies[(int)BodyPart.LeftElbow].UserIndex = info.LeftElbow;
//rElbow
var rElbowBone = accessor.GetWorldTransform(info.RightElbow);
var rHand = accessor.Pos(info.RightHand);
ragdoll.RigidBodies[(int)BodyPart.RightElbow] = CreateLimb(ref rElbowBone, rHand, headWidth * 0.5f);
ragdoll.RigidBodies[(int)BodyPart.RightElbow].UserIndex = info.RightElbow;
//lHip
var lHipBone = accessor.GetWorldTransform(info.LeftHip);
var lKnee = accessor.Pos(info.LeftKnee);
ragdoll.RigidBodies[(int)BodyPart.LeftHip] = CreateLimb(ref lHipBone, lKnee, headWidth * 0.55f);
ragdoll.RigidBodies[(int)BodyPart.LeftHip].UserIndex = info.LeftHip;
//rHip
var rHipBone = accessor.GetWorldTransform(info.RightHip);
var rKnee = accessor.Pos(info.RightKnee);
ragdoll.RigidBodies[(int)BodyPart.RightHip] = CreateLimb(ref rHipBone, rKnee, headWidth * 0.55f);
ragdoll.RigidBodies[(int)BodyPart.RightHip].UserIndex = info.RightHip;
//lKnee
var lKneeBone = accessor.GetWorldTransform(info.LeftKnee);
var lFoot = accessor.Pos(info.LeftFoot);
ragdoll.RigidBodies[(int)BodyPart.LeftKnee] = CreateLimb(ref lKneeBone, lFoot, headWidth * 0.5f);
ragdoll.RigidBodies[(int)BodyPart.LeftKnee].UserIndex = info.LeftKnee;
//rKnee
var rKneeBone = accessor.GetWorldTransform(info.RightKnee);
var rFoot = accessor.Pos(info.RightFoot);
ragdoll.RigidBodies[(int)BodyPart.RightKnee] = CreateLimb(ref rKneeBone, rFoot, headWidth * 0.5f);
ragdoll.RigidBodies[(int)BodyPart.RightKnee].UserIndex = info.RightKnee;
//body
var pelvis = accessor.Pos(info.Pelvis);
var pelvisBone = accessor.GetWorldTransform(info.Pelvis);
var bodyHeight = (head - pelvis).Length;
var bodyShape = new CapsuleShape(bodyWidth / 2, bodyHeight - bodyWidth);
var bodyRigidTrans = BulletMathUtils.CenterOf(pelvis, head).LookAt(in head, in Vector3.UnitY);
ragdoll.RigidBodies[(int)BodyPart.Pelvis] = BulletHelper.CreateBoneRigidbody(3, ref pelvisBone, ref bodyRigidTrans, bodyShape);
ragdoll.RigidBodies[(int)BodyPart.Pelvis].UserIndex = info.Pelvis;
//==============Constraint=============
Debug.LogLine("Now setup constraint...");
var bodys = ragdoll.RigidBodies;
// head constraint
{
//ragdoll.Constraints[0] = CreateJoint(bodys[(int)BodyPart.Head], bodys[(int)BodyPart.Pelvis], head);
var bodyHead = bodys[(int)BodyPart.Head];
var bodyPelvis = bodys[(int)BodyPart.Pelvis];
var jointTrans = Matrix.Translation(head).LookAt(rigidOrigin, Vector3.UnitX);
var localHead = jointTrans * bodyHead.WorldTransform.GetInverse();
var localPelvis = jointTrans * bodyPelvis.WorldTransform.GetInverse();
ragdoll.Constraints[0] = new ConeTwistConstraint(bodyHead, bodyPelvis, localHead, localPelvis);
(ragdoll.Constraints[0] as ConeTwistConstraint).SetLimit((float)Math.PI / 6.5f, (float)Math.PI / 6.5f, (float)Math.PI * 0.3333333f);
}
// left arm constraint
{
//ragdoll.Constraints[1] = CreateJoint(bodys[(int)BodyPart.LeftArm], bodys[(int)BodyPart.Pelvis], lArm);
var bodyLeftArm = bodys[(int)BodyPart.LeftArm];
var bodyPelvis = bodys[(int)BodyPart.Pelvis];
var jointOrigin = accessor.Pos(info.LeftArm);
var jointTransform = Matrix.Translation(jointOrigin).LookAt(jointOrigin + new Vector3(0, -1, 0), Vector3.UnitX);
var localInLeftArm = jointTransform * bodyLeftArm.WorldTransform.GetInverse();
var localPelvis = jointTransform * bodyPelvis.WorldTransform.GetInverse();
var joint = new ConeTwistConstraint(bodyLeftArm, bodyPelvis, localInLeftArm, localPelvis);
joint.SetLimit(3.1415926f * 0.6f, 3.1415926f * 0.6f, 0);
ragdoll.Constraints[1] = joint;
}
// right arm constraint
{
//ragdoll.Constraints[2] = CreateJoint(bodys[(int)BodyPart.RightArm], bodys[(int)BodyPart.Pelvis], rArm);
var bodyRightArm = bodys[(int)BodyPart.RightArm];
var bodyPelvis = bodys[(int)BodyPart.Pelvis];
var jointOrigin = accessor.Pos(info.RightArm);
var jointTransform = Matrix.Translation(jointOrigin).LookAt(jointOrigin + new Vector3(0, 1, 0), Vector3.UnitX);
var localInRightArm = jointTransform * bodyRightArm.WorldTransform.GetInverse();
var localPelvis = jointTransform * bodyPelvis.WorldTransform.GetInverse();
var joint = new ConeTwistConstraint(bodyRightArm, bodyPelvis, localInRightArm, localPelvis);
joint.SetLimit(3.1415926f * 0.6f, 3.1415926f * 0.6f, 0);
ragdoll.Constraints[2] = joint;
}
{// Left elbow
//ragdoll.Constraints[3] = CreateJoint(bodys[(int)BodyPart.LeftElbow], bodys[(int)BodyPart.LeftArm], lElbow);
var bodyArm = bodys[(int)BodyPart.LeftArm];
var bodyElbow = bodys[(int)BodyPart.LeftElbow];
var jointTrans = Matrix.Translation(lElbow).LookAt(lElbow + new Vector3(0, 0, -1), Vector3.UnitZ);
// joint's local transform
var localInArm = jointTrans * bodyArm.WorldTransform.GetInverse();
var localInElbow = jointTrans * bodyElbow.WorldTransform.GetInverse();
var joint = new HingeConstraint(bodyArm, bodyElbow, localInArm, localInElbow);
joint.SetAxis(Vector3.UnitZ);
joint.SetLimit(-(float)Math.PI * 0.7f, 1);
ragdoll.Constraints[3] = joint;
}
{// right elbow
//ragdoll.Constraints[4] = CreateJoint(bodys[(int)BodyPart.RightElbow], bodys[(int)BodyPart.RightArm], rElbow);
var bodyArm = bodys[(int)BodyPart.RightArm];
var bodyElbow = bodys[(int)BodyPart.RightElbow];
var jointTrans = Matrix.Translation(rElbow).LookAt(rElbow + new Vector3(0, 0, -1), Vector3.UnitZ);
// joint's local transform
var localInArm = jointTrans * bodyArm.WorldTransform.GetInverse();
var localInElbow = jointTrans * bodyElbow.WorldTransform.GetInverse();
var joint = new HingeConstraint(bodyArm, bodyElbow, localInArm, localInElbow);
joint.SetAxis(Vector3.UnitZ);
joint.SetLimit(-(float)Math.PI * 0.7f, 1);
ragdoll.Constraints[4] = joint;
}
{
//ragdoll.Constraints[5] = CreateJoint(bodys[(int)BodyPart.LeftHip], bodys[(int)BodyPart.Pelvis], accessor.Pos(info.LeftHip));
var bodyHipL = bodys[(int)BodyPart.LeftHip];
var bodyPelvis = bodys[(int)BodyPart.Pelvis];
var leglen = (accessor.Pos(info.LeftHip) - lKnee).Length;
var x = (float)Math.Cos(Math.PI / 6) * leglen;
var z = (float)Math.Sin(Math.PI / 6) * leglen;
var posA = accessor.Pos(info.LeftHip);
posA.Z += z;
posA.X += x;
var posB = lKnee;
var posCenter = (posA + posB) / 2;
var jointTrans = Matrix.Translation(accessor.Pos(info.LeftHip)).LookAt(posCenter, Vector3.UnitX);
var localPelvis = jointTrans * bodyPelvis.WorldTransform.GetInverse();
var localHipL = jointTrans * bodyHipL.WorldTransform.GetInverse();
ragdoll.Constraints[5] = new ConeTwistConstraint(bodyHipL, bodyPelvis, localHipL, localPelvis);
(ragdoll.Constraints[5] as ConeTwistConstraint).SetLimit((float)Math.PI / 4, (float)Math.PI / 4, (float)Math.PI / 12);
}
{
//ragdoll.Constraints[6] = CreateJoint(bodys[(int)BodyPart.RightHip], bodys[(int)BodyPart.Pelvis], accessor.Pos(info.RightHip));
var bodyHipR = bodys[(int)BodyPart.RightHip];
var bodyPelvis = bodys[(int)BodyPart.Pelvis];
var leglen = (accessor.Pos(info.RightHip) - rKnee).Length;
var x = (float)Math.Cos(Math.PI / 6) * leglen;
var z = (float)Math.Sin(Math.PI / 6) * leglen;
var posA = accessor.Pos(info.RightHip);
posA.Z += z;
posA.X += x;
var posB = rKnee;
var posCenter = (posA + posB) / 2;
var jointTrans = Matrix.Translation(accessor.Pos(info.RightHip)).LookAt(posCenter, Vector3.UnitX);
var localPelvis = jointTrans * bodyPelvis.WorldTransform.GetInverse();
var localHipR = jointTrans * bodyHipR.WorldTransform.GetInverse();
ragdoll.Constraints[6] = new ConeTwistConstraint(bodyHipR, bodyPelvis, localHipR, localPelvis);
(ragdoll.Constraints[6] as ConeTwistConstraint).SetLimit((float)Math.PI / 4, (float)Math.PI / 4, (float)Math.PI / 12);
}
{ // left knee joint
{ // p2p joint
//ragdoll.Constraints[7] = CreateJoint(bodys[(int)BodyPart.LeftKnee], bodys[(int)BodyPart.LeftHip], lKnee);
}
{// hinge joint 1
var bodyHipL = bodys[(int)BodyPart.LeftHip];
var bodyKneeL = bodys[(int)BodyPart.LeftKnee];
var jointTrans = Matrix.Translation(lKnee).LookAt(lKnee + new Vector3(0, 1, 0), Vector3.UnitZ);
// joint's local transform in left hip
var localInHipL = jointTrans * bodyHipL.WorldTransform.GetInverse();
var localInKneeL = jointTrans * bodyKneeL.WorldTransform.GetInverse();
ragdoll.Constraints[7] = new HingeConstraint(bodyHipL, bodyKneeL, localInHipL, localInKneeL);
(ragdoll.Constraints[7] as HingeConstraint).SetLimit(-(float)Math.PI * 0.7f, 0);
}
{// hinge joint 2
//var bodyHipL = bodys[(int)BodyPart.LeftHip];
//var bodyKneeL = bodys[(int)BodyPart.LeftKnee];
//Vector3 axisInA = new Vector3(1, 0, 0);
//Matrix transform = Matrix.Invert(bodyB.WorldTransform) * bodyA.WorldTransform;
//Vector3 pivotInB = Vector3.TransformCoordinate(pivotInA, transform);
//transform = Matrix.Invert(bodyB.WorldTransform) * bodyB.WorldTransform;
//Vector3 axisInB = Vector3.TransformCoordinate(axisInA, transform);
//var jointTrans = Matrix.Translation(lKnee).LookAt(lKnee + new Vector3(0, 1, 0), Vector3.UnitZ);
//// joint's local transform in left hip
//var localInHipL = jointTrans * bodyHipL.WorldTransform.GetInverse();
//var localInKneeL = jointTrans * bodyKneeL.WorldTransform.GetInverse();
//ragdoll.Constraints[7] = new HingeConstraint(bodyHipL, bodyKneeL, localInHipL, localInKneeL);
//(ragdoll.Constraints[7] as HingeConstraint).SetLimit(-(float)Math.PI / 6, (float)Math.PI / 6);
}
}
{
//ragdoll.Constraints[8] = CreateJoint(bodys[(int)BodyPart.RightKnee], bodys[(int)BodyPart.RightHip], rKnee);
{// hinge joint 1
var bodyHipR = bodys[(int)BodyPart.RightHip];
var bodyKneeR = bodys[(int)BodyPart.RightKnee];
var jointTrans = Matrix.Translation(rKnee).LookAt(rKnee + new Vector3(0, 1, 0), Vector3.UnitZ);
// joint's local transform in left hip
var localInHipR = jointTrans * bodyHipR.WorldTransform.GetInverse();
var localInKneeR = jointTrans * bodyKneeR.WorldTransform.GetInverse();
ragdoll.Constraints[8] = new HingeConstraint(bodyHipR, bodyKneeR, localInHipR, localInKneeR);
(ragdoll.Constraints[8] as HingeConstraint).SetLimit(-(float)Math.PI * 0.7f, 0);
}
}
foreach (var i in ragdoll.RigidBodies)
{
i.SetDamping(0.05f, 0.85f);
i.DeactivationTime = 0.8f;
i.SetSleepingThresholds(1.6f, 2.5f);
i.Friction = 2.2f;//v1.0 is 2
}
// damping, friction and restitution document forum
//https://pybullet.org/Bullet/phpBB3/viewtopic.php?t=8100
// What is the unit of Damping in Generic Spring Constraint?
// https://github.com/bulletphysics/bullet3/issues/345
foreach (var i in ragdoll.Constraints)
{
i.DebugDrawSize = 3;
}
Debug.LogLine("Bipped build complete.");
Debug.LogLine("=========================================");
return(ragdoll);
}
public Physics()
{
SetupEmptyDynamicsWorld();
CollisionShape groundShape = new BoxShape(50, 1, 50);
//CollisionShape groundShape = new StaticPlaneShape(Vector3.UnitY, 40);
CollisionShapes.Add(groundShape);
RigidBody body = LocalCreateRigidBody(0, Matrix.Translation(0, -16, 0), groundShape);
body.UserObject = "Ground";
CollisionShape shape = new BoxShape(new Vector3(CubeHalfExtents));
CollisionShapes.Add(shape);
float mass = 1.0f;
RigidBody body0 = LocalCreateRigidBody(mass, Matrix.Translation(0, 20, 0), shape);
RigidBody body1 = null;//LocalCreateRigidBody(mass, Matrix.Translation(2*CUBE_HALF_EXTENTS,20,0), shape);
//RigidBody body1 = LocalCreateRigidBody(0, Matrix.Translation(2*CUBE_HALF_EXTENTS,20,0), null);
//body1.ActivationState = ActivationState.DisableDeactivation;
//body1.SetDamping(0.3f, 0.3f);
Vector3 pivotInA = new Vector3(CubeHalfExtents, -CubeHalfExtents, -CubeHalfExtents);
Vector3 axisInA = new Vector3(0, 0, 1);
Vector3 pivotInB;
if (body1 != null)
{
Matrix transform = Matrix.Invert(body1.CenterOfMassTransform) * body0.CenterOfMassTransform;
pivotInB = Vector3.TransformCoordinate(pivotInA, transform);
}
else
{
pivotInB = pivotInA;
}
Vector3 axisInB;
if (body1 != null)
{
Matrix transform = Matrix.Invert(body1.CenterOfMassTransform) * body1.CenterOfMassTransform;
axisInB = Vector3.TransformCoordinate(axisInA, transform);
}
else
{
axisInB = Vector3.TransformCoordinate(axisInA, body0.CenterOfMassTransform);
}
if (P2P)
{
TypedConstraint p2p = new Point2PointConstraint(body0, pivotInA);
//TypedConstraint p2p = new Point2PointConstraint(body0,body1,pivotInA,pivotInB);
//TypedConstraint hinge = new HingeConstraint(body0,body1,pivotInA,pivotInB,axisInA,axisInB);
World.AddConstraint(p2p);
p2p.DebugDrawSize = 5;
}
else
{
HingeConstraint hinge = new HingeConstraint(body0, pivotInA, axisInA);
//use zero targetVelocity and a small maxMotorImpulse to simulate joint friction
//float targetVelocity = 0.f;
//float maxMotorImpulse = 0.01;
float targetVelocity = 1.0f;
float maxMotorImpulse = 1.0f;
hinge.EnableAngularMotor(true, targetVelocity, maxMotorImpulse);
World.AddConstraint(hinge);
hinge.DebugDrawSize = 5;
}
//create a slider, using the generic D6 constraint
Vector3 sliderWorldPos = new Vector3(0, 10, 0);
Vector3 sliderAxis = Vector3.UnitX;
float angle = 0;//SIMD_RADS_PER_DEG * 10.f;
Matrix trans = Matrix.RotationAxis(sliderAxis, angle) * Matrix.Translation(sliderWorldPos);
d6body0 = LocalCreateRigidBody(mass, trans, shape);
d6body0.ActivationState = ActivationState.DisableDeactivation;
RigidBody fixedBody1 = LocalCreateRigidBody(0, trans, null);
World.AddRigidBody(fixedBody1);
Matrix frameInA = Matrix.Translation(0, 5, 0);
Matrix frameInB = Matrix.Translation(0, 5, 0);
//bool useLinearReferenceFrameA = false;//use fixed frame B for linear llimits
bool useLinearReferenceFrameA = true;//use fixed frame A for linear llimits
spSlider6Dof = new Generic6DofConstraint(fixedBody1, d6body0, frameInA, frameInB, useLinearReferenceFrameA);
spSlider6Dof.LinearLowerLimit = lowerSliderLimit;
spSlider6Dof.LinearUpperLimit = hiSliderLimit;
//range should be small, otherwise singularities will 'explode' the constraint
//spSlider6Dof.AngularLowerLimit = new Vector3(-1.5f,0,0);
//spSlider6Dof.AngularUpperLimit = new Vector3(1.5f,0,0);
//spSlider6Dof.AngularLowerLimit = new Vector3(0,0,0);
//spSlider6Dof.AngularUpperLimit = new Vector3(0,0,0);
spSlider6Dof.AngularLowerLimit = new Vector3((float)-Math.PI, 0, 0);
spSlider6Dof.AngularUpperLimit = new Vector3(1.5f, 0, 0);
spSlider6Dof.TranslationalLimitMotor.EnableMotor[0] = true;
spSlider6Dof.TranslationalLimitMotor.TargetVelocity = new Vector3(-5.0f, 0, 0);
spSlider6Dof.TranslationalLimitMotor.MaxMotorForce = new Vector3(0.1f, 0, 0);
World.AddConstraint(spSlider6Dof);
spSlider6Dof.DebugDrawSize = 5;
// create a door using hinge constraint attached to the world
CollisionShape pDoorShape = new BoxShape(2.0f, 5.0f, 0.2f);
CollisionShapes.Add(pDoorShape);
RigidBody pDoorBody = LocalCreateRigidBody(1.0f, Matrix.Translation(-5.0f, -2.0f, 0.0f), pDoorShape);
pDoorBody.ActivationState = ActivationState.DisableDeactivation;
Vector3 btPivotA = new Vector3(10.0f + 2.1f, -2.0f, 0.0f); // right next to the door slightly outside
Vector3 btAxisA = Vector3.UnitY; // pointing upwards, aka Y-axis
spDoorHinge = new HingeConstraint(pDoorBody, btPivotA, btAxisA);
//spDoorHinge.SetLimit(0.0f, (float)Math.PI / 2);
// test problem values
//spDoorHinge.SetLimit(-(float)Math.PI, (float)Math.PI * 0.8f);
//spDoorHinge.SetLimit(1, -1);
//spDoorHinge.SetLimit(-(float)Math.PI * 0.8f, (float)Math.PI);
//spDoorHinge.SetLimit(-(float)Math.PI * 0.8f, (float)Math.PI, 0.9f, 0.3f, 0.0f);
//spDoorHinge.SetLimit(-(float)Math.PI * 0.8f, (float)Math.PI, 0.9f, 0.01f, 0.0f); // "sticky limits"
spDoorHinge.SetLimit(-(float)Math.PI * 0.25f, (float)Math.PI * 0.25f);
//spDoorHinge.SetLimit(0, 0);
World.AddConstraint(spDoorHinge);
spDoorHinge.DebugDrawSize = 5;
RigidBody pDropBody = LocalCreateRigidBody(10.0f, Matrix.Translation(-5.0f, 2.0f, 0.0f), shape);
// create a generic 6DOF constraint
//RigidBody pBodyA = LocalCreateRigidBody(mass, Matrix.Translation(10.0f, 6.0f, 0), shape);
RigidBody pBodyA = LocalCreateRigidBody(0, Matrix.Translation(10, 6, 0), shape);
//RigidBody pBodyA = LocalCreateRigidBody(0, Matrix.Translation(10, 6, 0), null);
pBodyA.ActivationState = ActivationState.DisableDeactivation;
RigidBody pBodyB = LocalCreateRigidBody(mass, Matrix.Translation(0, 6, 0), shape);
//RigidBody pBodyB = LocalCreateRigidBody(0, Matrix.Translation(0, 6, 0), shape);
pBodyB.ActivationState = ActivationState.DisableDeactivation;
frameInA = Matrix.Translation(-5, 0, 0);
frameInB = Matrix.Translation(5, 0, 0);
Generic6DofConstraint pGen6DOF = new Generic6DofConstraint(pBodyA, pBodyB, frameInA, frameInB, true);
//Generic6DofConstraint pGen6DOF = new Generic6DofConstraint(pBodyA, pBodyB, frameInA, frameInB, false);
pGen6DOF.LinearLowerLimit = new Vector3(-10, -2, -1);
pGen6DOF.LinearUpperLimit = new Vector3(10, 2, 1);
//pGen6DOF.LinearLowerLimit = new Vector3(-10, 0, 0);
//pGen6DOF.LinearUpperLimit = new Vector3(10, 0, 0);
//pGen6DOF.LinearLowerLimit = new Vector3(0, 0, 0);
//pGen6DOF.LinearUpperLimit = new Vector3(0, 0, 0);
//pGen6DOF.TranslationalLimitMotor.EnableMotor[0] = true;
//pGen6DOF.TranslationalLimitMotor.TargetVelocity = new Vector3(5, 0, 0);
//pGen6DOF.TranslationalLimitMotor.MaxMotorForce = new Vector3(0.1f, 0, 0);
//pGen6DOF.AngularLowerLimit = new Vector3(0, (float)Math.PI * 0.9f, 0);
//pGen6DOF.AngularUpperLimit = new Vector3(0, -(float)Math.PI * 0.9f, 0);
//pGen6DOF.AngularLowerLimit = new Vector3(0, 0, -(float)Math.PI);
//pGen6DOF.AngularUpperLimit = new Vector3(0, 0, (float)Math.PI);
pGen6DOF.AngularLowerLimit = new Vector3(-(float)Math.PI / 4, -0.75f, -(float)Math.PI * 0.4f);
pGen6DOF.AngularUpperLimit = new Vector3((float)Math.PI / 4, 0.75f, (float)Math.PI * 0.4f);
//pGen6DOF.AngularLowerLimit = new Vector3(0, -0.75f, (float)Math.PI * 0.8f);
//pGen6DOF.AngularUpperLimit = new Vector3(0, 0.75f, -(float)Math.PI * 0.8f);
//pGen6DOF.AngularLowerLimit = new Vector3(0, -(float)Math.PI * 0.8f, (float)Math.PI * 1.98f);
//pGen6DOF.AngularUpperLimit = new Vector3(0, (float)Math.PI * 0.8f, -(float)Math.PI * 1.98f);
//pGen6DOF.AngularLowerLimit = new Vector3(-0.75f, -0.5f, -0.5f);
//pGen6DOF.AngularUpperLimit = new Vector3(0.75f, 0.5f, 0.5f);
//pGen6DOF.AngularLowerLimit = new Vector3(-0.75f, 0, 0);
//pGen6DOF.AngularUpperLimit = new Vector3(0.75f, 0, 0);
//pGen6DOF.AngularLowerLimit = new Vector3(0, -0.7f, 0);
//pGen6DOF.AngularUpperLimit = new Vector3(0, 0.7f, 0);
//pGen6DOF.AngularLowerLimit = new Vector3(-1, 0, 0);
//pGen6DOF.AngularUpperLimit = new Vector3(1, 0, 0);
// create a ConeTwist constraint
pBodyA = LocalCreateRigidBody(1.0f, Matrix.Translation(-10, 5, 0), shape);
//pBodyA = LocalCreateRigidBody(0, Matrix.Translation(-10, 5, 0), shape);
pBodyA.ActivationState = ActivationState.DisableDeactivation;
pBodyB = LocalCreateRigidBody(0, Matrix.Translation(-10, -5, 0), shape);
//pBodyB = LocalCreateRigidBody(1.0f, Matrix.Translation(-10, -5, 0), shape);
frameInA = Matrix.RotationYawPitchRoll(0, 0, (float)Math.PI / 2);
frameInA *= Matrix.Translation(0, -5, 0);
frameInB = Matrix.RotationYawPitchRoll(0, 0, (float)Math.PI / 2);
frameInB *= Matrix.Translation(0, 5, 0);
coneTwist = new ConeTwistConstraint(pBodyA, pBodyB, frameInA, frameInB);
//coneTwist.SetLimit((float)Math.PI / 4, (float)Math.PI / 4, (float)Math.PI * 0.8f);
//coneTwist.SetLimit((((float)Math.PI / 4) * 0.6f), (float)Math.PI / 4, (float)Math.PI * 0.8f, 1.0f); // soft limit == hard limit
coneTwist.SetLimit((((float)Math.PI / 4) * 0.6f), (float)Math.PI / 4, (float)Math.PI * 0.8f, 0.5f);
World.AddConstraint(coneTwist, true);
coneTwist.DebugDrawSize = 5;
// Hinge connected to the world, with motor (to hinge motor with new and old constraint solver)
RigidBody pBody = LocalCreateRigidBody(1.0f, Matrix.Identity, shape);
pBody.ActivationState = ActivationState.DisableDeactivation;
Vector3 PivotA = new Vector3(10.0f, 0.0f, 0.0f);
btAxisA = new Vector3(0.0f, 0.0f, 1.0f);
HingeConstraint pHinge = new HingeConstraint(pBody, btPivotA, btAxisA);
//pHinge.EnableAngularMotor(true, -1.0f, 0.165f); // use for the old solver
pHinge.EnableAngularMotor(true, -1.0f, 1.65f); // use for the new SIMD solver
World.AddConstraint(pHinge);
pHinge.DebugDrawSize = 5;
// create a universal joint using generic 6DOF constraint
// create two rigid bodies
// static bodyA (parent) on top:
pBodyA = LocalCreateRigidBody(0, Matrix.Translation(20, 4, 0), shape);
pBodyA.ActivationState = ActivationState.DisableDeactivation;
// dynamic bodyB (child) below it :
pBodyB = LocalCreateRigidBody(1.0f, Matrix.Translation(20, 0, 0), shape);
pBodyB.ActivationState = ActivationState.DisableDeactivation;
// add some (arbitrary) data to build constraint frames
Vector3 parentAxis = new Vector3(1, 0, 0);
Vector3 childAxis = new Vector3(0, 0, 1);
Vector3 anchor = new Vector3(20, 2, 0);
UniversalConstraint pUniv = new UniversalConstraint(pBodyA, pBodyB, anchor, parentAxis, childAxis);
pUniv.SetLowerLimit(-(float)Math.PI / 4, -(float)Math.PI / 4);
pUniv.SetUpperLimit((float)Math.PI / 4, (float)Math.PI / 4);
// add constraint to world
World.AddConstraint(pUniv, true);
// draw constraint frames and limits for debugging
pUniv.DebugDrawSize = 5;
World.AddConstraint(pGen6DOF, true);
pGen6DOF.DebugDrawSize = 5;
// create a generic 6DOF constraint with springs
pBodyA = LocalCreateRigidBody(0, Matrix.Translation(-20, 16, 0), shape);
pBodyA.ActivationState = ActivationState.DisableDeactivation;
pBodyB = LocalCreateRigidBody(1.0f, Matrix.Translation(-10, 16, 0), shape);
pBodyB.ActivationState = ActivationState.DisableDeactivation;
frameInA = Matrix.Translation(10, 0, 0);
frameInB = Matrix.Identity;
Generic6DofSpringConstraint pGen6DOFSpring = new Generic6DofSpringConstraint(pBodyA, pBodyB, frameInA, frameInB, true);
pGen6DOFSpring.LinearUpperLimit = new Vector3(5, 0, 0);
pGen6DOFSpring.LinearLowerLimit = new Vector3(-5, 0, 0);
pGen6DOFSpring.AngularLowerLimit = new Vector3(0, 0, -1.5f);
pGen6DOFSpring.AngularUpperLimit = new Vector3(0, 0, 1.5f);
World.AddConstraint(pGen6DOFSpring, true);
pGen6DOFSpring.DebugDrawSize = 5;
pGen6DOFSpring.EnableSpring(0, true);
pGen6DOFSpring.SetStiffness(0, 39.478f);
pGen6DOFSpring.SetDamping(0, 0.5f);
pGen6DOFSpring.EnableSpring(5, true);
pGen6DOFSpring.SetStiffness(5, 39.478f);
pGen6DOFSpring.SetDamping(0, 0.3f);
pGen6DOFSpring.SetEquilibriumPoint();
// create a Hinge2 joint
// create two rigid bodies
// static bodyA (parent) on top:
pBodyA = LocalCreateRigidBody(0, Matrix.Translation(-20, 4, 0), shape);
pBodyA.ActivationState = ActivationState.DisableDeactivation;
// dynamic bodyB (child) below it :
pBodyB = LocalCreateRigidBody(1.0f, Matrix.Translation(-20, 0, 0), shape);
pBodyB.ActivationState = ActivationState.DisableDeactivation;
// add some data to build constraint frames
parentAxis = new Vector3(0, 1, 0);
childAxis = new Vector3(1, 0, 0);
anchor = new Vector3(-20, 0, 0);
Hinge2Constraint pHinge2 = new Hinge2Constraint(pBodyA, pBodyB, anchor, parentAxis, childAxis);
pHinge2.SetLowerLimit(-(float)Math.PI / 4);
pHinge2.SetUpperLimit((float)Math.PI / 4);
// add constraint to world
World.AddConstraint(pHinge2, true);
// draw constraint frames and limits for debugging
pHinge2.DebugDrawSize = 5;
// create a Hinge joint between two dynamic bodies
// create two rigid bodies
// static bodyA (parent) on top:
pBodyA = LocalCreateRigidBody(1.0f, Matrix.Translation(-20, -2, 0), shape);
pBodyA.ActivationState = ActivationState.DisableDeactivation;
// dynamic bodyB:
pBodyB = LocalCreateRigidBody(10.0f, Matrix.Translation(-30, -2, 0), shape);
pBodyB.ActivationState = ActivationState.DisableDeactivation;
// add some data to build constraint frames
Vector3 axisA = new Vector3(0, 1, 0);
Vector3 axisB = new Vector3(0, 1, 0);
Vector3 pivotA = new Vector3(-5, 0, 0);
Vector3 pivotB = new Vector3(5, 0, 0);
spHingeDynAB = new HingeConstraint(pBodyA, pBodyB, pivotA, pivotB, axisA, axisB);
spHingeDynAB.SetLimit(-(float)Math.PI / 4, (float)Math.PI / 4);
// add constraint to world
World.AddConstraint(spHingeDynAB, true);
// draw constraint frames and limits for debugging
spHingeDynAB.DebugDrawSize = 5;
}
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 override void InitializeDemo()
{
m_collisionConfiguration = new DefaultCollisionConfiguration();
m_dispatcher = new CollisionDispatcher(m_collisionConfiguration);
IndexedVector3 worldMin = new IndexedVector3(-1000, -1000, -1000);
IndexedVector3 worldMax = new IndexedVector3(1000, 1000, 1000);
m_broadphase = new AxisSweep3Internal(ref worldMin, ref worldMax, 0xfffe, 0xffff, 16384, null, false);
m_constraintSolver = new SequentialImpulseConstraintSolver();
m_dynamicsWorld = new DiscreteDynamicsWorld(m_dispatcher, m_broadphase, m_constraintSolver, m_collisionConfiguration);
m_dynamicsWorld.SetDebugDrawer(m_debugDraw);
SetCameraDistance(26f);
//CollisionShape groundShape = new BoxShape(new IndexedVector3(50f, 40f, 50f));
CollisionShape groundShape = new StaticPlaneShape(new IndexedVector3(0, 1, 0), 40);
m_collisionShapes.Add(groundShape);
IndexedMatrix groundTransform = IndexedMatrix.Identity;
groundTransform._origin = new IndexedVector3(0, -56, 0);
RigidBody groundBody = LocalCreateRigidBody(0, ref groundTransform, groundShape);
CollisionShape shape = new BoxShape(new IndexedVector3(CUBE_HALF_EXTENTS, CUBE_HALF_EXTENTS, CUBE_HALF_EXTENTS));
m_collisionShapes.Add(shape);
IndexedMatrix trans = IndexedMatrix.Identity;
trans._origin = new IndexedVector3(0, 20, 0);
float mass = 1f;
#if true
//point to point constraint (ball socket)
//SEEMS OK
{
RigidBody body0 = LocalCreateRigidBody(mass, ref trans, shape);
trans._origin = new IndexedVector3(2 * CUBE_HALF_EXTENTS, 20, 0);
mass = 1f;
RigidBody body1 = null; //localCreateRigidBody( mass,trans,shape);
IndexedVector3 pivotInA = new IndexedVector3(CUBE_HALF_EXTENTS, -CUBE_HALF_EXTENTS, -CUBE_HALF_EXTENTS);
IndexedVector3 axisInA = new IndexedVector3(0, 0, 1);
IndexedVector3 pivotInB = body1 != null?body1.GetCenterOfMassTransform().Inverse() * (body0.GetCenterOfMassTransform() * (pivotInA)) : pivotInA;
IndexedVector3 axisInB = body1 != null ? (body1.GetCenterOfMassTransform()._basis.Inverse() * (body1.GetCenterOfMassTransform()._basis *axisInA)) :
body0.GetCenterOfMassTransform()._basis *axisInA;
HingeConstraint hinge = new HingeConstraint(body0, ref pivotInA, ref axisInA, false);
float targetVelocity = 1f;
float maxMotorImpulse = 1.0f;
hinge.EnableAngularMotor(true, targetVelocity, maxMotorImpulse);
m_dynamicsWorld.AddConstraint(hinge); //p2p);
hinge.SetDbgDrawSize(5f);
}
#endif
#if true
//create a slider, using the generic D6 constraint
// SEEMS OK
{
mass = 1f;
IndexedVector3 sliderWorldPos = new IndexedVector3(0, 10, 0);
IndexedVector3 sliderAxis = new IndexedVector3(1, 0, 0);
float angle = 0f;//SIMD_RADS_PER_DEG * 10.f;
IndexedBasisMatrix sliderOrientation = new IndexedBasisMatrix(Quaternion.CreateFromAxisAngle(sliderAxis.ToVector3(), angle));
trans = IndexedMatrix.Identity;
trans._origin = sliderWorldPos;
//trans.setBasis(sliderOrientation);
sliderTransform = trans;
d6body0 = LocalCreateRigidBody(mass, ref trans, shape);
d6body0.SetActivationState(ActivationState.DISABLE_DEACTIVATION);
RigidBody fixedBody1 = LocalCreateRigidBody(0, ref trans, null);
m_dynamicsWorld.AddRigidBody(fixedBody1);
IndexedMatrix frameInA, frameInB;
frameInA = IndexedMatrix.Identity;
frameInB = IndexedMatrix.Identity;
frameInA._origin = new IndexedVector3(0, 5, 0);
frameInB._origin = new IndexedVector3(0, 5, 0);
// bool useLinearReferenceFrameA = false;//use fixed frame B for linear llimits
bool useLinearReferenceFrameA = true; //use fixed frame A for linear llimits
spSlider6Dof = new Generic6DofConstraint(fixedBody1, d6body0, ref frameInA, ref frameInB, useLinearReferenceFrameA);
spSlider6Dof.SetLinearLowerLimit(ref lowerSliderLimit);
spSlider6Dof.SetLinearUpperLimit(ref hiSliderLimit);
//range should be small, otherwise singularities will 'explode' the constraint
IndexedVector3 angularLower = new IndexedVector3(-1.5f, 0, 0);
IndexedVector3 angularUpper = -angularLower;
spSlider6Dof.SetAngularLowerLimit(ref angularLower);
spSlider6Dof.SetAngularUpperLimit(ref angularUpper);
// slider.setAngularLowerLimit(IndexedVector3(0,0,0));
// slider.setAngularUpperLimit(IndexedVector3(0,0,0));
spSlider6Dof.GetTranslationalLimitMotor().m_enableMotor[0] = true;
spSlider6Dof.GetTranslationalLimitMotor().m_targetVelocity.X = -5.0f;
spSlider6Dof.GetTranslationalLimitMotor().m_maxMotorForce.X = 0.1f;
m_dynamicsWorld.AddConstraint(spSlider6Dof);
spSlider6Dof.SetDbgDrawSize(5f);
}
#endif
#if true
{ // create a door using hinge constraint attached to the world
CollisionShape pDoorShape = new BoxShape(new IndexedVector3(2.0f, 5.0f, 0.2f));
m_collisionShapes.Add(pDoorShape);
IndexedMatrix doorTrans = IndexedMatrix.Identity;
doorTrans._origin = new IndexedVector3(-5.0f, -2.0f, 0.0f);
RigidBody pDoorBody = LocalCreateRigidBody(1.0f, ref doorTrans, pDoorShape);
pDoorBody.SetActivationState(ActivationState.DISABLE_DEACTIVATION);
IndexedVector3 btPivotA = new IndexedVector3(10f + 2.1f, -2.0f, 0.0f); // right next to the door slightly outside
IndexedVector3 btAxisA = new IndexedVector3(0.0f, 1.0f, 0.0f); // pointing upwards, aka Y-axis
spDoorHinge = new HingeConstraint(pDoorBody, ref btPivotA, ref btAxisA, false);
spDoorHinge.SetLimit(-MathUtil.SIMD_PI * 0.25f, MathUtil.SIMD_PI * 0.25f);
m_dynamicsWorld.AddConstraint(spDoorHinge);
spDoorHinge.SetDbgDrawSize(5.0f);
}
#endif
#if true
{ // create a generic 6DOF constraint
// SEEMS OK - But debug draw a bit wrong?
IndexedMatrix tr = IndexedMatrix.Identity;
tr._origin = new IndexedVector3(10f, 6f, 0f);
//tr.getBasis().setEulerZYX(0,0,0);
// RigidBody pBodyA = localCreateRigidBody( mass, tr, shape);
RigidBody pBodyA = LocalCreateRigidBody(0.0f, ref tr, shape);
// RigidBody pBodyA = localCreateRigidBody( 0.0, tr, 0);
pBodyA.SetActivationState(ActivationState.DISABLE_DEACTIVATION);
tr = IndexedMatrix.Identity;
tr._origin = new IndexedVector3(0f, 6f, 0f);
//tr.getBasis().setEulerZYX(0,0,0);
RigidBody pBodyB = LocalCreateRigidBody(mass, ref tr, shape);
pBodyB.SetActivationState(ActivationState.DISABLE_DEACTIVATION);
IndexedMatrix frameInA, frameInB;
frameInA = IndexedMatrix.CreateTranslation(-5, 0, 0);
frameInB = IndexedMatrix.CreateTranslation(5, 0, 0);
Generic6DofConstraint pGen6DOF = new Generic6DofConstraint(pBodyA, pBodyB, ref frameInA, ref frameInB, true);
// btGeneric6DofConstraint* pGen6DOF = new btGeneric6DofConstraint(*pBodyA, *pBodyB, frameInA, frameInB, false);
IndexedVector3 linearLower = new IndexedVector3(-10, -2, -1);
pGen6DOF.SetLinearLowerLimit(ref linearLower);
IndexedVector3 linearUpper = new IndexedVector3(10, 2, 1);
pGen6DOF.SetLinearUpperLimit(ref linearUpper);
// ? why again?
//linearLower = new IndexedVector3(-10,0,0);
//pGen6DOF.setLinearLowerLimit(ref linearLower);
// pGen6DOF.setLinearUpperLimit(IndexedVector3(10., 0., 0.));
// pGen6DOF.setLinearLowerLimit(IndexedVector3(0., 0., 0.));
// pGen6DOF.setLinearUpperLimit(IndexedVector3(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(IndexedVector3(0., SIMD_HALF_PI*0.9, 0.));
// pGen6DOF.setAngularUpperLimit(IndexedVector3(0., -SIMD_HALF_PI*0.9, 0.));
// pGen6DOF.setAngularLowerLimit(IndexedVector3(0., 0., -SIMD_HALF_PI));
// pGen6DOF.setAngularUpperLimit(IndexedVector3(0., 0., SIMD_HALF_PI));
IndexedVector3 angularLower = new IndexedVector3(-MathUtil.SIMD_HALF_PI * 0.5f, -0.75f, -MathUtil.SIMD_HALF_PI * 0.8f);
IndexedVector3 angularUpper = -angularLower;
pGen6DOF.SetAngularLowerLimit(ref angularLower);
pGen6DOF.SetAngularUpperLimit(ref angularUpper);
// pGen6DOF.setAngularLowerLimit(IndexedVector3(0.f, -0.75, SIMD_HALF_PI * 0.8f));
// pGen6DOF.setAngularUpperLimit(IndexedVector3(0.f, 0.75, -SIMD_HALF_PI * 0.8f));
// pGen6DOF.setAngularLowerLimit(IndexedVector3(0.f, -SIMD_HALF_PI * 0.8f, SIMD_HALF_PI * 1.98f));
// pGen6DOF.setAngularUpperLimit(IndexedVector3(0.f, SIMD_HALF_PI * 0.8f, -SIMD_HALF_PI * 1.98f));
// pGen6DOF.setAngularLowerLimit(IndexedVector3(-0.75,-0.5, -0.5));
// pGen6DOF.setAngularUpperLimit(IndexedVector3(0.75,0.5, 0.5));
// pGen6DOF.setAngularLowerLimit(IndexedVector3(-0.75,0., 0.));
// pGen6DOF.setAngularUpperLimit(IndexedVector3(0.75,0., 0.));
m_dynamicsWorld.AddConstraint(pGen6DOF, true);
pGen6DOF.SetDbgDrawSize(5.0f);
}
#endif
#if true
{ // create a ConeTwist constraint
IndexedMatrix tr = IndexedMatrix.CreateTranslation(-10, 5, 0);
RigidBody pBodyA = LocalCreateRigidBody(1.0f, ref tr, shape);
pBodyA.SetActivationState(ActivationState.DISABLE_DEACTIVATION);
tr = IndexedMatrix.CreateTranslation(-10, -5, 0);
RigidBody pBodyB = LocalCreateRigidBody(0.0f, ref tr, shape);
IndexedMatrix frameInA, frameInB;
frameInA = MathUtil.SetEulerZYX(0, 0, MathUtil.SIMD_HALF_PI);
frameInA._origin = new IndexedVector3(0, -5, 0);
frameInB = MathUtil.SetEulerZYX(0, 0, MathUtil.SIMD_HALF_PI);
frameInB._origin = new IndexedVector3(0, 5, 0);
ConeTwistConstraint pCT = new ConeTwistConstraint(pBodyA, pBodyB, ref frameInA, ref frameInB);
pCT.SetLimit(MathUtil.SIMD_QUARTER_PI, MathUtil.SIMD_QUARTER_PI, MathUtil.SIMD_PI * 0.8f, 1.0f, 0.3f, 1.0f); // soft limit == hard limit
m_dynamicsWorld.AddConstraint(pCT, true);
pCT.SetDbgDrawSize(5.0f);
}
#endif
#if true
{ // Hinge connected to the world, with motor (to hinge motor with new and old constraint solver)
// WORKS OK
IndexedMatrix tr = IndexedMatrix.Identity;
RigidBody pBody = LocalCreateRigidBody(1.0f, ref tr, shape);
pBody.SetActivationState(ActivationState.DISABLE_DEACTIVATION);
IndexedVector3 btPivotA = new IndexedVector3(10.0f, 0.0f, 0.0f);
IndexedVector3 btAxisA = new IndexedVector3(0.0f, 0.0f, 1.0f);
HingeConstraint pHinge = new HingeConstraint(pBody, ref btPivotA, ref btAxisA, false);
// pHinge.enableAngularMotor(true, -1.0, 0.165); // use for the old solver
pHinge.EnableAngularMotor(true, -1.0f, 1.65f); // use for the new SIMD solver
m_dynamicsWorld.AddConstraint(pHinge);
pHinge.SetDbgDrawSize(5.0f);
}
#endif
#if true
{
// WORKS OK
// create a universal joint using generic 6DOF constraint
// create two rigid bodies
// static bodyA (parent) on top:
IndexedMatrix tr = IndexedMatrix.CreateTranslation(20, 4, 0);
RigidBody pBodyA = LocalCreateRigidBody(0.0f, ref tr, shape);
pBodyA.SetActivationState(ActivationState.DISABLE_DEACTIVATION);
// dynamic bodyB (child) below it :
tr = IndexedMatrix.CreateTranslation(20, 0, 0);
RigidBody pBodyB = LocalCreateRigidBody(1.0f, ref tr, shape);
pBodyB.SetActivationState(ActivationState.DISABLE_DEACTIVATION);
// add some (arbitrary) data to build constraint frames
IndexedVector3 parentAxis = new IndexedVector3(1.0f, 0.0f, 0.0f);
IndexedVector3 childAxis = new IndexedVector3(0.0f, 0.0f, 1.0f);
IndexedVector3 anchor = new IndexedVector3(20.0f, 2.0f, 0.0f);
UniversalConstraint pUniv = new UniversalConstraint(pBodyA, pBodyB, ref anchor, ref parentAxis, ref childAxis);
pUniv.SetLowerLimit(-MathUtil.SIMD_HALF_PI * 0.5f, -MathUtil.SIMD_HALF_PI * 0.5f);
pUniv.SetUpperLimit(MathUtil.SIMD_HALF_PI * 0.5f, MathUtil.SIMD_HALF_PI * 0.5f);
// add constraint to world
m_dynamicsWorld.AddConstraint(pUniv, true);
// draw constraint frames and limits for debugging
pUniv.SetDbgDrawSize(5.0f);
}
#endif
#if true
// WORKS OK
{ // create a generic 6DOF constraint with springs
IndexedMatrix tr = IndexedMatrix.CreateTranslation(-20f, 16f, 0f);
//tr.setIdentity();
//tr.setOrigin(btVector3(btScalar(-20.), btScalar(16.), btScalar(0.)));
//tr.getBasis().setEulerZYX(0,0,0);
RigidBody pBodyA = LocalCreateRigidBody(0.0f, ref tr, shape);
pBodyA.SetActivationState(ActivationState.DISABLE_DEACTIVATION);
//tr.setIdentity();
//tr.setOrigin(btVector3(btScalar(-10.), btScalar(16.), btScalar(0.)));
//tr.getBasis().setEulerZYX(0,0,0);
tr = IndexedMatrix.CreateTranslation(-10, 16, 0);
RigidBody pBodyB = LocalCreateRigidBody(1.0f, ref tr, shape);
pBodyB.SetActivationState(ActivationState.DISABLE_DEACTIVATION);
IndexedMatrix frameInA = IndexedMatrix.CreateTranslation(10f, 0f, 0f);
IndexedMatrix frameInB = IndexedMatrix.CreateTranslation(0f, 0f, 0f);
Generic6DofSpringConstraint pGen6DOFSpring = new Generic6DofSpringConstraint(pBodyA, pBodyB, ref frameInA, ref frameInB, true);
pGen6DOFSpring.SetLinearUpperLimit(new IndexedVector3(5f, 0f, 0f));
pGen6DOFSpring.SetLinearLowerLimit(new IndexedVector3(-5f, 0f, 0f));
pGen6DOFSpring.SetAngularLowerLimit(new IndexedVector3(0f, 0f, -1.5f));
pGen6DOFSpring.SetAngularUpperLimit(new IndexedVector3(0f, 0f, 1.5f));
m_dynamicsWorld.AddConstraint(pGen6DOFSpring, true);
pGen6DOFSpring.SetDbgDrawSize(5.0f);
pGen6DOFSpring.EnableSpring(0, true);
pGen6DOFSpring.SetStiffness(0, 39.478f);
pGen6DOFSpring.SetDamping(0, 0.5f);
pGen6DOFSpring.EnableSpring(5, true);
pGen6DOFSpring.SetStiffness(5, 39.478f);
pGen6DOFSpring.SetDamping(0, 0.3f);
pGen6DOFSpring.SetEquilibriumPoint();
}
#endif
#if true
{
// WORKS OK
// create a Hinge2 joint
// create two rigid bodies
// static bodyA (parent) on top:
IndexedMatrix tr = IndexedMatrix.CreateTranslation(-20f, 4f, 0f);
RigidBody pBodyA = LocalCreateRigidBody(0.0f, ref tr, shape);
pBodyA.SetActivationState(ActivationState.DISABLE_DEACTIVATION);
// dynamic bodyB (child) below it :
tr = IndexedMatrix.CreateTranslation(-20f, 0f, 0f);
RigidBody pBodyB = LocalCreateRigidBody(1.0f, ref tr, shape);
pBodyB.SetActivationState(ActivationState.DISABLE_DEACTIVATION);
// add some data to build constraint frames
IndexedVector3 parentAxis = new IndexedVector3(0.0f, 1.0f, 0.0f);
IndexedVector3 childAxis = new IndexedVector3(1.0f, 0.0f, 0.0f);
IndexedVector3 anchor = new IndexedVector3(-20.0f, 0.0f, 0.0f);
Hinge2Constraint pHinge2 = new Hinge2Constraint(pBodyA, pBodyB, ref anchor, ref parentAxis, ref childAxis);
pHinge2.SetLowerLimit(-MathUtil.SIMD_HALF_PI * 0.5f);
pHinge2.SetUpperLimit(MathUtil.SIMD_HALF_PI * 0.5f);
// add constraint to world
m_dynamicsWorld.AddConstraint(pHinge2, true);
// draw constraint frames and limits for debugging
pHinge2.SetDbgDrawSize(5.0f);
}
#endif
#if true
{
// WORKS OK
// create a Hinge joint between two dynamic bodies
// create two rigid bodies
// static bodyA (parent) on top:
IndexedMatrix tr = IndexedMatrix.CreateTranslation(-20f, -2f, 0f);
RigidBody pBodyA = LocalCreateRigidBody(1.0f, ref tr, shape);
pBodyA.SetActivationState(ActivationState.DISABLE_DEACTIVATION);
// dynamic bodyB:
tr = IndexedMatrix.CreateTranslation(-30f, -2f, 0f);
RigidBody pBodyB = LocalCreateRigidBody(10.0f, ref tr, shape);
pBodyB.SetActivationState(ActivationState.DISABLE_DEACTIVATION);
// add some data to build constraint frames
IndexedVector3 axisA = new IndexedVector3(0.0f, 1.0f, 0.0f);
IndexedVector3 axisB = new IndexedVector3(0.0f, 1.0f, 0.0f);
IndexedVector3 pivotA = new IndexedVector3(-5.0f, 0.0f, 0.0f);
IndexedVector3 pivotB = new IndexedVector3(5.0f, 0.0f, 0.0f);
spHingeDynAB = new HingeConstraint(pBodyA, pBodyB, ref pivotA, ref pivotB, ref axisA, ref axisB);
spHingeDynAB.SetLimit(-MathUtil.SIMD_HALF_PI * 0.5f, MathUtil.SIMD_HALF_PI * 0.5f);
// add constraint to world
m_dynamicsWorld.AddConstraint(spHingeDynAB, true);
// draw constraint frames and limits for debugging
spHingeDynAB.SetDbgDrawSize(5.0f);
}
#endif
}
//----------------------------------------------------------------------------------------------------------------
public override void InitializeDemo()
{
CollisionShape groundShape = new BoxShape(new IndexedVector3(50, 0.1f, 50));
IndexedVector3 wheelDimensions = new IndexedVector3(wheelWidth, wheelRadius, wheelRadius);
m_wheelShape = new CylinderShapeX(ref wheelDimensions);
m_collisionShapes.Add(groundShape);
m_collisionConfiguration = new DefaultCollisionConfiguration();
m_dispatcher = new CollisionDispatcher(m_collisionConfiguration);
IndexedVector3 worldMin = new IndexedVector3(-1000, -1000, -1000);
IndexedVector3 worldMax = new IndexedVector3(1000, 1000, 1000);
m_broadphase = new AxisSweep3Internal(ref worldMin, ref worldMax, 0xfffe, 0xffff, 16384, null, false);
m_constraintSolver = new SequentialImpulseConstraintSolver();
m_dynamicsWorld = new DiscreteDynamicsWorld(m_dispatcher, m_broadphase, m_constraintSolver, m_collisionConfiguration);
IndexedMatrix tr = IndexedMatrix.CreateTranslation(0, -10, 0);
LocalCreateRigidBody(0f, ref tr, groundShape);
#if true
CollisionShape chassisShape = new BoxShape(new IndexedVector3(1.0f, 0.5f, 2.0f));
//CollisionShape chassisShape = new BoxShape(new IndexedVector3(1.0f, 0.5f, 1.0f));
m_collisionShapes.Add(chassisShape);
CompoundShape compound = new CompoundShape();
m_collisionShapes.Add(compound);
//localTrans effectively shifts the center of mass with respect to the chassis
IndexedMatrix localTrans = IndexedMatrix.CreateTranslation(0, 1, 0);
compound.AddChildShape(ref localTrans, chassisShape);
{
CollisionShape suppShape = new BoxShape(new IndexedVector3(0.5f, 0.1f, 0.5f));
//localTrans effectively shifts the center of mass with respect to the chassis
IndexedMatrix suppLocalTrans = IndexedMatrix.CreateTranslation(0f, 1.0f, 2.5f);
compound.AddChildShape(ref suppLocalTrans, suppShape);
}
tr._origin = IndexedVector3.Zero;
m_carChassis = LocalCreateRigidBody(800f, ref tr, compound);//chassisShape);
#endif
{
#if true
CollisionShape liftShape = new BoxShape(new IndexedVector3(0.5f, 2.0f, 0.05f));
m_collisionShapes.Add(liftShape);
m_liftStartPos = new IndexedVector3(0.0f, 2.5f, 3.05f);
IndexedMatrix liftTrans = IndexedMatrix.CreateTranslation(m_liftStartPos);
m_liftBody = LocalCreateRigidBody(10f, ref liftTrans, liftShape);
IndexedMatrix localA = MathUtil.SetEulerZYX(0f, MathUtil.SIMD_HALF_PI, 0f);
localA._origin = new IndexedVector3(0f, 1.0f, 3.05f);
IndexedMatrix localB = MathUtil.SetEulerZYX(0f, MathUtil.SIMD_HALF_PI, 0f);
localB._origin = new IndexedVector3(0f, -1.5f, -0.05f);
m_liftHinge = new HingeConstraint(m_carChassis, m_liftBody, ref localA, ref localB);
MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "hinge aFrame", m_liftHinge.GetAFrame());
MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "hinge bFrame", m_liftHinge.GetBFrame());
m_liftHinge.SetLimit(0.0f, 0.0f);
m_dynamicsWorld.AddConstraint(m_liftHinge, true);
CollisionShape forkShapeA = new BoxShape(new IndexedVector3(1.0f, 0.1f, 0.1f));
m_collisionShapes.Add(forkShapeA);
CompoundShape forkCompound = new CompoundShape();
m_collisionShapes.Add(forkCompound);
IndexedMatrix forkLocalTrans = IndexedMatrix.Identity;
forkCompound.AddChildShape(ref forkLocalTrans, forkShapeA);
CollisionShape forkShapeB = new BoxShape(new IndexedVector3(0.1f, 0.02f, 0.6f));
m_collisionShapes.Add(forkShapeB);
forkLocalTrans = IndexedMatrix.CreateTranslation(-0.9f, -0.08f, 0.7f);
forkCompound.AddChildShape(ref forkLocalTrans, forkShapeB);
CollisionShape forkShapeC = new BoxShape(new IndexedVector3(0.1f, 0.02f, 0.6f));
m_collisionShapes.Add(forkShapeC);
forkLocalTrans = IndexedMatrix.CreateTranslation(0.9f, -0.08f, 0.7f);
forkCompound.AddChildShape(ref forkLocalTrans, forkShapeC);
m_forkStartPos = new IndexedVector3(0.0f, 0.6f, 3.2f);
IndexedMatrix forkTrans = IndexedMatrix.CreateTranslation(m_forkStartPos);
m_forkBody = LocalCreateRigidBody(5f, ref forkTrans, forkCompound);
localA = IndexedMatrix.Identity;
localB = IndexedMatrix.Identity;
localA._basis.SetEulerZYX(0, 0, MathUtil.SIMD_HALF_PI);
localA._origin = new IndexedVector3(0.0f, -1.9f, 0.05f);
localB._basis.SetEulerZYX(0, 0, MathUtil.SIMD_HALF_PI);
localB._origin = new IndexedVector3(0.0f, 0.0f, -0.1f);
m_forkSlider = new SliderConstraint(m_liftBody, m_forkBody, ref localA, ref localB, true);
m_forkSlider.SetLowerLinLimit(0.1f);
m_forkSlider.SetUpperLinLimit(0.1f);
// m_forkSlider.setLowerAngLimit(-LIFT_EPS);
// m_forkSlider.setUpperAngLimit(LIFT_EPS);
m_forkSlider.SetLowerAngLimit(0.0f);
m_forkSlider.SetUpperAngLimit(0.0f);
m_dynamicsWorld.AddConstraint(m_forkSlider, true);
#endif
#if true
CompoundShape loadCompound = new CompoundShape(false);
m_collisionShapes.Add(loadCompound);
CollisionShape loadShapeA = new BoxShape(new IndexedVector3(2.0f, 0.5f, 0.5f));
m_collisionShapes.Add(loadShapeA);
IndexedMatrix loadTrans = IndexedMatrix.Identity;
loadCompound.AddChildShape(ref loadTrans, loadShapeA);
CollisionShape loadShapeB = new BoxShape(new IndexedVector3(0.1f, 1.0f, 1.0f));
m_collisionShapes.Add(loadShapeB);
loadTrans = IndexedMatrix.CreateTranslation(2.1f, 0.0f, 0.0f);
loadCompound.AddChildShape(ref loadTrans, loadShapeB);
CollisionShape loadShapeC = new BoxShape(new IndexedVector3(0.1f, 1.0f, 1.0f));
m_collisionShapes.Add(loadShapeC);
loadTrans = IndexedMatrix.CreateTranslation(-2.1f, 0.0f, 0.0f);
loadCompound.AddChildShape(ref loadTrans, loadShapeC);
m_loadStartPos = new IndexedVector3(0.0f, -3.5f, 7.0f);
loadTrans = IndexedMatrix.CreateTranslation(m_loadStartPos);
m_loadBody = LocalCreateRigidBody(4f, ref loadTrans, loadCompound);
#endif
}
//m_carChassis.setDamping(0.2f, 0.2f);
ClientResetScene();
/// create vehicle
{
m_vehicleRayCaster = new DefaultVehicleRaycaster(m_dynamicsWorld);
m_vehicle = new RaycastVehicle(m_tuning, m_carChassis, m_vehicleRayCaster);
///never deactivate the vehicle
m_carChassis.SetActivationState(ActivationState.DISABLE_DEACTIVATION);
m_dynamicsWorld.AddVehicle(m_vehicle);
float connectionHeight = 1.2f;
bool isFrontWheel = true;
//choose coordinate system
m_vehicle.SetCoordinateSystem(rightIndex, upIndex, forwardIndex);
IndexedVector3 connectionPointCS0 = IndexedVector3.Zero;
//connectionPointCS0 = new IndexedVector3(CUBE_HALF_EXTENTS, connectionHeight, CUBE_HALF_EXTENTS);
//m_vehicle.addWheel(ref connectionPointCS0, ref wheelDirectionCS0, ref wheelAxleCS, suspensionRestLength, wheelRadius, m_tuning, isFrontWheel);
connectionPointCS0 = new IndexedVector3(CUBE_HALF_EXTENTS - (0.3f * wheelWidth), connectionHeight, 2.0f * CUBE_HALF_EXTENTS - wheelRadius);
m_vehicle.AddWheel(ref connectionPointCS0, ref wheelDirectionCS0, ref wheelAxleCS, suspensionRestLength, wheelRadius, m_tuning, isFrontWheel);
connectionPointCS0 = new IndexedVector3(-CUBE_HALF_EXTENTS + (0.3f * wheelWidth), connectionHeight, 2.0f * CUBE_HALF_EXTENTS - wheelRadius);
m_vehicle.AddWheel(ref connectionPointCS0, ref wheelDirectionCS0, ref wheelAxleCS, suspensionRestLength, wheelRadius, m_tuning, isFrontWheel);
//isFrontWheel = false;
connectionPointCS0 = new IndexedVector3(-CUBE_HALF_EXTENTS + (0.3f * wheelWidth), connectionHeight, -2.0f * CUBE_HALF_EXTENTS + wheelRadius);
m_vehicle.AddWheel(ref connectionPointCS0, ref wheelDirectionCS0, ref wheelAxleCS, suspensionRestLength, wheelRadius, m_tuning, isFrontWheel);
connectionPointCS0 = new IndexedVector3(CUBE_HALF_EXTENTS - (0.3f * wheelWidth), connectionHeight, -2.0f * CUBE_HALF_EXTENTS + wheelRadius);
m_vehicle.AddWheel(ref connectionPointCS0, ref wheelDirectionCS0, ref wheelAxleCS, suspensionRestLength, wheelRadius, m_tuning, isFrontWheel);
for (int i = 0; i < m_vehicle.GetNumWheels(); i++)
{
WheelInfo wheel = m_vehicle.GetWheelInfo(i);
wheel.m_suspensionStiffness = suspensionStiffness;
wheel.m_wheelsDampingRelaxation = suspensionDamping;
wheel.m_wheelsDampingCompression = suspensionCompression;
wheel.m_frictionSlip = wheelFriction;
wheel.m_rollInfluence = rollInfluence;
}
}
SetCameraDistance(36.0f);
SetTexturing(true);
SetShadows(true);
}
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);
}
/// <summary>
/// Sets the limit.
/// </summary>
/// <param name="low">The low.</param>
/// <param name="high">The high.</param>
/// <param name="softness">The softness.</param>
/// <param name="biasFactor">The bias factor.</param>
/// <param name="relaxationFactor">The relaxation factor.</param>
public void SetLimit(float low, float high, float softness = 0.9f, float biasFactor = 0.3f, float relaxationFactor = 1)
{
_hci.SetLimit(low, high, softness, biasFactor, relaxationFactor);
}
