private void PickRigidBody(RigidBody body, ref Vector3 pickPosition) { if (body.IsStaticObject || body.IsKinematicObject) { return; } _pickedBody = body; _pickedBody.ActivationState = ActivationState.DisableDeactivation; DiscreteDynamicsWorld world = _demo.Simulation.World; Vector3 localPivot = Vector3.TransformCoordinate(pickPosition, Matrix.Invert(body.CenterOfMassTransform)); if (_demo.Input.KeysDown.Contains(Keys.ShiftKey)) { var dof6 = new Generic6DofConstraint(body, Matrix.Translation(localPivot), false) { LinearLowerLimit = Vector3.Zero, LinearUpperLimit = Vector3.Zero, AngularLowerLimit = Vector3.Zero, AngularUpperLimit = Vector3.Zero }; world.AddConstraint(dof6); _rigidBodyPickConstraint = dof6; dof6.SetParam(ConstraintParam.StopCfm, 0.8f, 0); dof6.SetParam(ConstraintParam.StopCfm, 0.8f, 1); dof6.SetParam(ConstraintParam.StopCfm, 0.8f, 2); dof6.SetParam(ConstraintParam.StopCfm, 0.8f, 3); dof6.SetParam(ConstraintParam.StopCfm, 0.8f, 4); dof6.SetParam(ConstraintParam.StopCfm, 0.8f, 5); dof6.SetParam(ConstraintParam.StopErp, 0.1f, 0); dof6.SetParam(ConstraintParam.StopErp, 0.1f, 1); dof6.SetParam(ConstraintParam.StopErp, 0.1f, 2); dof6.SetParam(ConstraintParam.StopErp, 0.1f, 3); dof6.SetParam(ConstraintParam.StopErp, 0.1f, 4); dof6.SetParam(ConstraintParam.StopErp, 0.1f, 5); } else { var p2p = new Point2PointConstraint(body, localPivot); world.AddConstraint(p2p); _rigidBodyPickConstraint = p2p; p2p.Setting.ImpulseClamp = 30; //very weak constraint for picking p2p.Setting.Tau = 0.001f; /* * p2p.SetParam(ConstraintParams.Cfm, 0.8f, 0); * p2p.SetParam(ConstraintParams.Cfm, 0.8f, 1); * p2p.SetParam(ConstraintParams.Cfm, 0.8f, 2); * p2p.SetParam(ConstraintParams.Erp, 0.1f, 0); * p2p.SetParam(ConstraintParams.Erp, 0.1f, 1); * p2p.SetParam(ConstraintParams.Erp, 0.1f, 2); */ } }
protected void Add(PhyBody phyBody) { _AddPhyBody(phyBody); AddBulletPhysicsTransform(phyBody); phyBody.SetWorldTransform(phyBody.matrix); if (phyBody.Constraint != null) { World.AddConstraint(phyBody.Constraint); } }
void InstalizeJoints(JointContainer[] jcons) { joints = new Joint[jcons.Length]; for (int i = 0; i < jcons.Length; i++) { joints[i] = new Joint(jcons[i], rigitBodies); if (joints[i].Constraint != null) { World.AddConstraint(joints[i].Constraint, true); } } }
void CreateParticles() { //ZeroBody zb = new ZeroShape(); BoxShape brick = new BoxShape(0.1, 0.1, 0.1); RigidBody fix = CreateBody(0, brick, new Vector3(0, 0, 0)); fix.SetSleepingThresholds(0, 0); fix.SetAnisotropicFriction(Vector3.Zero, AnisotropicFrictionFlags.FrictionDisabled); fix.Friction = 0; fix.SetDamping(0, 0); Random rand = new Random(); SphereShape ss = new SphereShape(particleSize); for (int xx = 0; xx < 115; xx++) { for (int yy = 0; yy < 90; yy++) { RigidBody r = CreateBody(1, ss, new Vector3(5 + xx * 0.51, 5 + yy * 0.51, 0)); r.Friction = 0.0; r.Restitution = 1; r.RollingFriction = 0; r.SetDamping(0.01, 0.1); Particles.Add(r); r.SetSleepingThresholds(0, 0); r.SetAnisotropicFriction(Vector3.Zero, AnisotropicFrictionFlags.FrictionDisabled); var c = new Generic6DofConstraint(fix, r, Matrix.Translation(1, 0, 0), Matrix.Translation(1, 0, 0), true); //c.BreakingImpulseThreshold = 999999999999999; c.SetLimit(0, 0, 150); c.SetLimit(1, 0, 100); c.SetLimit(2, 0, 0); c.SetLimit(3, 0, 0); c.SetLimit(4, 0, 0); c.SetLimit(5, 0, 0); //r.ApplyCentralImpulse(new Vector3(10, 10, 0)); //r.ApplyCentralForce(new Vector3(5, 5, 0)); world.AddConstraint(c); } } Console.WriteLine("Created"); }
//Register a constraint with the simulation public override void Register(TypedConstraint constraint) { if (!_initlialized) { InitializeWorld(BulletUpdate); Debug.LogWarning("A Constraint attempted to register with the simulation before it was initialized!\n" + "Please check your script execution order"); } //Check if already registered and warn the user if (_bulletConstraints.Contains(constraint)) { Debug.LogError("Specified Constraint has already been registered with the simulation!\n"); return; } //Register the Bullet RigidBody with the simulation and list for tracking _bulletConstraints.Add(constraint); _dynamicsWorld.AddConstraint(constraint); }
public void AddJoint(Physics3DJoint joint, Physics3DRigidBody r1, Physics3DRigidBody r2, Components.JointDesc desc) { var t0 = MatrixExt.Transform(desc.Position, ToQuaternion(desc.Rotation)); Matrix4x4.Invert(t0, out var res); Matrix4x4.Invert(MatrixExt.Transform(r1.defaultPosition, r1.defaultRotation), out var t1); Matrix4x4.Invert(MatrixExt.Transform(r2.defaultPosition, r2.defaultRotation), out var t2); t1 = t0 * t1; t2 = t0 * t2; var j = new Generic6DofSpringConstraint(r1.rigidBody, r2.rigidBody, GetMatrix(t1), GetMatrix(t2), true); joint.constraint = j; j.LinearLowerLimit = GetVector3(desc.PositionMinimum); j.LinearUpperLimit = GetVector3(desc.PositionMaximum); j.AngularLowerLimit = GetVector3(desc.RotationMinimum); j.AngularUpperLimit = GetVector3(desc.RotationMaximum); S(0, desc.PositionSpring.X); S(1, desc.PositionSpring.Y); S(2, desc.PositionSpring.Z); S(3, desc.RotationSpring.X); S(4, desc.RotationSpring.Y); S(5, desc.RotationSpring.Z); void S(int index, float f) { if (f != 0.0f) { j.EnableSpring(index, true); j.SetStiffness(index, f); } else { j.EnableSpring(index, false); } } world.AddConstraint(joint.constraint); }
protected override void OnInitializePhysics() { // collision configuration contains default setup for memory, collision setup CollisionConf = new DefaultCollisionConfiguration(); Dispatcher = new CollisionDispatcher(CollisionConf); Broadphase = new DbvtBroadphase(); World = new DiscreteDynamicsWorld(Dispatcher, Broadphase, null, CollisionConf); World.Gravity = new Vector3(0, -10, 0); GImpactCollisionAlgorithm.RegisterAlgorithm(Dispatcher); string bulletFile; string[] args = Environment.GetCommandLineArgs(); if (args.Length == 1) { bulletFile = "testFile.bullet"; } else { bulletFile = args[1]; } fileLoader = new CustomBulletWorldImporter(World); if (!fileLoader.LoadFile(bulletFile)) { CollisionShape groundShape = new BoxShape(50); CollisionShapes.Add(groundShape); RigidBody ground = LocalCreateRigidBody(0, Matrix.Translation(0, -50, 0), groundShape); ground.UserObject = "Ground"; // create a few dynamic rigidbodies float mass = 1.0f; Vector3[] positions = new Vector3[2] { new Vector3(0.1f, 0.2f, 0.3f), new Vector3(0.4f, 0.5f, 0.6f) }; float[] radi = new float[2] { 0.3f, 0.4f }; CollisionShape colShape = new MultiSphereShape(positions, radi); //CollisionShape colShape = new CapsuleShapeZ(1, 1); //CollisionShape colShape = new CylinderShapeZ(1, 1, 1); //CollisionShape colShape = new BoxShape(1); //CollisionShape colShape = new SphereShape(1); CollisionShapes.Add(colShape); Vector3 localInertia = colShape.CalculateLocalInertia(mass); float start_x = StartPosX - ArraySizeX / 2; float start_y = StartPosY; float start_z = StartPosZ - ArraySizeZ / 2; int k, i, j; for (k = 0; k < ArraySizeY; k++) { for (i = 0; i < ArraySizeX; i++) { for (j = 0; j < ArraySizeZ; j++) { Matrix startTransform = Matrix.Translation( 2 * i + start_x, 2 * k + start_y, 2 * j + start_z ); // using motionstate is recommended, it provides interpolation capabilities // and only synchronizes 'active' objects DefaultMotionState myMotionState = new DefaultMotionState(startTransform); RigidBodyConstructionInfo rbInfo = new RigidBodyConstructionInfo(mass, myMotionState, colShape, localInertia); RigidBody body = new RigidBody(rbInfo); rbInfo.Dispose(); // make it drop from a height body.Translate(new Vector3(0, 20, 0)); World.AddRigidBody(body); } } } DefaultSerializer serializer = new DefaultSerializer(); serializer.RegisterNameForObject(ground, "GroundName"); for (i = 0; i < CollisionShapes.Count; i++) { serializer.RegisterNameForObject(CollisionShapes[i], "name" + i.ToString()); } Point2PointConstraint p2p = new Point2PointConstraint((RigidBody)World.CollisionObjectArray[2], new Vector3(0, 1, 0)); World.AddConstraint(p2p); serializer.RegisterNameForObject(p2p, "constraintje"); World.Serialize(serializer); BulletSharp.DataStream data = serializer.LockBuffer(); byte[] dataBytes = new byte[data.Length]; data.Read(dataBytes, 0, dataBytes.Length); FileStream file = new FileStream("testFile.bullet", FileMode.Create); file.Write(dataBytes, 0, dataBytes.Length); file.Close(); } }
//Point2point public IPoint2PointConstraintImp AddPoint2PointConstraint(IRigidBodyImp rigidBodyA, float3 pivotInA) { var rigidBodyAImp = (RigidBodyImp)rigidBodyA; var btRigidBodyA = rigidBodyAImp._rbi; var btP2PConstraint = new Point2PointConstraint(btRigidBodyA, new Vector3(pivotInA.x, pivotInA.y, pivotInA.z)); BtWorld.AddConstraint(btP2PConstraint); var retval = new Point2PointConstraintImp(); retval._p2pci = btP2PConstraint; btP2PConstraint.UserObject = retval; return(retval); }
public override void AddPoser(Poser poser) { var model = poser.Model; if (_rigidBodies.ContainsKey(poser)) { return; } poser.ResetPosing(); var motionStates = new List <PoserMotionState>(); _motionStates.Add(poser, motionStates); var rigidBodies = new List <RigidBody>(); _rigidBodies.Add(poser, rigidBodies); var constraints = new List <Generic6DofSpringConstraint>(); _constraints.Add(poser, constraints); foreach (var body in model.Rigidbodies) { var bodyDimension = body.Dimemsions; CollisionShape btShape = null; var btMass = 0.0f; var btLocalInertia = new Vector3(0.0f, 0.0f, 0.0f); switch (body.Shape) { case MmdRigidBody.RigidBodyShape.RigidShapeSphere: btShape = new SphereShape(bodyDimension.x); break; case MmdRigidBody.RigidBodyShape.RigidShapeBox: btShape = new BoxShape(new Vector3(bodyDimension.x, bodyDimension.y, bodyDimension.z)); break; case MmdRigidBody.RigidBodyShape.RigidShapeCapsule: btShape = new CapsuleShape(bodyDimension.x, bodyDimension.y); break; default: throw new ArgumentOutOfRangeException(); } if (body.Type != MmdRigidBody.RigidBodyType.RigidTypeKinematic) { btMass = body.Mass; btShape.CalculateLocalInertia(btMass, out btLocalInertia); } var bodyTransform = MathUtil.QuaternionToMatrix4X4(MathUtil.YxzToQuaternion(body.Rotation)); MathUtil.SetTransToMatrix4X4(body.Position, ref bodyTransform); var btBodyTransform = new Matrix(); MathUtil.UnityMatrixToBulletMatrix(bodyTransform, ref btBodyTransform); var btMotionState = new PoserMotionState(poser, body, btBodyTransform); var btInfo = new RigidBodyConstructionInfo(btMass, btMotionState, btShape, btLocalInertia) { LinearDamping = body.TranslateDamp, AngularDamping = body.RotateDamp, Restitution = body.Restitution, Friction = body.Friction }; var btRigidBody = new RigidBody(btInfo) { ActivationState = ActivationState.DisableDeactivation }; if (body.Type == MmdRigidBody.RigidBodyType.RigidTypeKinematic) { btRigidBody.CollisionFlags = btRigidBody.CollisionFlags | CollisionFlags.KinematicObject; } _world.AddRigidBody(btRigidBody, (short)(1 << body.CollisionGroup), (short)body.CollisionMask); #if MMD_PHYSICS_DEBUG CreateUnityCollisionObjectProxy(btRigidBody, body.Name); #endif motionStates.Add(btMotionState); rigidBodies.Add(btRigidBody); } foreach (var constraint in model.Constraints) { var btBody1 = rigidBodies[constraint.AssociatedRigidBodyIndex[0]]; var btBody2 = rigidBodies[constraint.AssociatedRigidBodyIndex[1]]; var positionLowLimit = constraint.PositionLowLimit; var positionHiLimit = constraint.PositionHiLimit; var rotationLoLimit = constraint.RotationLowLimit; var rotationHiLimit = constraint.RotationHiLimit; var constraintTransform = MathUtil.QuaternionToMatrix4X4(MathUtil.YxzToQuaternion(constraint.Rotation)); MathUtil.SetTransToMatrix4X4(constraint.Position, ref constraintTransform); var btConstraintTransform = new Matrix(); MathUtil.UnityMatrixToBulletMatrix(constraintTransform, ref btConstraintTransform); var btLocalizationTransform1 = btConstraintTransform * Matrix.Invert(btBody1.WorldTransform); //TODO 验证这个和mmdlib里算出来的是否一样 var btLocalizationTransform2 = btConstraintTransform * Matrix.Invert(btBody2.WorldTransform); var btConstraint = new Generic6DofSpringConstraint(btBody1, btBody2, btLocalizationTransform1, btLocalizationTransform2, true) { LinearLowerLimit = new Vector3(positionLowLimit.x, positionLowLimit.y, positionLowLimit.z), LinearUpperLimit = new Vector3(positionHiLimit.x, positionHiLimit.y, positionHiLimit.z), AngularLowerLimit = new Vector3(rotationLoLimit.x, rotationLoLimit.y, rotationLoLimit.z), AngularUpperLimit = new Vector3(rotationHiLimit.x, rotationHiLimit.y, rotationHiLimit.z) }; for (var j = 0; j < 3; ++j) { btConstraint.SetStiffness(j, constraint.SpringTranslate[j]); btConstraint.EnableSpring(j, true); btConstraint.SetStiffness(j + 3, constraint.SpringRotate[j]); btConstraint.EnableSpring(j + 3, true); } _world.AddConstraint(btConstraint); constraints.Add(btConstraint); } }
//---------------------------------------------------------------------------------------------------------------- public override void InitializeDemo() { CollisionShape groundShape = new BoxShape(new IndexedVector3(50, 0.1f, 50)); IndexedVector3 wheelDimensions = new IndexedVector3(wheelWidth, wheelRadius, wheelRadius); m_wheelShape = new CylinderShapeX(ref wheelDimensions); m_collisionShapes.Add(groundShape); m_collisionConfiguration = new DefaultCollisionConfiguration(); m_dispatcher = new CollisionDispatcher(m_collisionConfiguration); IndexedVector3 worldMin = new IndexedVector3(-1000, -1000, -1000); IndexedVector3 worldMax = new IndexedVector3(1000, 1000, 1000); m_broadphase = new AxisSweep3Internal(ref worldMin, ref worldMax, 0xfffe, 0xffff, 16384, null, false); m_constraintSolver = new SequentialImpulseConstraintSolver(); m_dynamicsWorld = new DiscreteDynamicsWorld(m_dispatcher, m_broadphase, m_constraintSolver, m_collisionConfiguration); IndexedMatrix tr = IndexedMatrix.CreateTranslation(0, -10, 0); LocalCreateRigidBody(0f, ref tr, groundShape); #if true CollisionShape chassisShape = new BoxShape(new IndexedVector3(1.0f, 0.5f, 2.0f)); //CollisionShape chassisShape = new BoxShape(new IndexedVector3(1.0f, 0.5f, 1.0f)); m_collisionShapes.Add(chassisShape); CompoundShape compound = new CompoundShape(); m_collisionShapes.Add(compound); //localTrans effectively shifts the center of mass with respect to the chassis IndexedMatrix localTrans = IndexedMatrix.CreateTranslation(0, 1, 0); compound.AddChildShape(ref localTrans, chassisShape); { CollisionShape suppShape = new BoxShape(new IndexedVector3(0.5f, 0.1f, 0.5f)); //localTrans effectively shifts the center of mass with respect to the chassis IndexedMatrix suppLocalTrans = IndexedMatrix.CreateTranslation(0f, 1.0f, 2.5f); compound.AddChildShape(ref suppLocalTrans, suppShape); } tr._origin = IndexedVector3.Zero; m_carChassis = LocalCreateRigidBody(800f, ref tr, compound);//chassisShape); #endif { #if true CollisionShape liftShape = new BoxShape(new IndexedVector3(0.5f, 2.0f, 0.05f)); m_collisionShapes.Add(liftShape); m_liftStartPos = new IndexedVector3(0.0f, 2.5f, 3.05f); IndexedMatrix liftTrans = IndexedMatrix.CreateTranslation(m_liftStartPos); m_liftBody = LocalCreateRigidBody(10f, ref liftTrans, liftShape); IndexedMatrix localA = MathUtil.SetEulerZYX(0f, MathUtil.SIMD_HALF_PI, 0f); localA._origin = new IndexedVector3(0f, 1.0f, 3.05f); IndexedMatrix localB = MathUtil.SetEulerZYX(0f, MathUtil.SIMD_HALF_PI, 0f); localB._origin = new IndexedVector3(0f, -1.5f, -0.05f); m_liftHinge = new HingeConstraint(m_carChassis, m_liftBody, ref localA, ref localB); MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "hinge aFrame", m_liftHinge.GetAFrame()); MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "hinge bFrame", m_liftHinge.GetBFrame()); m_liftHinge.SetLimit(0.0f, 0.0f); m_dynamicsWorld.AddConstraint(m_liftHinge, true); CollisionShape forkShapeA = new BoxShape(new IndexedVector3(1.0f, 0.1f, 0.1f)); m_collisionShapes.Add(forkShapeA); CompoundShape forkCompound = new CompoundShape(); m_collisionShapes.Add(forkCompound); IndexedMatrix forkLocalTrans = IndexedMatrix.Identity; forkCompound.AddChildShape(ref forkLocalTrans, forkShapeA); CollisionShape forkShapeB = new BoxShape(new IndexedVector3(0.1f, 0.02f, 0.6f)); m_collisionShapes.Add(forkShapeB); forkLocalTrans = IndexedMatrix.CreateTranslation(-0.9f, -0.08f, 0.7f); forkCompound.AddChildShape(ref forkLocalTrans, forkShapeB); CollisionShape forkShapeC = new BoxShape(new IndexedVector3(0.1f, 0.02f, 0.6f)); m_collisionShapes.Add(forkShapeC); forkLocalTrans = IndexedMatrix.CreateTranslation(0.9f, -0.08f, 0.7f); forkCompound.AddChildShape(ref forkLocalTrans, forkShapeC); m_forkStartPos = new IndexedVector3(0.0f, 0.6f, 3.2f); IndexedMatrix forkTrans = IndexedMatrix.CreateTranslation(m_forkStartPos); m_forkBody = LocalCreateRigidBody(5f, ref forkTrans, forkCompound); localA = IndexedMatrix.Identity; localB = IndexedMatrix.Identity; localA._basis.SetEulerZYX(0, 0, MathUtil.SIMD_HALF_PI); localA._origin = new IndexedVector3(0.0f, -1.9f, 0.05f); localB._basis.SetEulerZYX(0, 0, MathUtil.SIMD_HALF_PI); localB._origin = new IndexedVector3(0.0f, 0.0f, -0.1f); m_forkSlider = new SliderConstraint(m_liftBody, m_forkBody, ref localA, ref localB, true); m_forkSlider.SetLowerLinLimit(0.1f); m_forkSlider.SetUpperLinLimit(0.1f); // m_forkSlider.setLowerAngLimit(-LIFT_EPS); // m_forkSlider.setUpperAngLimit(LIFT_EPS); m_forkSlider.SetLowerAngLimit(0.0f); m_forkSlider.SetUpperAngLimit(0.0f); m_dynamicsWorld.AddConstraint(m_forkSlider, true); #endif #if true CompoundShape loadCompound = new CompoundShape(false); m_collisionShapes.Add(loadCompound); CollisionShape loadShapeA = new BoxShape(new IndexedVector3(2.0f, 0.5f, 0.5f)); m_collisionShapes.Add(loadShapeA); IndexedMatrix loadTrans = IndexedMatrix.Identity; loadCompound.AddChildShape(ref loadTrans, loadShapeA); CollisionShape loadShapeB = new BoxShape(new IndexedVector3(0.1f, 1.0f, 1.0f)); m_collisionShapes.Add(loadShapeB); loadTrans = IndexedMatrix.CreateTranslation(2.1f, 0.0f, 0.0f); loadCompound.AddChildShape(ref loadTrans, loadShapeB); CollisionShape loadShapeC = new BoxShape(new IndexedVector3(0.1f, 1.0f, 1.0f)); m_collisionShapes.Add(loadShapeC); loadTrans = IndexedMatrix.CreateTranslation(-2.1f, 0.0f, 0.0f); loadCompound.AddChildShape(ref loadTrans, loadShapeC); m_loadStartPos = new IndexedVector3(0.0f, -3.5f, 7.0f); loadTrans = IndexedMatrix.CreateTranslation(m_loadStartPos); m_loadBody = LocalCreateRigidBody(4f, ref loadTrans, loadCompound); #endif } //m_carChassis.setDamping(0.2f, 0.2f); ClientResetScene(); /// create vehicle { m_vehicleRayCaster = new DefaultVehicleRaycaster(m_dynamicsWorld); m_vehicle = new RaycastVehicle(m_tuning, m_carChassis, m_vehicleRayCaster); ///never deactivate the vehicle m_carChassis.SetActivationState(ActivationState.DISABLE_DEACTIVATION); m_dynamicsWorld.AddVehicle(m_vehicle); float connectionHeight = 1.2f; bool isFrontWheel = true; //choose coordinate system m_vehicle.SetCoordinateSystem(rightIndex, upIndex, forwardIndex); IndexedVector3 connectionPointCS0 = IndexedVector3.Zero; //connectionPointCS0 = new IndexedVector3(CUBE_HALF_EXTENTS, connectionHeight, CUBE_HALF_EXTENTS); //m_vehicle.addWheel(ref connectionPointCS0, ref wheelDirectionCS0, ref wheelAxleCS, suspensionRestLength, wheelRadius, m_tuning, isFrontWheel); connectionPointCS0 = new IndexedVector3(CUBE_HALF_EXTENTS - (0.3f * wheelWidth), connectionHeight, 2.0f * CUBE_HALF_EXTENTS - wheelRadius); m_vehicle.AddWheel(ref connectionPointCS0, ref wheelDirectionCS0, ref wheelAxleCS, suspensionRestLength, wheelRadius, m_tuning, isFrontWheel); connectionPointCS0 = new IndexedVector3(-CUBE_HALF_EXTENTS + (0.3f * wheelWidth), connectionHeight, 2.0f * CUBE_HALF_EXTENTS - wheelRadius); m_vehicle.AddWheel(ref connectionPointCS0, ref wheelDirectionCS0, ref wheelAxleCS, suspensionRestLength, wheelRadius, m_tuning, isFrontWheel); //isFrontWheel = false; connectionPointCS0 = new IndexedVector3(-CUBE_HALF_EXTENTS + (0.3f * wheelWidth), connectionHeight, -2.0f * CUBE_HALF_EXTENTS + wheelRadius); m_vehicle.AddWheel(ref connectionPointCS0, ref wheelDirectionCS0, ref wheelAxleCS, suspensionRestLength, wheelRadius, m_tuning, isFrontWheel); connectionPointCS0 = new IndexedVector3(CUBE_HALF_EXTENTS - (0.3f * wheelWidth), connectionHeight, -2.0f * CUBE_HALF_EXTENTS + wheelRadius); m_vehicle.AddWheel(ref connectionPointCS0, ref wheelDirectionCS0, ref wheelAxleCS, suspensionRestLength, wheelRadius, m_tuning, isFrontWheel); for (int i = 0; i < m_vehicle.GetNumWheels(); i++) { WheelInfo wheel = m_vehicle.GetWheelInfo(i); wheel.m_suspensionStiffness = suspensionStiffness; wheel.m_wheelsDampingRelaxation = suspensionDamping; wheel.m_wheelsDampingCompression = suspensionCompression; wheel.m_frictionSlip = wheelFriction; wheel.m_rollInfluence = rollInfluence; } } SetCameraDistance(36.0f); SetTexturing(true); SetShadows(true); }
public override void InitializeDemo() { m_collisionConfiguration = new DefaultCollisionConfiguration(); m_dispatcher = new CollisionDispatcher(m_collisionConfiguration); IndexedVector3 worldMin = new IndexedVector3(-1000, -1000, -1000); IndexedVector3 worldMax = new IndexedVector3(1000, 1000, 1000); m_broadphase = new AxisSweep3Internal(ref worldMin, ref worldMax, 0xfffe, 0xffff, 16384, null, false); m_constraintSolver = new SequentialImpulseConstraintSolver(); m_dynamicsWorld = new DiscreteDynamicsWorld(m_dispatcher, m_broadphase, m_constraintSolver, m_collisionConfiguration); m_dynamicsWorld.SetDebugDrawer(m_debugDraw); SetCameraDistance(26f); //CollisionShape groundShape = new BoxShape(new IndexedVector3(50f, 40f, 50f)); CollisionShape groundShape = new StaticPlaneShape(new IndexedVector3(0, 1, 0), 40); m_collisionShapes.Add(groundShape); IndexedMatrix groundTransform = IndexedMatrix.Identity; groundTransform._origin = new IndexedVector3(0, -56, 0); RigidBody groundBody = LocalCreateRigidBody(0, ref groundTransform, groundShape); CollisionShape shape = new BoxShape(new IndexedVector3(CUBE_HALF_EXTENTS, CUBE_HALF_EXTENTS, CUBE_HALF_EXTENTS)); m_collisionShapes.Add(shape); IndexedMatrix trans = IndexedMatrix.Identity; trans._origin = new IndexedVector3(0, 20, 0); float mass = 1f; #if true //point to point constraint (ball socket) //SEEMS OK { RigidBody body0 = LocalCreateRigidBody(mass, ref trans, shape); trans._origin = new IndexedVector3(2 * CUBE_HALF_EXTENTS, 20, 0); mass = 1f; RigidBody body1 = null; //localCreateRigidBody( mass,trans,shape); IndexedVector3 pivotInA = new IndexedVector3(CUBE_HALF_EXTENTS, -CUBE_HALF_EXTENTS, -CUBE_HALF_EXTENTS); IndexedVector3 axisInA = new IndexedVector3(0, 0, 1); IndexedVector3 pivotInB = body1 != null?body1.GetCenterOfMassTransform().Inverse() * (body0.GetCenterOfMassTransform() * (pivotInA)) : pivotInA; IndexedVector3 axisInB = body1 != null ? (body1.GetCenterOfMassTransform()._basis.Inverse() * (body1.GetCenterOfMassTransform()._basis *axisInA)) : body0.GetCenterOfMassTransform()._basis *axisInA; HingeConstraint hinge = new HingeConstraint(body0, ref pivotInA, ref axisInA, false); float targetVelocity = 1f; float maxMotorImpulse = 1.0f; hinge.EnableAngularMotor(true, targetVelocity, maxMotorImpulse); m_dynamicsWorld.AddConstraint(hinge); //p2p); hinge.SetDbgDrawSize(5f); } #endif #if true //create a slider, using the generic D6 constraint // SEEMS OK { mass = 1f; IndexedVector3 sliderWorldPos = new IndexedVector3(0, 10, 0); IndexedVector3 sliderAxis = new IndexedVector3(1, 0, 0); float angle = 0f;//SIMD_RADS_PER_DEG * 10.f; IndexedBasisMatrix sliderOrientation = new IndexedBasisMatrix(Quaternion.CreateFromAxisAngle(sliderAxis.ToVector3(), angle)); trans = IndexedMatrix.Identity; trans._origin = sliderWorldPos; //trans.setBasis(sliderOrientation); sliderTransform = trans; d6body0 = LocalCreateRigidBody(mass, ref trans, shape); d6body0.SetActivationState(ActivationState.DISABLE_DEACTIVATION); RigidBody fixedBody1 = LocalCreateRigidBody(0, ref trans, null); m_dynamicsWorld.AddRigidBody(fixedBody1); IndexedMatrix frameInA, frameInB; frameInA = IndexedMatrix.Identity; frameInB = IndexedMatrix.Identity; frameInA._origin = new IndexedVector3(0, 5, 0); frameInB._origin = new IndexedVector3(0, 5, 0); // bool useLinearReferenceFrameA = false;//use fixed frame B for linear llimits bool useLinearReferenceFrameA = true; //use fixed frame A for linear llimits spSlider6Dof = new Generic6DofConstraint(fixedBody1, d6body0, ref frameInA, ref frameInB, useLinearReferenceFrameA); spSlider6Dof.SetLinearLowerLimit(ref lowerSliderLimit); spSlider6Dof.SetLinearUpperLimit(ref hiSliderLimit); //range should be small, otherwise singularities will 'explode' the constraint IndexedVector3 angularLower = new IndexedVector3(-1.5f, 0, 0); IndexedVector3 angularUpper = -angularLower; spSlider6Dof.SetAngularLowerLimit(ref angularLower); spSlider6Dof.SetAngularUpperLimit(ref angularUpper); // slider.setAngularLowerLimit(IndexedVector3(0,0,0)); // slider.setAngularUpperLimit(IndexedVector3(0,0,0)); spSlider6Dof.GetTranslationalLimitMotor().m_enableMotor[0] = true; spSlider6Dof.GetTranslationalLimitMotor().m_targetVelocity.X = -5.0f; spSlider6Dof.GetTranslationalLimitMotor().m_maxMotorForce.X = 0.1f; m_dynamicsWorld.AddConstraint(spSlider6Dof); spSlider6Dof.SetDbgDrawSize(5f); } #endif #if true { // create a door using hinge constraint attached to the world CollisionShape pDoorShape = new BoxShape(new IndexedVector3(2.0f, 5.0f, 0.2f)); m_collisionShapes.Add(pDoorShape); IndexedMatrix doorTrans = IndexedMatrix.Identity; doorTrans._origin = new IndexedVector3(-5.0f, -2.0f, 0.0f); RigidBody pDoorBody = LocalCreateRigidBody(1.0f, ref doorTrans, pDoorShape); pDoorBody.SetActivationState(ActivationState.DISABLE_DEACTIVATION); IndexedVector3 btPivotA = new IndexedVector3(10f + 2.1f, -2.0f, 0.0f); // right next to the door slightly outside IndexedVector3 btAxisA = new IndexedVector3(0.0f, 1.0f, 0.0f); // pointing upwards, aka Y-axis spDoorHinge = new HingeConstraint(pDoorBody, ref btPivotA, ref btAxisA, false); spDoorHinge.SetLimit(-MathUtil.SIMD_PI * 0.25f, MathUtil.SIMD_PI * 0.25f); m_dynamicsWorld.AddConstraint(spDoorHinge); spDoorHinge.SetDbgDrawSize(5.0f); } #endif #if true { // create a generic 6DOF constraint // SEEMS OK - But debug draw a bit wrong? IndexedMatrix tr = IndexedMatrix.Identity; tr._origin = new IndexedVector3(10f, 6f, 0f); //tr.getBasis().setEulerZYX(0,0,0); // RigidBody pBodyA = localCreateRigidBody( mass, tr, shape); RigidBody pBodyA = LocalCreateRigidBody(0.0f, ref tr, shape); // RigidBody pBodyA = localCreateRigidBody( 0.0, tr, 0); pBodyA.SetActivationState(ActivationState.DISABLE_DEACTIVATION); tr = IndexedMatrix.Identity; tr._origin = new IndexedVector3(0f, 6f, 0f); //tr.getBasis().setEulerZYX(0,0,0); RigidBody pBodyB = LocalCreateRigidBody(mass, ref tr, shape); pBodyB.SetActivationState(ActivationState.DISABLE_DEACTIVATION); IndexedMatrix frameInA, frameInB; frameInA = IndexedMatrix.CreateTranslation(-5, 0, 0); frameInB = IndexedMatrix.CreateTranslation(5, 0, 0); Generic6DofConstraint pGen6DOF = new Generic6DofConstraint(pBodyA, pBodyB, ref frameInA, ref frameInB, true); // btGeneric6DofConstraint* pGen6DOF = new btGeneric6DofConstraint(*pBodyA, *pBodyB, frameInA, frameInB, false); IndexedVector3 linearLower = new IndexedVector3(-10, -2, -1); pGen6DOF.SetLinearLowerLimit(ref linearLower); IndexedVector3 linearUpper = new IndexedVector3(10, 2, 1); pGen6DOF.SetLinearUpperLimit(ref linearUpper); // ? why again? //linearLower = new IndexedVector3(-10,0,0); //pGen6DOF.setLinearLowerLimit(ref linearLower); // pGen6DOF.setLinearUpperLimit(IndexedVector3(10., 0., 0.)); // pGen6DOF.setLinearLowerLimit(IndexedVector3(0., 0., 0.)); // pGen6DOF.setLinearUpperLimit(IndexedVector3(0., 0., 0.)); // pGen6DOF.getTranslationalLimitMotor().m_enableMotor[0] = true; // pGen6DOF.getTranslationalLimitMotor().m_targetVelocity[0] = 5.0f; // pGen6DOF.getTranslationalLimitMotor().m_maxMotorForce[0] = 0.1f; // pGen6DOF.setAngularLowerLimit(IndexedVector3(0., SIMD_HALF_PI*0.9, 0.)); // pGen6DOF.setAngularUpperLimit(IndexedVector3(0., -SIMD_HALF_PI*0.9, 0.)); // pGen6DOF.setAngularLowerLimit(IndexedVector3(0., 0., -SIMD_HALF_PI)); // pGen6DOF.setAngularUpperLimit(IndexedVector3(0., 0., SIMD_HALF_PI)); IndexedVector3 angularLower = new IndexedVector3(-MathUtil.SIMD_HALF_PI * 0.5f, -0.75f, -MathUtil.SIMD_HALF_PI * 0.8f); IndexedVector3 angularUpper = -angularLower; pGen6DOF.SetAngularLowerLimit(ref angularLower); pGen6DOF.SetAngularUpperLimit(ref angularUpper); // pGen6DOF.setAngularLowerLimit(IndexedVector3(0.f, -0.75, SIMD_HALF_PI * 0.8f)); // pGen6DOF.setAngularUpperLimit(IndexedVector3(0.f, 0.75, -SIMD_HALF_PI * 0.8f)); // pGen6DOF.setAngularLowerLimit(IndexedVector3(0.f, -SIMD_HALF_PI * 0.8f, SIMD_HALF_PI * 1.98f)); // pGen6DOF.setAngularUpperLimit(IndexedVector3(0.f, SIMD_HALF_PI * 0.8f, -SIMD_HALF_PI * 1.98f)); // pGen6DOF.setAngularLowerLimit(IndexedVector3(-0.75,-0.5, -0.5)); // pGen6DOF.setAngularUpperLimit(IndexedVector3(0.75,0.5, 0.5)); // pGen6DOF.setAngularLowerLimit(IndexedVector3(-0.75,0., 0.)); // pGen6DOF.setAngularUpperLimit(IndexedVector3(0.75,0., 0.)); m_dynamicsWorld.AddConstraint(pGen6DOF, true); pGen6DOF.SetDbgDrawSize(5.0f); } #endif #if true { // create a ConeTwist constraint IndexedMatrix tr = IndexedMatrix.CreateTranslation(-10, 5, 0); RigidBody pBodyA = LocalCreateRigidBody(1.0f, ref tr, shape); pBodyA.SetActivationState(ActivationState.DISABLE_DEACTIVATION); tr = IndexedMatrix.CreateTranslation(-10, -5, 0); RigidBody pBodyB = LocalCreateRigidBody(0.0f, ref tr, shape); IndexedMatrix frameInA, frameInB; frameInA = MathUtil.SetEulerZYX(0, 0, MathUtil.SIMD_HALF_PI); frameInA._origin = new IndexedVector3(0, -5, 0); frameInB = MathUtil.SetEulerZYX(0, 0, MathUtil.SIMD_HALF_PI); frameInB._origin = new IndexedVector3(0, 5, 0); ConeTwistConstraint pCT = new ConeTwistConstraint(pBodyA, pBodyB, ref frameInA, ref frameInB); pCT.SetLimit(MathUtil.SIMD_QUARTER_PI, MathUtil.SIMD_QUARTER_PI, MathUtil.SIMD_PI * 0.8f, 1.0f, 0.3f, 1.0f); // soft limit == hard limit m_dynamicsWorld.AddConstraint(pCT, true); pCT.SetDbgDrawSize(5.0f); } #endif #if true { // Hinge connected to the world, with motor (to hinge motor with new and old constraint solver) // WORKS OK IndexedMatrix tr = IndexedMatrix.Identity; RigidBody pBody = LocalCreateRigidBody(1.0f, ref tr, shape); pBody.SetActivationState(ActivationState.DISABLE_DEACTIVATION); IndexedVector3 btPivotA = new IndexedVector3(10.0f, 0.0f, 0.0f); IndexedVector3 btAxisA = new IndexedVector3(0.0f, 0.0f, 1.0f); HingeConstraint pHinge = new HingeConstraint(pBody, ref btPivotA, ref btAxisA, false); // pHinge.enableAngularMotor(true, -1.0, 0.165); // use for the old solver pHinge.EnableAngularMotor(true, -1.0f, 1.65f); // use for the new SIMD solver m_dynamicsWorld.AddConstraint(pHinge); pHinge.SetDbgDrawSize(5.0f); } #endif #if true { // WORKS OK // create a universal joint using generic 6DOF constraint // create two rigid bodies // static bodyA (parent) on top: IndexedMatrix tr = IndexedMatrix.CreateTranslation(20, 4, 0); RigidBody pBodyA = LocalCreateRigidBody(0.0f, ref tr, shape); pBodyA.SetActivationState(ActivationState.DISABLE_DEACTIVATION); // dynamic bodyB (child) below it : tr = IndexedMatrix.CreateTranslation(20, 0, 0); RigidBody pBodyB = LocalCreateRigidBody(1.0f, ref tr, shape); pBodyB.SetActivationState(ActivationState.DISABLE_DEACTIVATION); // add some (arbitrary) data to build constraint frames IndexedVector3 parentAxis = new IndexedVector3(1.0f, 0.0f, 0.0f); IndexedVector3 childAxis = new IndexedVector3(0.0f, 0.0f, 1.0f); IndexedVector3 anchor = new IndexedVector3(20.0f, 2.0f, 0.0f); UniversalConstraint pUniv = new UniversalConstraint(pBodyA, pBodyB, ref anchor, ref parentAxis, ref childAxis); pUniv.SetLowerLimit(-MathUtil.SIMD_HALF_PI * 0.5f, -MathUtil.SIMD_HALF_PI * 0.5f); pUniv.SetUpperLimit(MathUtil.SIMD_HALF_PI * 0.5f, MathUtil.SIMD_HALF_PI * 0.5f); // add constraint to world m_dynamicsWorld.AddConstraint(pUniv, true); // draw constraint frames and limits for debugging pUniv.SetDbgDrawSize(5.0f); } #endif #if true // WORKS OK { // create a generic 6DOF constraint with springs IndexedMatrix tr = IndexedMatrix.CreateTranslation(-20f, 16f, 0f); //tr.setIdentity(); //tr.setOrigin(btVector3(btScalar(-20.), btScalar(16.), btScalar(0.))); //tr.getBasis().setEulerZYX(0,0,0); RigidBody pBodyA = LocalCreateRigidBody(0.0f, ref tr, shape); pBodyA.SetActivationState(ActivationState.DISABLE_DEACTIVATION); //tr.setIdentity(); //tr.setOrigin(btVector3(btScalar(-10.), btScalar(16.), btScalar(0.))); //tr.getBasis().setEulerZYX(0,0,0); tr = IndexedMatrix.CreateTranslation(-10, 16, 0); RigidBody pBodyB = LocalCreateRigidBody(1.0f, ref tr, shape); pBodyB.SetActivationState(ActivationState.DISABLE_DEACTIVATION); IndexedMatrix frameInA = IndexedMatrix.CreateTranslation(10f, 0f, 0f); IndexedMatrix frameInB = IndexedMatrix.CreateTranslation(0f, 0f, 0f); Generic6DofSpringConstraint pGen6DOFSpring = new Generic6DofSpringConstraint(pBodyA, pBodyB, ref frameInA, ref frameInB, true); pGen6DOFSpring.SetLinearUpperLimit(new IndexedVector3(5f, 0f, 0f)); pGen6DOFSpring.SetLinearLowerLimit(new IndexedVector3(-5f, 0f, 0f)); pGen6DOFSpring.SetAngularLowerLimit(new IndexedVector3(0f, 0f, -1.5f)); pGen6DOFSpring.SetAngularUpperLimit(new IndexedVector3(0f, 0f, 1.5f)); m_dynamicsWorld.AddConstraint(pGen6DOFSpring, true); pGen6DOFSpring.SetDbgDrawSize(5.0f); pGen6DOFSpring.EnableSpring(0, true); pGen6DOFSpring.SetStiffness(0, 39.478f); pGen6DOFSpring.SetDamping(0, 0.5f); pGen6DOFSpring.EnableSpring(5, true); pGen6DOFSpring.SetStiffness(5, 39.478f); pGen6DOFSpring.SetDamping(0, 0.3f); pGen6DOFSpring.SetEquilibriumPoint(); } #endif #if true { // WORKS OK // create a Hinge2 joint // create two rigid bodies // static bodyA (parent) on top: IndexedMatrix tr = IndexedMatrix.CreateTranslation(-20f, 4f, 0f); RigidBody pBodyA = LocalCreateRigidBody(0.0f, ref tr, shape); pBodyA.SetActivationState(ActivationState.DISABLE_DEACTIVATION); // dynamic bodyB (child) below it : tr = IndexedMatrix.CreateTranslation(-20f, 0f, 0f); RigidBody pBodyB = LocalCreateRigidBody(1.0f, ref tr, shape); pBodyB.SetActivationState(ActivationState.DISABLE_DEACTIVATION); // add some data to build constraint frames IndexedVector3 parentAxis = new IndexedVector3(0.0f, 1.0f, 0.0f); IndexedVector3 childAxis = new IndexedVector3(1.0f, 0.0f, 0.0f); IndexedVector3 anchor = new IndexedVector3(-20.0f, 0.0f, 0.0f); Hinge2Constraint pHinge2 = new Hinge2Constraint(pBodyA, pBodyB, ref anchor, ref parentAxis, ref childAxis); pHinge2.SetLowerLimit(-MathUtil.SIMD_HALF_PI * 0.5f); pHinge2.SetUpperLimit(MathUtil.SIMD_HALF_PI * 0.5f); // add constraint to world m_dynamicsWorld.AddConstraint(pHinge2, true); // draw constraint frames and limits for debugging pHinge2.SetDbgDrawSize(5.0f); } #endif #if true { // WORKS OK // create a Hinge joint between two dynamic bodies // create two rigid bodies // static bodyA (parent) on top: IndexedMatrix tr = IndexedMatrix.CreateTranslation(-20f, -2f, 0f); RigidBody pBodyA = LocalCreateRigidBody(1.0f, ref tr, shape); pBodyA.SetActivationState(ActivationState.DISABLE_DEACTIVATION); // dynamic bodyB: tr = IndexedMatrix.CreateTranslation(-30f, -2f, 0f); RigidBody pBodyB = LocalCreateRigidBody(10.0f, ref tr, shape); pBodyB.SetActivationState(ActivationState.DISABLE_DEACTIVATION); // add some data to build constraint frames IndexedVector3 axisA = new IndexedVector3(0.0f, 1.0f, 0.0f); IndexedVector3 axisB = new IndexedVector3(0.0f, 1.0f, 0.0f); IndexedVector3 pivotA = new IndexedVector3(-5.0f, 0.0f, 0.0f); IndexedVector3 pivotB = new IndexedVector3(5.0f, 0.0f, 0.0f); spHingeDynAB = new HingeConstraint(pBodyA, pBodyB, ref pivotA, ref pivotB, ref axisA, ref axisB); spHingeDynAB.SetLimit(-MathUtil.SIMD_HALF_PI * 0.5f, MathUtil.SIMD_HALF_PI * 0.5f); // add constraint to world m_dynamicsWorld.AddConstraint(spHingeDynAB, true); // draw constraint frames and limits for debugging spHingeDynAB.SetDbgDrawSize(5.0f); } #endif }
public SerializeDemoSimulation() { CollisionConfiguration = new DefaultCollisionConfiguration(); Dispatcher = new CollisionDispatcher(CollisionConfiguration); Broadphase = new DbvtBroadphase(); World = new DiscreteDynamicsWorld(Dispatcher, Broadphase, null, CollisionConfiguration); GImpactCollisionAlgorithm.RegisterAlgorithm(Dispatcher); string bulletFile; string[] args = Environment.GetCommandLineArgs(); if (args.Length == 1) { bulletFile = "testFile.bullet"; } else { bulletFile = args[1]; } _fileLoader = new CustomBulletWorldImporter(World); if (!_fileLoader.LoadFile(bulletFile)) { var groundShape = new BoxShape(50); _collisionShapes.Add(groundShape); RigidBody ground = PhysicsHelper.CreateStaticBody(Matrix.Translation(0, -50, 0), groundShape, World); ground.UserObject = "Ground"; // create a few dynamic rigidbodies float mass = 1.0f; Vector3[] positions = new[] { new Vector3(0.1f, 0.2f, 0.3f), new Vector3(0.4f, 0.5f, 0.6f) }; float[] radi = new float[2] { 0.3f, 0.4f }; var colShape = new MultiSphereShape(positions, radi); //var colShape = new CapsuleShapeZ(1, 1); //var colShape = new CylinderShapeZ(1, 1, 1); //var colShape = new BoxShape(1); //var colShape = new SphereShape(1); _collisionShapes.Add(colShape); Vector3 localInertia = colShape.CalculateLocalInertia(mass); float startX = StartPosX - NumObjectsX / 2; float startY = StartPosY; float startZ = StartPosZ - NumObjectsZ / 2; for (int y = 0; y < NumObjectsY; y++) { for (int x = 0; x < NumObjectsX; x++) { for (int z = 0; z < NumObjectsZ; z++) { Matrix startTransform = Matrix.Translation( 2 * x + startX, 2 * y + startY, 2 * z + startZ ); // using motionstate is recommended, it provides interpolation capabilities // and only synchronizes 'active' objects DefaultMotionState myMotionState = new DefaultMotionState(startTransform); RigidBodyConstructionInfo rbInfo = new RigidBodyConstructionInfo(mass, myMotionState, colShape, localInertia); RigidBody body = new RigidBody(rbInfo); rbInfo.Dispose(); // make it drop from a height body.Translate(new Vector3(0, 20, 0)); World.AddRigidBody(body); } } } using (var serializer = new DefaultSerializer()) { serializer.RegisterNameForObject(ground, "GroundName"); for (int i = 0; i < _collisionShapes.Count; i++) { serializer.RegisterNameForObject(_collisionShapes[i], "name" + i.ToString()); } var p2p = new Point2PointConstraint((RigidBody)World.CollisionObjectArray[2], new Vector3(0, 1, 0)); World.AddConstraint(p2p); serializer.RegisterNameForObject(p2p, "constraintje"); World.Serialize(serializer); byte[] dataBytes = new byte[serializer.CurrentBufferSize]; Marshal.Copy(serializer.BufferPointer, dataBytes, 0, dataBytes.Length); using (var file = new FileStream("testFile.bullet", FileMode.Create)) { file.Write(dataBytes, 0, dataBytes.Length); } } } }
public override void InitializeDemo() { //string filename = @"E:\users\man\bullet\xna-constraint-output.txt"; //FileStream filestream = File.Open(filename, FileMode.Create, FileAccess.Write, FileShare.Read); //BulletGlobals.g_streamWriter = new StreamWriter(filestream); //maxiterations = 100; m_collisionConfiguration = new DefaultCollisionConfiguration(); m_dispatcher = new CollisionDispatcher(m_collisionConfiguration); IndexedVector3 worldMin = new IndexedVector3(-1000,-1000,-1000); IndexedVector3 worldMax = new IndexedVector3(1000,1000,1000); m_broadphase = new AxisSweep3Internal(ref worldMin, ref worldMax, 0xfffe, 0xffff, 16384, null, false); m_constraintSolver = new SequentialImpulseConstraintSolver(); m_dynamicsWorld = new DiscreteDynamicsWorld(m_dispatcher,m_broadphase,m_constraintSolver,m_collisionConfiguration); m_dynamicsWorld.SetDebugDrawer(m_debugDraw); SetCameraDistance(26f); //CollisionShape groundShape = new BoxShape(new IndexedVector3(50f, 40f, 50f)); CollisionShape groundShape = new StaticPlaneShape(new IndexedVector3(0, 1, 0), 40); m_collisionShapes.Add(groundShape); IndexedMatrix groundTransform = IndexedMatrix.Identity; groundTransform._origin = new IndexedVector3(0, -56, 0); RigidBody groundBody = LocalCreateRigidBody(0, ref groundTransform, groundShape); CollisionShape shape = new BoxShape(new IndexedVector3(CUBE_HALF_EXTENTS, CUBE_HALF_EXTENTS, CUBE_HALF_EXTENTS)); m_collisionShapes.Add(shape); IndexedMatrix trans = IndexedMatrix.Identity; trans._origin = new IndexedVector3(0, 20, 0); float mass = 1f; #if true //point to point constraint with a breaking threshold { trans = IndexedMatrix.Identity; trans._origin = new IndexedVector3(1,30,-5); LocalCreateRigidBody( mass,trans,shape); trans._origin = new IndexedVector3(0,0,-5); RigidBody body0 = LocalCreateRigidBody( mass,trans,shape); trans._origin = new IndexedVector3(2*CUBE_HALF_EXTENTS,20,0); mass = 1.0f; RigidBody body1 = null;//localCreateRigidBody( mass,trans,shape); IndexedVector3 pivotInA = new IndexedVector3(CUBE_HALF_EXTENTS,CUBE_HALF_EXTENTS,0); TypedConstraint p2p = new Point2PointConstraint(body0,ref pivotInA); m_dynamicsWorld.AddConstraint(p2p); p2p.SetBreakingImpulseThreshold(10.2f); p2p.SetDbgDrawSize(5.0f); } #endif #if true //point to point constraint (ball socket) //SEEMS OK { //trans = IndexedMatrix.Identity; RigidBody body0 = LocalCreateRigidBody( mass,ref trans,shape); trans._origin = new IndexedVector3(2*CUBE_HALF_EXTENTS,20,0); mass = 1f; RigidBody body1 = null;//localCreateRigidBody( mass,trans,shape); IndexedVector3 pivotInA = new IndexedVector3(CUBE_HALF_EXTENTS,-CUBE_HALF_EXTENTS,-CUBE_HALF_EXTENTS); IndexedVector3 axisInA = new IndexedVector3(0,0,1); IndexedVector3 pivotInB = body1 != null ? body1.GetCenterOfMassTransform().Inverse() * (body0.GetCenterOfMassTransform() * (pivotInA)) : pivotInA; IndexedVector3 axisInB = body1 != null ? (body1.GetCenterOfMassTransform()._basis.Inverse() * (body1.GetCenterOfMassTransform()._basis * axisInA)) : body0.GetCenterOfMassTransform()._basis * axisInA; #if P2P TypedConstraint p2p = new Point2PointConstraint(body0,ref pivotInA); //btTypedConstraint* p2p = new btPoint2PointConstraint(*body0,*body1,pivotInA,pivotInB); //btTypedConstraint* hinge = new btHingeConstraint(*body0,*body1,pivotInA,pivotInB,axisInA,axisInB); m_dynamicsWorld.AddConstraint(p2p); p2p.SetDbgDrawSize(5.0f); #else HingeConstraint hinge = new HingeConstraint(body0,ref pivotInA,ref axisInA,false); float targetVelocity = 1f; float maxMotorImpulse = 1.0f; hinge.EnableAngularMotor(true,targetVelocity,maxMotorImpulse); m_dynamicsWorld.AddConstraint(hinge);//p2p); hinge.SetDbgDrawSize(5f); #endif } #endif #if true //create a slider, using the generic D6 constraint // SEEMS OK { mass = 1f; IndexedVector3 sliderWorldPos = new IndexedVector3(0,10,0); IndexedVector3 sliderAxis = new IndexedVector3(1,0,0); float angle=0f;//SIMD_RADS_PER_DEG * 10.f; IndexedBasisMatrix sliderOrientation = new IndexedBasisMatrix(new IndexedQuaternion(sliderAxis,angle)); trans = IndexedMatrix.Identity; trans._origin = sliderWorldPos; //trans.setBasis(sliderOrientation); sliderTransform = trans; d6body0 = LocalCreateRigidBody( mass,ref trans,shape); d6body0.SetActivationState(ActivationState.DISABLE_DEACTIVATION); RigidBody fixedBody1 = LocalCreateRigidBody(0,ref trans,null); m_dynamicsWorld.AddRigidBody(fixedBody1); IndexedMatrix frameInA, frameInB; frameInA = IndexedMatrix.Identity; frameInB = IndexedMatrix.Identity; frameInA._origin = new IndexedVector3(0, 5, 0); frameInB._origin = new IndexedVector3(0, 5, 0); // bool useLinearReferenceFrameA = false;//use fixed frame B for linear llimits bool useLinearReferenceFrameA = true;//use fixed frame A for linear llimits spSlider6Dof = new Generic6DofConstraint(fixedBody1, d6body0,ref frameInA,ref frameInB,useLinearReferenceFrameA); spSlider6Dof.SetLinearLowerLimit(ref lowerSliderLimit); spSlider6Dof.SetLinearUpperLimit(ref hiSliderLimit); //range should be small, otherwise singularities will 'explode' the constraint IndexedVector3 angularLower = new IndexedVector3(-1.5f,0,0); IndexedVector3 angularUpper = -angularLower; spSlider6Dof.SetAngularLowerLimit(ref angularLower); spSlider6Dof.SetAngularUpperLimit(ref angularUpper); // slider.setAngularLowerLimit(IndexedVector3(0,0,0)); // slider.setAngularUpperLimit(IndexedVector3(0,0,0)); spSlider6Dof.SetAngularLowerLimit(new IndexedVector3(-MathUtil.SIMD_PI, 0, 0)); spSlider6Dof.SetAngularUpperLimit(new IndexedVector3(1.5f, 0, 0)); spSlider6Dof.GetTranslationalLimitMotor().m_enableMotor[0] = true; spSlider6Dof.GetTranslationalLimitMotor().m_targetVelocity.X = -5.0f; spSlider6Dof.GetTranslationalLimitMotor().m_maxMotorForce.X = 0.1f; m_dynamicsWorld.AddConstraint(spSlider6Dof); spSlider6Dof.SetDbgDrawSize(5f); } #endif #if true { // create a door using hinge constraint attached to the world CollisionShape pDoorShape = new BoxShape(new IndexedVector3(2.0f, 5.0f, 0.2f)); m_collisionShapes.Add(pDoorShape); IndexedMatrix doorTrans = IndexedMatrix.Identity; doorTrans._origin = new IndexedVector3(-5.0f, -2.0f, 0.0f); RigidBody pDoorBody = LocalCreateRigidBody( 1.0f, ref doorTrans, pDoorShape); pDoorBody.SetActivationState(ActivationState.DISABLE_DEACTIVATION); IndexedVector3 btPivotA = new IndexedVector3( 10f+2.1f, -2.0f, 0.0f ); // right next to the door slightly outside IndexedVector3 btAxisA = new IndexedVector3( 0.0f, 1.0f, 0.0f ); // pointing upwards, aka Y-axis spDoorHinge = new HingeConstraint( pDoorBody, ref btPivotA, ref btAxisA,false ); spDoorHinge.SetLimit(-MathUtil.SIMD_PI * 0.25f, MathUtil.SIMD_PI * 0.25f); m_dynamicsWorld.AddConstraint(spDoorHinge); spDoorHinge.SetDbgDrawSize(5.0f); } #endif #if true { // create a generic 6DOF constraint // SEEMS OK - But debug draw a bit wrong? IndexedMatrix tr = IndexedMatrix.Identity; tr._origin = new IndexedVector3(10f, 6f, 0f); //tr.getBasis().setEulerZYX(0,0,0); // RigidBody pBodyA = localCreateRigidBody( mass, tr, shape); RigidBody pBodyA = LocalCreateRigidBody( 0.0f, ref tr, shape); // RigidBody pBodyA = localCreateRigidBody( 0.0, tr, 0); pBodyA.SetActivationState(ActivationState.DISABLE_DEACTIVATION); tr = IndexedMatrix.Identity; tr._origin = new IndexedVector3(0f, 6f, 0f); //tr.getBasis().setEulerZYX(0,0,0); RigidBody pBodyB = LocalCreateRigidBody(mass, ref tr, shape); pBodyB.SetActivationState(ActivationState.DISABLE_DEACTIVATION); IndexedMatrix frameInA, frameInB; frameInA = IndexedMatrix.CreateTranslation(-5,0,0); frameInB = IndexedMatrix.CreateTranslation(5,0,0); Generic6DofConstraint pGen6DOF = new Generic6DofConstraint(pBodyA, pBodyB, ref frameInA, ref frameInB, true); // btGeneric6DofConstraint* pGen6DOF = new btGeneric6DofConstraint(*pBodyA, *pBodyB, frameInA, frameInB, false); IndexedVector3 linearLower = new IndexedVector3(-10, -2, -1); pGen6DOF.SetLinearLowerLimit(ref linearLower); IndexedVector3 linearUpper = new IndexedVector3(10,2,1); pGen6DOF.SetLinearUpperLimit(ref linearUpper); // ? why again? //linearLower = new IndexedVector3(-10,0,0); //pGen6DOF.setLinearLowerLimit(ref linearLower); // pGen6DOF.setLinearUpperLimit(IndexedVector3(10., 0., 0.)); // pGen6DOF.setLinearLowerLimit(IndexedVector3(0., 0., 0.)); // pGen6DOF.setLinearUpperLimit(IndexedVector3(0., 0., 0.)); // pGen6DOF.getTranslationalLimitMotor().m_enableMotor[0] = true; // pGen6DOF.getTranslationalLimitMotor().m_targetVelocity[0] = 5.0f; // pGen6DOF.getTranslationalLimitMotor().m_maxMotorForce[0] = 0.1f; // pGen6DOF.setAngularLowerLimit(IndexedVector3(0., SIMD_HALF_PI*0.9, 0.)); // pGen6DOF.setAngularUpperLimit(IndexedVector3(0., -SIMD_HALF_PI*0.9, 0.)); // pGen6DOF.setAngularLowerLimit(IndexedVector3(0., 0., -SIMD_HALF_PI)); // pGen6DOF.setAngularUpperLimit(IndexedVector3(0., 0., SIMD_HALF_PI)); IndexedVector3 angularLower = new IndexedVector3(-MathUtil.SIMD_HALF_PI * 0.5f, -0.75f, -MathUtil.SIMD_HALF_PI * 0.8f); IndexedVector3 angularUpper = -angularLower; pGen6DOF.SetAngularLowerLimit(ref angularLower); pGen6DOF.SetAngularUpperLimit(ref angularUpper); // pGen6DOF.setAngularLowerLimit(IndexedVector3(0.f, -0.75, SIMD_HALF_PI * 0.8f)); // pGen6DOF.setAngularUpperLimit(IndexedVector3(0.f, 0.75, -SIMD_HALF_PI * 0.8f)); // pGen6DOF.setAngularLowerLimit(IndexedVector3(0.f, -SIMD_HALF_PI * 0.8f, SIMD_HALF_PI * 1.98f)); // pGen6DOF.setAngularUpperLimit(IndexedVector3(0.f, SIMD_HALF_PI * 0.8f, -SIMD_HALF_PI * 1.98f)); // pGen6DOF.setAngularLowerLimit(IndexedVector3(-0.75,-0.5, -0.5)); // pGen6DOF.setAngularUpperLimit(IndexedVector3(0.75,0.5, 0.5)); // pGen6DOF.setAngularLowerLimit(IndexedVector3(-0.75,0., 0.)); // pGen6DOF.setAngularUpperLimit(IndexedVector3(0.75,0., 0.)); m_dynamicsWorld.AddConstraint(pGen6DOF, true); pGen6DOF.SetDbgDrawSize(5.0f); } #endif #if true { // create a ConeTwist constraint IndexedMatrix tr = IndexedMatrix.CreateTranslation(-10,5,0); RigidBody pBodyA = LocalCreateRigidBody( 1.0f, ref tr, shape); pBodyA.SetActivationState(ActivationState.DISABLE_DEACTIVATION); tr = IndexedMatrix.CreateTranslation(-10,-5,0); RigidBody pBodyB = LocalCreateRigidBody(0.0f, ref tr, shape); IndexedMatrix frameInA, frameInB; frameInA = MathUtil.SetEulerZYX(0, 0, MathUtil.SIMD_HALF_PI); frameInA._origin = new IndexedVector3(0, -5, 0); frameInB = MathUtil.SetEulerZYX(0, 0, MathUtil.SIMD_HALF_PI); frameInB._origin = new IndexedVector3(0, 5, 0); ConeTwistConstraint pCT = new ConeTwistConstraint(pBodyA, pBodyB, ref frameInA, ref frameInB); pCT.SetLimit(MathUtil.SIMD_QUARTER_PI * 0.6f, MathUtil.SIMD_QUARTER_PI, MathUtil.SIMD_PI * 0.8f, 0.5f); // soft limit == hard limit m_dynamicsWorld.AddConstraint(pCT, true); pCT.SetDbgDrawSize(5.0f); } #endif #if true { // Hinge connected to the world, with motor (to hinge motor with new and old constraint solver) // WORKS OK IndexedMatrix tr = IndexedMatrix.Identity; RigidBody pBody = LocalCreateRigidBody( 1.0f, ref tr, shape); pBody.SetActivationState(ActivationState.DISABLE_DEACTIVATION); IndexedVector3 btPivotA = new IndexedVector3( 10.0f, 0.0f, 0.0f ); IndexedVector3 btAxisA = new IndexedVector3( 0.0f, 0.0f, 1.0f ); HingeConstraint pHinge = new HingeConstraint(pBody, ref btPivotA, ref btAxisA,false); // pHinge.enableAngularMotor(true, -1.0, 0.165); // use for the old solver pHinge.EnableAngularMotor(true, -1.0f, 1.65f); // use for the new SIMD solver m_dynamicsWorld.AddConstraint(pHinge); pHinge.SetDbgDrawSize(5.0f); } #endif #if true { // WORKS OK // create a universal joint using generic 6DOF constraint // create two rigid bodies // static bodyA (parent) on top: IndexedMatrix tr = IndexedMatrix.CreateTranslation(20,4,0); RigidBody pBodyA = LocalCreateRigidBody( 0.0f, ref tr, shape); pBodyA.SetActivationState(ActivationState.DISABLE_DEACTIVATION); // dynamic bodyB (child) below it : tr = IndexedMatrix.CreateTranslation(20,0,0); RigidBody pBodyB = LocalCreateRigidBody(1.0f, ref tr, shape); pBodyB.SetActivationState(ActivationState.DISABLE_DEACTIVATION); // add some (arbitrary) data to build constraint frames IndexedVector3 parentAxis = new IndexedVector3(1.0f, 0.0f, 0.0f); IndexedVector3 childAxis = new IndexedVector3(0.0f, 0.0f, 1.0f); IndexedVector3 anchor = new IndexedVector3(20.0f, 2.0f, 0.0f); UniversalConstraint pUniv = new UniversalConstraint(pBodyA, pBodyB, ref anchor, ref parentAxis, ref childAxis); pUniv.SetLowerLimit(-MathUtil.SIMD_HALF_PI * 0.5f, -MathUtil.SIMD_HALF_PI * 0.5f); pUniv.SetUpperLimit(MathUtil.SIMD_HALF_PI * 0.5f, MathUtil.SIMD_HALF_PI * 0.5f); // add constraint to world m_dynamicsWorld.AddConstraint(pUniv, true); // draw constraint frames and limits for debugging pUniv.SetDbgDrawSize(5.0f); } #endif #if true // WORKS OK { // create a generic 6DOF constraint with springs IndexedMatrix tr = IndexedMatrix.CreateTranslation(-20f,16f,0f); //tr.setIdentity(); //tr.setOrigin(btVector3(btScalar(-20.), btScalar(16.), btScalar(0.))); //tr.getBasis().setEulerZYX(0,0,0); RigidBody pBodyA = LocalCreateRigidBody( 0.0f, ref tr, shape); pBodyA.SetActivationState(ActivationState.DISABLE_DEACTIVATION); //tr.setIdentity(); //tr.setOrigin(btVector3(btScalar(-10.), btScalar(16.), btScalar(0.))); //tr.getBasis().setEulerZYX(0,0,0); tr = IndexedMatrix.CreateTranslation(-10,16,0); RigidBody pBodyB = LocalCreateRigidBody(1.0f, ref tr, shape); pBodyB.SetActivationState(ActivationState.DISABLE_DEACTIVATION); IndexedMatrix frameInA = IndexedMatrix.CreateTranslation(10f,0f,0f); IndexedMatrix frameInB = IndexedMatrix.CreateTranslation(0f,0f,0f); Generic6DofSpringConstraint pGen6DOFSpring = new Generic6DofSpringConstraint(pBodyA, pBodyB, ref frameInA, ref frameInB, true); pGen6DOFSpring.SetLinearUpperLimit(new IndexedVector3(5f, 0f, 0f)); pGen6DOFSpring.SetLinearLowerLimit(new IndexedVector3(-5f, 0f, 0f)); pGen6DOFSpring.SetAngularLowerLimit(new IndexedVector3(0f, 0f, -1.5f)); pGen6DOFSpring.SetAngularUpperLimit(new IndexedVector3(0f, 0f, 1.5f)); m_dynamicsWorld.AddConstraint(pGen6DOFSpring, true); pGen6DOFSpring.SetDbgDrawSize(5.0f); pGen6DOFSpring.EnableSpring(0, true); pGen6DOFSpring.SetStiffness(0, 39.478f); pGen6DOFSpring.SetDamping(0, 0.5f); pGen6DOFSpring.EnableSpring(5, true); pGen6DOFSpring.SetStiffness(5, 39.478f); pGen6DOFSpring.SetDamping(0, 0.3f); pGen6DOFSpring.SetEquilibriumPoint(); } #endif #if true { // WORKS OK // create a Hinge2 joint // create two rigid bodies // static bodyA (parent) on top: IndexedMatrix tr = IndexedMatrix.CreateTranslation(-20f,4f,0f); RigidBody pBodyA = LocalCreateRigidBody( 0.0f, ref tr, shape); pBodyA.SetActivationState(ActivationState.DISABLE_DEACTIVATION); // dynamic bodyB (child) below it : tr = IndexedMatrix.CreateTranslation(-20f,0f,0f); RigidBody pBodyB = LocalCreateRigidBody(1.0f, ref tr, shape); pBodyB.SetActivationState(ActivationState.DISABLE_DEACTIVATION); // add some data to build constraint frames IndexedVector3 parentAxis = new IndexedVector3(0.0f, 1.0f, 0.0f); IndexedVector3 childAxis = new IndexedVector3(1.0f, 0.0f, 0.0f); IndexedVector3 anchor = new IndexedVector3(-20.0f, 0.0f, 0.0f); Hinge2Constraint pHinge2 = new Hinge2Constraint(pBodyA, pBodyB, ref anchor, ref parentAxis, ref childAxis); pHinge2.SetLowerLimit(-MathUtil.SIMD_HALF_PI * 0.5f); pHinge2.SetUpperLimit(MathUtil.SIMD_HALF_PI * 0.5f); // add constraint to world m_dynamicsWorld.AddConstraint(pHinge2, true); // draw constraint frames and limits for debugging pHinge2.SetDbgDrawSize(5.0f); } #endif #if true { // WORKS OK // create a Hinge joint between two dynamic bodies // create two rigid bodies // static bodyA (parent) on top: IndexedMatrix tr = IndexedMatrix.CreateTranslation(-20f,-2f,0f); RigidBody pBodyA = LocalCreateRigidBody( 1.0f, ref tr, shape); pBodyA.SetActivationState(ActivationState.DISABLE_DEACTIVATION); // dynamic bodyB: tr = IndexedMatrix.CreateTranslation(-30f,-2f,0f); RigidBody pBodyB = LocalCreateRigidBody(10.0f, ref tr, shape); pBodyB.SetActivationState(ActivationState.DISABLE_DEACTIVATION); // add some data to build constraint frames IndexedVector3 axisA = new IndexedVector3(0.0f, 1.0f, 0.0f); IndexedVector3 axisB = new IndexedVector3(0.0f, 1.0f, 0.0f); IndexedVector3 pivotA = new IndexedVector3(-5.0f, 0.0f, 0.0f); IndexedVector3 pivotB = new IndexedVector3(5.0f, 0.0f, 0.0f); spHingeDynAB = new HingeConstraint(pBodyA, pBodyB, ref pivotA, ref pivotB, ref axisA, ref axisB); spHingeDynAB.SetLimit(-MathUtil.SIMD_HALF_PI * 0.5f, MathUtil.SIMD_HALF_PI * 0.5f); // add constraint to world m_dynamicsWorld.AddConstraint(spHingeDynAB, true); // draw constraint frames and limits for debugging spHingeDynAB.SetDbgDrawSize(5.0f); } #endif #if true { // 6DOF connected to the world, with motor IndexedMatrix tr = IndexedMatrix.CreateTranslation(10,-15,0); RigidBody pBody = LocalCreateRigidBody( 1.0f, ref tr, shape); pBody.SetActivationState(ActivationState.DISABLE_DEACTIVATION); IndexedMatrix frameB = IndexedMatrix.Identity; Generic6DofConstraint pGen6Dof = new Generic6DofConstraint(pBody, ref frameB, false ); m_dynamicsWorld.AddConstraint(pGen6Dof); pGen6Dof.SetDbgDrawSize(5.0f); pGen6Dof.SetAngularLowerLimit(new IndexedVector3(0,0,0)); pGen6Dof.SetAngularUpperLimit(new IndexedVector3(0,0,0)); pGen6Dof.SetLinearLowerLimit(new IndexedVector3(-10.0f, 0, 0)); pGen6Dof.SetLinearUpperLimit(new IndexedVector3(10.0f, 0, 0)); pGen6Dof.GetTranslationalLimitMotor().m_enableMotor[0] = true; pGen6Dof.GetTranslationalLimitMotor().m_targetVelocity[0] = 5.0f; pGen6Dof.GetTranslationalLimitMotor().m_maxMotorForce[0] = 0.1f; } #endif }
/// <summary> /// Called when the game has determined that game logic needs to be processed. /// </summary> /// <param name="gameTime">Time passed since the last call to this function.</param> protected override void Update(GameTime gameTime) { MouseState mouseState = Mouse.GetState(); Vector3 rayTo = getRayTo(mouseState.X, mouseState.Y); if (mouseState.LeftButton == ButtonState.Pressed && _prevMouseState.LeftButton == ButtonState.Released) { shootBox(rayTo); } if (mouseState.MiddleButton == ButtonState.Pressed && _prevMouseState.MiddleButton == ButtonState.Released) { if (_world != null) { CollisionWorld.ClosestRayResultCallback rayCallback = new CollisionWorld.ClosestRayResultCallback(_camera.Position, rayTo); _world.RayTest(_camera.Position, rayTo, rayCallback); if (rayCallback.HasHit) { RigidBody body = RigidBody.Upcast(rayCallback.CollisionObject); if (body != null) { //other exclusions? if (!(body.IsStaticObject || body.IsKinematicObject)) { _pickedBody = body; _pickedBody.ActivationState = ActivationState.DisableDeactivation; Vector3 pickPos = rayCallback.HitPointWorld; Vector3 localPivot = Vector3.Transform(pickPos, XnaDevRu.BulletX.MathHelper.InvertMatrix(body.CenterOfMassTransform)); Point2PointConstraint p2p = new Point2PointConstraint(body, localPivot); _world.AddConstraint(p2p); _pickConstraint = p2p; //save mouse position for dragging _oldPickingPos = rayTo; Vector3 eyePos = new Vector3(_camera.Position.X, _camera.Position.Y, _camera.Position.Z); _oldPickingDist = (eyePos - pickPos).Length(); //very weak constraint for picking p2p.Settings.Tau = 1.1f; } } } } } else if (mouseState.MiddleButton == ButtonState.Released && _prevMouseState.MiddleButton == ButtonState.Pressed) { if (_pickConstraint != null && _world != null) { _world.RemoveConstraint(_pickConstraint); _pickConstraint = null; _pickedBody.ForceActivationState(ActivationState.Active); _pickedBody.DeactivationTime = 0f; _pickedBody = null; } } if (_pickConstraint != null) { //move the constraint pivot Point2PointConstraint p2p = _pickConstraint as Point2PointConstraint; if (p2p != null) { //keep it at the same picking distance Vector3 dir = rayTo - _camera.Position; dir.Normalize(); dir *= _oldPickingDist; Vector3 newPos = _camera.Position + dir; p2p.PivotInB = newPos; } } _prevMouseState = mouseState; if (Keyboard.GetState().IsKeyDown(Keys.Space)) { //world.stepSimulation(1.0f/60.0f,0); int numObjects = _world.CollisionObjectsCount; for (int i = 0; i < numObjects; i++) { CollisionObject colObj = _world.CollisionObjects[i]; RigidBody body = RigidBody.Upcast(colObj); if (body != null) { if (body.MotionState != null) { DefaultMotionState myMotionState = (DefaultMotionState)body.MotionState; myMotionState.GraphicsWorldTransform = myMotionState.StartWorldTransform; colObj.WorldTransform = myMotionState.GraphicsWorldTransform; colObj.InterpolationWorldTransform = myMotionState.StartWorldTransform; colObj.Activate(); } //removed cached contact points _world.Broadphase.CleanProxyFromPairs(colObj.Broadphase); if (body != null && !body.IsStaticObject) { RigidBody.Upcast(colObj).LinearVelocity = new Vector3(0, 0, 0); RigidBody.Upcast(colObj).AngularVelocity = new Vector3(0, 0, 0); } } } } else if (Keyboard.GetState().IsKeyDown(Keys.Escape) || GamePad.GetState(PlayerIndex.One).Buttons.Back == ButtonState.Pressed) { Exit(); } else { //world.stepSimulation(1.0f / 60.0f, 1); } base.Update(gameTime); }
/// <summary> /// 剛体に空間上の点への拘束を追加 /// </summary> /// <param name="body">剛体</param> /// <param name="pivot">拘束する点</param> public void AddPointToPointConstraint(RigidBody body, ref Vector3 pivot) { dynamicsWorld.AddConstraint(new Point2PointConstraint(body, pivot)); }