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