public unsafe static void SetMotorTarget(this HingeConstraint obj, ref OpenTK.Quaternion qAinB, float dt)
{
fixed(OpenTK.Quaternion *qAinBPtr = &qAinB)
{
obj.SetMotorTarget(ref *(BulletSharp.Math.Quaternion *)qAinBPtr, dt);
}
}
/*
* public unsafe static void GetInfo2Internal(this HingeConstraint obj, ConstraintInfo2 info, ref OpenTK.Matrix4 transA, ref OpenTK.Matrix4 transB, ref OpenTK.Vector3 angVelA, ref OpenTK.Vector3 angVelB)
* {
* fixed (OpenTK.Matrix4* transAPtr = &transA)
* {
* fixed (OpenTK.Matrix4* transBPtr = &transB)
* {
* fixed (OpenTK.Vector3* angVelAPtr = &angVelA)
* {
* fixed (OpenTK.Vector3* angVelBPtr = &angVelB)
* {
* obj.GetInfo2Internal(info, ref *(BulletSharp.Math.Matrix*)transAPtr, ref *(BulletSharp.Math.Matrix*)transBPtr, ref *(BulletSharp.Math.Vector3*)angVelAPtr, ref *(BulletSharp.Math.Vector3*)angVelBPtr);
* }
* }
* }
* }
* }
*
* public unsafe static void GetInfo2InternalUsingFrameOffset(this HingeConstraint obj, ConstraintInfo2 info, ref OpenTK.Matrix4 transA, ref OpenTK.Matrix4 transB, ref OpenTK.Vector3 angVelA, ref OpenTK.Vector3 angVelB)
* {
* fixed (OpenTK.Matrix4* transAPtr = &transA)
* {
* fixed (OpenTK.Matrix4* transBPtr = &transB)
* {
* fixed (OpenTK.Vector3* angVelAPtr = &angVelA)
* {
* fixed (OpenTK.Vector3* angVelBPtr = &angVelB)
* {
* obj.GetInfo2InternalUsingFrameOffset(info, ref *(BulletSharp.Math.Matrix*)transAPtr, ref *(BulletSharp.Math.Matrix*)transBPtr, ref *(BulletSharp.Math.Vector3*)angVelAPtr, ref *(BulletSharp.Math.Vector3*)angVelBPtr);
* }
* }
* }
* }
* }
*
* public unsafe static void GetInfo2NonVirtual(this HingeConstraint obj, ConstraintInfo2 info, ref OpenTK.Matrix4 transA, ref OpenTK.Matrix4 transB, ref OpenTK.Vector3 angVelA, ref OpenTK.Vector3 angVelB)
* {
* fixed (OpenTK.Matrix4* transAPtr = &transA)
* {
* fixed (OpenTK.Matrix4* transBPtr = &transB)
* {
* fixed (OpenTK.Vector3* angVelAPtr = &angVelA)
* {
* fixed (OpenTK.Vector3* angVelBPtr = &angVelB)
* {
* obj.GetInfo2NonVirtual(info, ref *(BulletSharp.Math.Matrix*)transAPtr, ref *(BulletSharp.Math.Matrix*)transBPtr, ref *(BulletSharp.Math.Vector3*)angVelAPtr, ref *(BulletSharp.Math.Vector3*)angVelBPtr);
* }
* }
* }
* }
* }
*/
public unsafe static void SetAxis(this HingeConstraint obj, ref OpenTK.Vector3 axisInA)
{
fixed(OpenTK.Vector3 *axisInAPtr = &axisInA)
{
obj.SetAxis(ref *(BulletSharp.Math.Vector3 *)axisInAPtr);
}
}
public unsafe static void GetAFrame(this HingeConstraint obj, out OpenTK.Matrix4 value)
{
fixed(OpenTK.Matrix4 *valuePtr = &value)
{
*(BulletSharp.Math.Matrix *)valuePtr = obj.AFrame;
}
}
public unsafe static void TestLimit(this HingeConstraint obj, ref OpenTK.Matrix4 transA, ref OpenTK.Matrix4 transB)
{
fixed(OpenTK.Matrix4 *transAPtr = &transA)
{
fixed(OpenTK.Matrix4 *transBPtr = &transB)
{
obj.TestLimit(ref *(BulletSharp.Math.Matrix *)transAPtr, ref *(BulletSharp.Math.Matrix *)transBPtr);
}
}
}
public unsafe static void SetFrames(this HingeConstraint obj, ref OpenTK.Matrix4 frameA, ref OpenTK.Matrix4 frameB)
{
fixed(OpenTK.Matrix4 *frameAPtr = &frameA)
{
fixed(OpenTK.Matrix4 *frameBPtr = &frameB)
{
obj.SetFrames(ref *(BulletSharp.Math.Matrix *)frameAPtr, ref *(BulletSharp.Math.Matrix *)frameBPtr);
}
}
}
public unsafe static float GetHingeAngle(this HingeConstraint obj, ref OpenTK.Matrix4 transA, ref OpenTK.Matrix4 transB)
{
fixed(OpenTK.Matrix4 *transAPtr = &transA)
{
fixed(OpenTK.Matrix4 *transBPtr = &transB)
{
return(obj.GetHingeAngle(ref *(BulletSharp.Math.Matrix *)transAPtr, ref *(BulletSharp.Math.Matrix *)transBPtr));
}
}
}
public static OpenTK.Matrix4 GetFrameOffsetB(this HingeConstraint obj)
{
OpenTK.Matrix4 value;
GetFrameOffsetB(obj, out value);
return(value);
}
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.05f, 0.45f);
shapes[(int)BodyPart.LeftLowerLeg] = new CapsuleShape(0.04f, 0.37f);
shapes[(int)BodyPart.RightUpperLeg] = new CapsuleShape(0.05f, 0.45f);
shapes[(int)BodyPart.RightLowerLeg] = new CapsuleShape(0.04f, 0.37f);
shapes[(int)BodyPart.LeftUpperArm] = new CapsuleShape(0.04f, 0.33f);
shapes[(int)BodyPart.LeftLowerArm] = new CapsuleShape(0.03f, 0.25f);
shapes[(int)BodyPart.RightUpperArm] = new CapsuleShape(0.04f, 0.33f);
shapes[(int)BodyPart.RightLowerArm] = new CapsuleShape(0.03f, 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.RotationZ(PI_2) * offset * Matrix.Translation(-0.35f, 1.45f, 0);
bodies[(int)BodyPart.LeftUpperArm] = LocalCreateRigidBody(1, transform, shapes[(int)BodyPart.LeftUpperArm]);
transform = Matrix.RotationZ(PI_2) * offset * Matrix.Translation(-0.7f, 1.45f, 0);
bodies[(int)BodyPart.LeftLowerArm] = LocalCreateRigidBody(1, transform, shapes[(int)BodyPart.LeftLowerArm]);
transform = Matrix.RotationZ(-PI_2) * offset * Matrix.Translation(0.35f, 1.45f, 0);
bodies[(int)BodyPart.RightUpperArm] = LocalCreateRigidBody(1, transform, shapes[(int)BodyPart.RightUpperArm]);
transform = Matrix.RotationZ(-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(PI_2, 0, 0) * Matrix.Translation(0, 0.15f, 0);
localB = Matrix.RotationYawPitchRoll(PI_2, 0, 0) * Matrix.Translation(0, -0.15f, 0);
hingeC = new HingeConstraint(bodies[(int)BodyPart.Pelvis], bodies[(int)BodyPart.Spine], localA, localB);
hingeC.SetLimit(-PI_4, PI_2);
joints[(int)Joint.PelvisSpine] = hingeC;
hingeC.DebugDrawSize = ConstraintDebugSize;
ownerWorld.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);
coneC = new ConeTwistConstraint(bodies[(int)BodyPart.Spine], bodies[(int)BodyPart.Head], localA, localB);
coneC.SetLimit(PI_4, PI_4, PI_2);
joints[(int)Joint.SpineHead] = coneC;
coneC.DebugDrawSize = ConstraintDebugSize;
ownerWorld.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);
coneC = new ConeTwistConstraint(bodies[(int)BodyPart.Pelvis], bodies[(int)BodyPart.LeftUpperLeg], localA, localB);
coneC.SetLimit(PI_4, PI_4, 0);
joints[(int)Joint.LeftHip] = coneC;
coneC.DebugDrawSize = ConstraintDebugSize;
ownerWorld.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);
hingeC = new HingeConstraint(bodies[(int)BodyPart.LeftUpperLeg], bodies[(int)BodyPart.LeftLowerLeg], localA, localB);
hingeC.SetLimit(0, PI_2);
joints[(int)Joint.LeftKnee] = hingeC;
hingeC.DebugDrawSize = ConstraintDebugSize;
ownerWorld.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);
coneC = new ConeTwistConstraint(bodies[(int)BodyPart.Pelvis], bodies[(int)BodyPart.RightUpperLeg], localA, localB);
coneC.SetLimit(PI_4, PI_4, 0);
joints[(int)Joint.RightHip] = coneC;
coneC.DebugDrawSize = ConstraintDebugSize;
ownerWorld.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);
hingeC = new HingeConstraint(bodies[(int)BodyPart.RightUpperLeg], bodies[(int)BodyPart.RightLowerLeg], localA, localB);
hingeC.SetLimit(0, 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, PI_2) * Matrix.Translation(0, -0.18f, 0);
coneC = new ConeTwistConstraint(bodies[(int)BodyPart.Spine], bodies[(int)BodyPart.LeftUpperArm], localA, localB);
coneC.SetLimit(PI_2, PI_2, 0);
joints[(int)Joint.LeftShoulder] = coneC;
coneC.DebugDrawSize = ConstraintDebugSize;
ownerWorld.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);
hingeC = new HingeConstraint(bodies[(int)BodyPart.LeftUpperArm], bodies[(int)BodyPart.LeftLowerArm], localA, localB);
hingeC.SetLimit(0, 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, PI_2) * Matrix.Translation(0, -0.18f, 0);
coneC = new ConeTwistConstraint(bodies[(int)BodyPart.Spine], bodies[(int)BodyPart.RightUpperArm], localA, localB);
coneC.SetLimit(PI_2, PI_2, 0);
joints[(int)Joint.RightShoulder] = coneC;
coneC.DebugDrawSize = ConstraintDebugSize;
ownerWorld.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);
hingeC = new HingeConstraint(bodies[(int)BodyPart.RightUpperArm], bodies[(int)BodyPart.RightLowerArm], localA, localB);
//hingeC.SetLimit(-PI_2, 0);
hingeC.SetLimit(0, PI_2);
joints[(int)Joint.RightElbow] = hingeC;
hingeC.DebugDrawSize = ConstraintDebugSize;
ownerWorld.AddConstraint(joints[(int)Joint.RightElbow], true);
}
public bool CreateRagdoll(RDSetup setup)
{
// No entity, setup or body count overflow - bypass function.
if (setup == null || setup.BodySetup.Count > Bf.BoneTags.Count)
{
return false;
}
var result = true;
// If ragdoll already exists, overwrite it with new one.
if (Bt.BtJoints.Count > 0)
{
result = DeleteRagdoll();
}
// Setup bodies.
Bt.BtJoints.Clear();
// update current character animation and full fix body to avoid starting ragdoll partially inside the wall or floor...
UpdateCurrentBoneFrame(Bf, Transform);
Bt.NoFixAll = false;
Bt.NoFixBodyParts = 0x00000000;
#if NOPE
int map_size = m_bf.animations.model->collision_map.size(); // does not works, strange...
m_bf.animations.model->collision_map.size() = m_bf.animations.model->mesh_count;
fixPenetrations(nullptr);
m_bf.animations.model->collision_map.size() = map_size;
#else
FixPenetrations(Vector3.Zero);
#endif
for(var i = 0; i < setup.BodySetup.Count; i++)
{
// TODO: First check useless?
if(i >= Bf.BoneTags.Count || Bt.BtBody[i] == null)
{
result = false;
continue; // If body is absent, return false and bypass this body setup.
}
var inertia = BulletSharp.Math.Vector3.Zero;
var mass = setup.BodySetup[i].Mass;
BtEngineDynamicsWorld.RemoveRigidBody(Bt.BtBody[i]);
Bt.BtBody[i].CollisionShape.CalculateLocalInertia(mass, out inertia);
Bt.BtBody[i].SetMassProps(mass, inertia);
Bt.BtBody[i].UpdateInertiaTensor();
Bt.BtBody[i].ClearForces();
Bt.BtBody[i].LinearFactor = BulletSharp.Math.Vector3.One;
Bt.BtBody[i].AngularFactor = BulletSharp.Math.Vector3.One;
Bt.BtBody[i].SetDamping(setup.BodySetup[i].Damping[0], setup.BodySetup[i].Damping[1]);
Bt.BtBody[i].Restitution = setup.BodySetup[i].Restitution;
Bt.BtBody[i].Friction = setup.BodySetup[i].Friction;
Bt.BtBody[i].SetSleepingThresholds(RD_DEFAULT_SLEEPING_THRESHOLD, RD_DEFAULT_SLEEPING_THRESHOLD);
if(Bf.BoneTags[i].Parent == null)
{
var r = GetInnerBBRadius(Bf.BoneTags[i].MeshBase.BBMin, Bf.BoneTags[i].MeshBase.BBMax);
Bt.BtBody[i].CcdMotionThreshold = 0.8f * r;
Bt.BtBody[i].CcdSweptSphereRadius = r;
}
}
UpdateRigidBody(true);
for(var i = 0; i < Bf.BoneTags.Count; i++)
{
BtEngineDynamicsWorld.AddRigidBody(Bt.BtBody[i]);
Bt.BtBody[i].Activate();
Bt.BtBody[i].LinearVelocity = Speed.ToBullet();
if (i < Bt.GhostObjects.Count && Bt.GhostObjects[i] != null)
{
BtEngineDynamicsWorld.RemoveCollisionObject(Bt.GhostObjects[i]);
BtEngineDynamicsWorld.AddCollisionObject(Bt.GhostObjects[i], CollisionFilterGroups.None, CollisionFilterGroups.None);
}
}
// Setup constraints.
Bt.BtJoints.Resize(setup.JointSetup.Count);
for(var i = 0; i < setup.JointSetup.Count; i++)
{
if(setup.JointSetup[i].BodyIndex >= Bf.BoneTags.Count || Bt.BtBody[setup.JointSetup[i].BodyIndex] == null)
{
result = false;
break; // If body 1 or body 2 are absent, return false and bypass this joint.
}
var localA = new Transform();
var localB = new Transform();
var btB = Bf.BoneTags[setup.JointSetup[i].BodyIndex];
var btA = btB.Parent;
if(btA == null)
{
result = false;
break;
}
#if NOPE
localA.setFromOpenGLMatrix(btB->transform);
localB.setIdentity();
#else
Helper.SetEulerZYX(ref localA.Basis, setup.JointSetup[i].Body1Angle);
//localA.Origin = setup.JointSetup[i].Body1Offset;
localA.Origin = btB.Transform.Origin;
Helper.SetEulerZYX(ref localB.Basis, setup.JointSetup[i].Body2Angle);
//localB.Origin = setup.JointSetup[i].Body2Offset;
localB.Origin = Vector3.Zero;
#endif
switch(setup.JointSetup[i].JointType)
{
case RDJointSetup.Type.Point:
Bt.BtJoints[i] = new Point2PointConstraint(Bt.BtBody[btA.Index], Bt.BtBody[btB.Index],
localA.Origin.ToBullet(), localB.Origin.ToBullet());
break;
case RDJointSetup.Type.Hinge:
var hingeC = new HingeConstraint(Bt.BtBody[btA.Index], Bt.BtBody[btB.Index], ((Matrix4) localA).ToBullet(),
((Matrix4) localB).ToBullet());
hingeC.SetLimit(setup.JointSetup[i].JointLimit[0], setup.JointSetup[i].JointLimit[1], 0.9f, 0.3f, 0.3f);
Bt.BtJoints[i] = hingeC;
break;
case RDJointSetup.Type.Cone:
var coneC = new ConeTwistConstraint(Bt.BtBody[btA.Index], Bt.BtBody[btB.Index], ((Matrix4)localA).ToBullet(),
((Matrix4)localB).ToBullet());
coneC.SetLimit(setup.JointSetup[i].JointLimit[0], setup.JointSetup[i].JointLimit[1], setup.JointSetup[i].JointLimit[2], 0.9f, 0.3f, 0.7f);
Bt.BtJoints[i] = coneC;
break;
}
Bt.BtJoints[i].SetParam(ConstraintParam.StopCfm, setup.JointCfm, -1);
Bt.BtJoints[i].SetParam(ConstraintParam.StopErp, setup.JointErp, -1);
Bt.BtJoints[i].DebugDrawSize = 64.0f;
BtEngineDynamicsWorld.AddConstraint(Bt.BtJoints[i], true);
}
if(!result)
{
DeleteRagdoll(); // PARANOID: Clean up the mess, if something went wrong.
}
else
{
TypeFlags |= ENTITY_TYPE.Dynamic;
}
return result;
}
//called by Physics World just before constraint is added to world.
//the current constraint properties are used to rebuild the constraint.
internal override bool _BuildConstraint()
{
BPhysicsWorld world = BPhysicsWorld.Get();
if (m_constraintPtr != null) {
if (m_isInWorld && world != null) {
m_isInWorld = false;
world.RemoveConstraint(m_constraintPtr);
}
}
BRigidBody targetRigidBodyA = GetComponent<BRigidBody>();
if (targetRigidBodyA == null)
{
Debug.LogError("BHingedConstraint needs to be added to a component with a BRigidBody.");
return false;
}
if (!targetRigidBodyA.isInWorld)
{
world.AddRigidBody(targetRigidBodyA);
}
if (m_localConstraintAxisX == Vector3.zero)
{
Debug.LogError("Constaint axis cannot be zero vector");
return false;
}
RigidBody rba = (RigidBody) targetRigidBodyA.GetCollisionObject();
if (rba == null) {
Debug.LogError("Constraint could not get bullet RigidBody from target rigid body");
return false;
}
if (m_constraintType == ConstraintType.constrainToAnotherBody)
{
if (m_otherRigidBody == null)
{
Debug.LogError("Other rigid body must not be null");
return false;
}
if (!m_otherRigidBody.isInWorld)
{
world.AddRigidBody(m_otherRigidBody);
}
RigidBody rbb = (RigidBody) m_otherRigidBody.GetCollisionObject();
if (rbb == null)
{
Debug.LogError("Constraint could not get bullet RigidBody from target rigid body");
return false;
}
BM.Matrix frameInA, frameInOther;
string errormsg = "";
if (CreateFramesA_B(m_localConstraintAxisX, m_localConstraintAxisY, m_localConstraintPoint, out frameInA, out frameInOther, ref errormsg))
{
//warning the frameInA, frameInB version of the constructor is broken
m_constraintPtr = new HingeConstraint(rbb, rba, frameInOther.Origin, frameInA.Origin, (BM.Vector3)frameInOther.Basis.Column1, (BM.Vector3)frameInA.Basis.Column1);
} else
{
Debug.LogError(errormsg);
return false;
}
}
else {
//BM.Matrix frameInA = BM.Matrix.Identity;
//CreateFrame(m_localConstraintForwardDir, m_localConstraintUpDir, m_localConstraintPoint, ref frameInA);
m_constraintPtr = new HingeConstraint(rba, m_localConstraintPoint.ToBullet(),m_localConstraintAxisX.ToBullet(), false);
}
if (m_enableMotor)
{
((HingeConstraint)m_constraintPtr).EnableAngularMotor(true, m_targetMotorAngularVelocity, m_maxMotorImpulse);
}
if (m_setLimit)
{
((HingeConstraint)m_constraintPtr).SetLimit(m_lowLimitAngleRadians, m_highLimitAngleRadians, m_limitSoftness, m_limitBiasFactor);
}
m_constraintPtr.Userobject = this;
m_constraintPtr.DebugDrawSize = m_debugDrawSize;
m_constraintPtr.BreakingImpulseThreshold = m_breakingImpulseThreshold;
return true;
}
void CreateGear(Vector3 pos, float speed)
{
Matrix startTransform = Matrix.Translation(pos);
CompoundShape shape = new CompoundShape();
#if true
shape.AddChildShape(Matrix.Identity, new BoxShape(5, 1, 6));
shape.AddChildShape(Matrix.RotationZ((float)Math.PI), new BoxShape(5, 1, 6));
#else
shape.AddChildShape(Matrix.Identity, new CylinderShapeZ(5,1,7));
shape.AddChildShape(Matrix.RotationZ((float)Math.PI), new BoxShape(4,1,8));
#endif
RigidBody body = LocalCreateRigidBody(10, startTransform, shape);
body.Friction = 1;
HingeConstraint hinge = new HingeConstraint(body, Matrix.Identity);
if (speed != 0) hinge.EnableAngularMotor(true, speed, 3);
World.AddConstraint(hinge);
}
protected override void OnInitializePhysics()
{
SetupEmptyDynamicsWorld();
IsDebugDrawEnabled = true;
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 HingeConstraint CreateHingeConstraint(RigidBody rbA, ref Matrix rbAFrame)
{
HingeConstraint constraint = new HingeConstraint(rbA, rbAFrame);
_allocatedConstraints.Add(constraint);
return constraint;
}
public HingeConstraint CreateHingeConstraint(RigidBody rbA, ref Matrix rbAFrame, bool useReferenceFrameA)
{
HingeConstraint constraint = new HingeConstraint(rbA, rbAFrame, useReferenceFrameA);
_allocatedConstraints.Add(constraint);
return constraint;
}
//called by Physics World just before constraint is added to world.
//the current constraint properties are used to rebuild the constraint.
internal override bool _BuildConstraint()
{
BPhysicsWorld world = BPhysicsWorld.Get();
if (constraintPtr != null) {
if (isInWorld && world != null) {
isInWorld = false;
world.RemoveConstraint(constraintPtr);
}
}
if (targetRigidBodyA == null) {
Debug.LogError("Constraint target rigid body was not set.");
return false;
}
if (localPivotAxisA == Vector3.zero)
{
Debug.LogError("Constaint axis cannot be zero vector");
return false;
}
RigidBody rba = targetRigidBodyA.GetRigidBody();
if (rba == null) {
Debug.LogError("Constraint could not get bullet RigidBody from target rigid body");
return false;
}
if (constraintType == ConstraintType.constrainToAnotherBody)
{
RigidBody rbb = targetRigidBodyB.GetRigidBody();
if (rbb == null)
{
Debug.LogError("Constraint could not get bullet RigidBody from target rigid body");
return false;
}
constraintPtr = new HingeConstraint(rba,rbb,localPointOnHingeOffsetA.ToBullet(),localPointOnHingeOffsetB.ToBullet(),localPivotAxisA.ToBullet(),localPivotAxisB.ToBullet());
}
else {
constraintPtr = new HingeConstraint(rba, localPointOnHingeOffsetA.ToBullet(), localPivotAxisA.ToBullet());
}
if (enableMotor)
{
((HingeConstraint)constraintPtr).EnableAngularMotor(true, targetMotorAngularVelocity, maxMotorImpulse);
}
if (setLimit)
{
((HingeConstraint)constraintPtr).SetLimit(lowLimitAngleRadians, highLimitAngleRadians, limitSoftness, limitBiasFactor);
}
return true;
}
public CarScene()
{
Object3dInfo waterInfo = Object3dGenerator.CreateGround(new Vector2(-200, -200), new Vector2(200, 200), new Vector2(100, 100), Vector3.UnitY);
var color = GenericMaterial.FromMedia("checked.png", "183_norm.JPG");
Mesh3d water = new Mesh3d(waterInfo, color);
water.SetMass(0);
water.Translate(0, -10, 0);
water.SetCollisionShape(new BulletSharp.StaticPlaneShape(Vector3.UnitY, 0));
World.Root.Add(water);
var lod1 = Object3dInfo.LoadFromRaw(Media.Get("lucy_lod1.vbo.raw"), Media.Get("lucy_lod1.indices.raw"));
var mat = new GenericMaterial(new Vector4(0, 0, 1, 1f));
var dragon = new Mesh3d(lod1, mat);
dragon.Transformation.Scale(2);
Add(dragon);
var rand = new Random();
var grasslod0 = Object3dInfo.LoadFromObjSingle(Media.Get("grasslod1.obj"));
var grasslod1 = Object3dInfo.LoadFromObjSingle(Media.Get("grasslod1.obj"));
var grasslod2 = Object3dInfo.LoadFromObjSingle(Media.Get("grasslod2.obj"));
var grasslod3 = Object3dInfo.LoadFromObjSingle(Media.Get("grasslod3.obj"));
var grasscolor = new GenericMaterial(Color.DarkGreen);
var grassInstanced = new InstancedMesh3d(grasslod2, grasscolor);
for(int x = -15; x < 15; x++)
{
for(int y = -15; y < 15; y++)
{
grassInstanced.Transformations.Add(new TransformationManager(new Vector3(x, 0, y), Quaternion.FromAxisAngle(Vector3.UnitY, (float)rand.NextDouble() * MathHelper.Pi), new Vector3(1, 1, 1)));
grassInstanced.Instances++;
}
}
grassInstanced.UpdateMatrix();
Add(grassInstanced);
ProjectionLight redConeLight = new ProjectionLight(new Vector3(1, 25, 1), Quaternion.Identity, 5000, 5000, MathHelper.PiOver2, 1.0f, 10000.0f);
redConeLight.LightColor = new Vector4(1, 1, 1, 1);
redConeLight.BuildOrthographicProjection(500, 500, -100, 100);
redConeLight.camera.LookAt(Vector3.Zero);
LightPool.Add(redConeLight);
var wheel3dInfo = Object3dInfo.LoadFromObjSingle(Media.Get("fcarwheel.obj"));
var car3dInfo = Object3dInfo.LoadFromObjSingle(Media.Get("fcarbody.obj"));
var car = new Mesh3d(car3dInfo, new GenericMaterial(Color.LightGreen));
car.SetMass(200);
//car.Translate(0, 3.76f, 0);
car.SetCollisionShape(new BoxShape(0.5f, 0.5f, 0.5f));
car.CreateRigidBody(new Vector3(0, -25, 0), true);
car.PhysicalBody.SetSleepingThresholds(0, 0);
car.PhysicalBody.ContactProcessingThreshold = 0;
car.PhysicalBody.CcdMotionThreshold = 0;
CurrentCar = car;
World.Root.Add(car);
GLThread.OnUpdate += (o, e) =>
{
redConeLight.SetPosition(car.GetPosition() + new Vector3(0, 15, 0));
};
var shape = new SphereShape(0.5f);
//var shape = wheel3dInfo.GetAccurateCollisionShape();
var wheelLF = new Mesh3d(wheel3dInfo, new GenericMaterial(Color.White));
wheelLF.SetMass(100);
wheelLF.Translate(1.640539f, 0.48866f, -1.94906f);
wheelLF.SetCollisionShape(shape);
wheelLF.CreateRigidBody();
World.Root.Add(wheelLF);
var wheelRF = new Mesh3d(wheel3dInfo, new GenericMaterial(Color.White));
wheelRF.SetMass(100);
wheelRF.Translate(1.640539f, 0.48866f, 1.94906f);
wheelRF.SetCollisionShape(shape);
wheelRF.CreateRigidBody();
World.Root.Add(wheelRF);
var wheelLR = new Mesh3d(wheel3dInfo, new GenericMaterial(Color.White));
wheelLR.SetMass(100);
wheelLR.Translate(-1.640539f, 0.48866f, -1.94906f);
wheelLR.SetCollisionShape(shape);
wheelLR.CreateRigidBody();
World.Root.Add(wheelLR);
var wheelRR = new Mesh3d(wheel3dInfo, new GenericMaterial(Color.White));
wheelRR.SetMass(100);
wheelRR.Translate(-1.640539f, 0.48866f, 1.94906f);
wheelRR.SetCollisionShape(shape);
wheelRR.CreateRigidBody();
World.Root.Add(wheelRR);
wheelLF.PhysicalBody.SetSleepingThresholds(0, 0);
wheelLF.PhysicalBody.ContactProcessingThreshold = 0;
wheelLF.PhysicalBody.CcdMotionThreshold = 0;
wheelRF.PhysicalBody.SetSleepingThresholds(0, 0);
wheelRF.PhysicalBody.ContactProcessingThreshold = 0;
wheelRF.PhysicalBody.CcdMotionThreshold = 0;
wheelLR.PhysicalBody.SetSleepingThresholds(0, 0);
wheelLR.PhysicalBody.ContactProcessingThreshold = 0;
wheelLR.PhysicalBody.CcdMotionThreshold = 0;
wheelLR.PhysicalBody.SetSleepingThresholds(0, 0);
wheelLR.PhysicalBody.ContactProcessingThreshold = 0;
wheelLR.PhysicalBody.CcdMotionThreshold = 0;
wheelRR.PhysicalBody.SetSleepingThresholds(0, 0);
wheelRR.PhysicalBody.ContactProcessingThreshold = 0;
wheelRR.PhysicalBody.CcdMotionThreshold = 0;
wheelLF.PhysicalBody.Friction = 1200;
wheelRF.PhysicalBody.Friction = 1200;
wheelLR.PhysicalBody.Friction = 1200;
wheelRR.PhysicalBody.Friction = 1200;
car.PhysicalBody.SetIgnoreCollisionCheck(wheelLF.PhysicalBody, true);
car.PhysicalBody.SetIgnoreCollisionCheck(wheelRF.PhysicalBody, true);
car.PhysicalBody.SetIgnoreCollisionCheck(wheelLR.PhysicalBody, true);
car.PhysicalBody.SetIgnoreCollisionCheck(wheelRR.PhysicalBody, true);
// location left rear x -5.27709 y 1.56474 z -3.13906
// location right rear x -5.27709 y 1.56474 z 3.13906
// location left front x 5.500539 y 1.56474 z -3.13906
// location right front x 5.500539 y 1.56474 z 3.13906
//public HingeConstraint(RigidBody rigidBodyA, RigidBody rigidBodyB, Vector3 pivotInA, Vector3 pivotInB, Vector3 axisInA, Vector3 axisInB);
var frontAxis = new HingeConstraint(wheelLF.PhysicalBody, wheelRF.PhysicalBody,
new Vector3(1.640539f, 0.48866f, 1.94906f), new Vector3(1.640539f, 0.48866f, -1.94906f), Vector3.UnitZ, Vector3.UnitZ);
var rearAxis = new HingeConstraint(wheelLR.PhysicalBody, wheelRR.PhysicalBody,
new Vector3(-1.640539f, 0.48866f, 1.94906f), new Vector3(-1.640539f, 0.48866f, -1.94906f), Vector3.UnitZ, Vector3.UnitZ);
var centerAxis1 = new HingeConstraint(car.PhysicalBody, wheelLF.PhysicalBody,
new Vector3(1.640539f, -0.48866f, -1.94906f), new Vector3(0), Vector3.UnitZ, Vector3.UnitZ);
var centerAxis2 = new HingeConstraint(car.PhysicalBody, wheelRF.PhysicalBody,
new Vector3(1.640539f, -0.48866f, 1.94906f), new Vector3(0), Vector3.UnitZ, Vector3.UnitZ);
var centerAxis3 = new HingeConstraint(car.PhysicalBody, wheelLR.PhysicalBody,
new Vector3(-1.640539f, -0.48866f, 1.94906f), new Vector3(0), Vector3.UnitZ, Vector3.UnitZ);
var centerAxis4 = new HingeConstraint(car.PhysicalBody, wheelRR.PhysicalBody,
new Vector3(-1.640539f, -0.48866f, -1.94906f), new Vector3(0), Vector3.UnitZ, Vector3.UnitZ);
centerAxis1.SetLimit(0, 10, 0, 0, 0);
centerAxis2.SetLimit(0, 10, 0, 0, 0);
centerAxis3.SetLimit(0, 10, 0, 0, 0);
centerAxis4.SetLimit(0, 10, 0, 0, 0);
//World.Root.PhysicalWorld.AddConstraint(frontAxis);
// World.Root.PhysicalWorld.AddConstraint(rearAxis);
// centerAxis1.SetLimit(0, 0.01f);
// centerAxis2.SetLimit(0, 0.01f);
//centerAxis3.SetLimit(0, 0.01f);
//centerAxis4.SetLimit(0, 0.01f);
World.Root.PhysicalWorld.AddConstraint(centerAxis1);
World.Root.PhysicalWorld.AddConstraint(centerAxis2);
World.Root.PhysicalWorld.AddConstraint(centerAxis3);
World.Root.PhysicalWorld.AddConstraint(centerAxis4);
GLThread.OnKeyDown += (object sender, OpenTK.Input.KeyboardKeyEventArgs e) =>
{
if(e.Key == OpenTK.Input.Key.R)
{
car.SetOrientation(Quaternion.Identity);
}
if(e.Key == OpenTK.Input.Key.Space)
{
centerAxis1.EnableMotor = false;
centerAxis2.EnableMotor = false;
}
if(e.Key == OpenTK.Input.Key.Up)
{
centerAxis1.EnableMotor = true;
centerAxis1.EnableAngularMotor(true, -100.0f, 10.0f);
centerAxis2.EnableMotor = true;
centerAxis2.EnableAngularMotor(true, -100.0f, 10.0f);
}
if(e.Key == OpenTK.Input.Key.Down)
{
centerAxis1.EnableMotor = true;
centerAxis1.EnableAngularMotor(true, 100.0f, 6.0f);
centerAxis2.EnableMotor = true;
centerAxis2.EnableAngularMotor(true, 100.0f, 6.0f);
}
if(e.Key == OpenTK.Input.Key.Left)
{
float angle = 0.6f / (car.PhysicalBody.LinearVelocity.Length + 1.0f);
centerAxis3.SetFrames(Matrix4.CreateRotationY(angle) * Matrix4.CreateTranslation(new Vector3(-1.640539f, -0.48866f, 1.94906f)), Matrix4.CreateTranslation(new Vector3(0)));
centerAxis4.SetFrames(Matrix4.CreateRotationY(angle) * Matrix4.CreateTranslation(new Vector3(-1.640539f, -0.48866f, -1.94906f)), Matrix4.CreateTranslation(new Vector3(0)));
//centerAxis3.SetAxis(new Vector3(1, 0, 1));
//centerAxis4.SetAxis(new Vector3(1, 0, 1));
}
if(e.Key == OpenTK.Input.Key.Right)
{
float angle = 0.6f / (car.PhysicalBody.LinearVelocity.Length + 1.0f);
centerAxis3.SetFrames(Matrix4.CreateRotationY(-angle) * Matrix4.CreateTranslation(new Vector3(-1.640539f, -0.48866f, 1.94906f)), Matrix4.CreateTranslation(new Vector3(0)));
centerAxis4.SetFrames(Matrix4.CreateRotationY(-angle) * Matrix4.CreateTranslation(new Vector3(-1.640539f, -0.48866f, -1.94906f)), Matrix4.CreateTranslation(new Vector3(0)));
//centerAxis3.SetAxis(new Vector3(-1, 0, 1));
//centerAxis4.SetAxis(new Vector3(-1, 0, 1));
}
};
GLThread.OnKeyUp += (object sender, OpenTK.Input.KeyboardKeyEventArgs e) =>
{
if(e.Key == OpenTK.Input.Key.Up)
{
centerAxis1.EnableMotor = false;
centerAxis2.EnableMotor = false;
}
if(e.Key == OpenTK.Input.Key.Down)
{
centerAxis1.EnableMotor = false;
centerAxis2.EnableMotor = false;
}
if(e.Key == OpenTK.Input.Key.Left)
{
centerAxis3.SetFrames(Matrix4.CreateTranslation(new Vector3(-1.640539f, -0.48866f, 1.94906f)), Matrix4.CreateTranslation(new Vector3(0)));
centerAxis4.SetFrames(Matrix4.CreateTranslation(new Vector3(-1.640539f, -0.48866f, -1.94906f)), Matrix4.CreateTranslation(new Vector3(0)));
//centerAxis3.SetAxis(new Vector3(0, 0, 1));
//centerAxis4.SetAxis(new Vector3(0, 0, 1));
}
if(e.Key == OpenTK.Input.Key.Right)
{
centerAxis3.SetFrames( Matrix4.CreateTranslation(new Vector3(-1.640539f, -0.48866f, 1.94906f)), Matrix4.CreateTranslation(new Vector3(0)));
centerAxis4.SetFrames(Matrix4.CreateTranslation(new Vector3(-1.640539f, -0.48866f, -1.94906f)), Matrix4.CreateTranslation(new Vector3(0)));
//centerAxis3.SetAxis(new Vector3(0, 0, 1));
//centerAxis4.SetAxis(new Vector3(0, 0, 1));
}
};
}
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 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;
}
