public Point2PointConstraint(RigidBody rbA, Vector3 pivotInA) : base(rbA) { _jacobian = new JacobianEntry[3]; _pivotInA = pivotInA; _pivotInB = MathHelper.MatrixToVector(rbA.CenterOfMassTransform, _pivotInA); }
public Point2PointConstraint(RigidBody rbA, RigidBody rbB, Vector3 pivotInA, Vector3 pivotInB) : base(rbA, rbB) { _jacobian = new JacobianEntry[3]; _pivotInA = pivotInA; _pivotInB = pivotInB; }
public HingeConstraint(RigidBody rbA, RigidBody rbB, Vector3 pivotInA, Vector3 pivotInB, Vector3 axisInA, Vector3 axisInB) : base(rbA, rbB) { _pivotInA = pivotInA; _pivotInB = pivotInB; _axisInA = axisInA; _axisInB = -axisInB; _angularOnly = false; }
public HingeConstraint(RigidBody rbA, Vector3 pivotInA, Vector3 axisInA) : base(rbA) { _pivotInA = pivotInA; _pivotInB = MathHelper.MatrixToVector(rbA.CenterOfMassTransform, pivotInA); _axisInA = axisInA; //fixed axis in worldspace _axisInB = MathHelper.TransformNormal(-axisInA, rbA.CenterOfMassTransform); _angularOnly = false; }
public TypedConstraint(RigidBody rbA, RigidBody rbB) { _userConstraintType = -1; _userConstraintId = -1; _rbA = rbA; _rbB = rbB; _appliedImpulse = 0; _fixed.SetMassProps(0, new Vector3()); }
// solve unilateral constraint (equality, direct method) public void ResolveUnilateralPairConstraint( RigidBody body1, RigidBody body2, Matrix world2A, Matrix world2B, Vector3 invInertiaADiag, float invMassA, Vector3 linvelA, Vector3 angvelA, Vector3 rel_posA1, Vector3 invInertiaBDiag, float invMassB, Vector3 linvelB, Vector3 angvelB, Vector3 rel_posA2, float depthA, Vector3 normalA, Vector3 rel_posB1, Vector3 rel_posB2, float depthB, Vector3 normalB, out float imp0, out float imp1) { imp0 = 0; imp1 = 0; float len = Math.Abs(normalA.Length()) - 1f; if (Math.Abs(len) >= float.Epsilon) return; BulletDebug.Assert(len < float.Epsilon); //this jacobian entry could be re-used for all iterations JacobianEntry jacA = new JacobianEntry(world2A, world2B, rel_posA1, rel_posA2, normalA, invInertiaADiag, invMassA, invInertiaBDiag, invMassB); JacobianEntry jacB = new JacobianEntry(world2A, world2B, rel_posB1, rel_posB2, normalB, invInertiaADiag, invMassA, invInertiaBDiag, invMassB); float vel0 = Vector3.Dot(normalA, body1.GetVelocityInLocalPoint(rel_posA1) - body2.GetVelocityInLocalPoint(rel_posA1)); float vel1 = Vector3.Dot(normalB, body1.GetVelocityInLocalPoint(rel_posB1) - body2.GetVelocityInLocalPoint(rel_posB1)); // btScalar penetrationImpulse = (depth*contactTau*timeCorrection) * massTerm;//jacDiagABInv float massTerm = 1f / (invMassA + invMassB); // calculate rhs (or error) terms float dv0 = depthA * _tau * massTerm - vel0 * _damping; float dv1 = depthB * _tau * massTerm - vel1 * _damping; float nonDiag = jacA.GetNonDiagonal(jacB, invMassA, invMassB); float invDet = 1.0f / (jacA.Diagonal * jacB.Diagonal - nonDiag * nonDiag); imp0 = dv0 * jacA.Diagonal * invDet + dv1 * -nonDiag * invDet; imp1 = dv1 * jacB.Diagonal * invDet + dv0 * -nonDiag * invDet; }
public Generic6DofConstraint(RigidBody rbA, RigidBody rbB, Matrix frameInA, Matrix frameInB) : base(rbA, rbB) { _frameInA = frameInA; _frameInB = frameInB; //free means upper < lower, //locked means upper == lower //limited means upper > lower //so start all locked for (int i = 0; i < 6; ++i) { _lowerLimit[i] = 0.0f; _upperLimit[i] = 0.0f; _accumulatedImpulse[i] = 0.0f; } }
public static float ResolveSingleFrictionOriginal( RigidBody bodyA, RigidBody bodyB, ManifoldPoint contactPoint, ContactSolverInfo solverInfo) { Vector3 posA = contactPoint.PositionWorldOnA; Vector3 posB = contactPoint.PositionWorldOnB; Vector3 relPosA = posA - bodyA.CenterOfMassPosition; Vector3 relPosB = posB - bodyB.CenterOfMassPosition; ConstraintPersistentData cpd = contactPoint.UserPersistentData as ConstraintPersistentData; if (cpd == null) throw new BulletException(); float combinedFriction = cpd.Friction; float limit = cpd.AppliedImpulse * combinedFriction; //if (contactPoint.m_appliedImpulse>0.f) //friction { //apply friction in the 2 tangential directions { // 1st tangent Vector3 velA = bodyA.GetVelocityInLocalPoint(relPosA); Vector3 velB = bodyB.GetVelocityInLocalPoint(relPosB); Vector3 vel = velA - velB; float vrel = Vector3.Dot(cpd.FrictionWorldTangentialA, vel); // calculate j that moves us to zero relative velocity float j = -vrel * cpd.JacDiagABInvTangentA; float total = cpd.AccumulatedTangentImpulseA + j; if (limit < total) total = limit; if (total < -limit) total = -limit; j = total - cpd.AccumulatedTangentImpulseA; cpd.AccumulatedTangentImpulseA = total; bodyA.ApplyImpulse(j * cpd.FrictionWorldTangentialA, relPosA); bodyB.ApplyImpulse(j * -cpd.FrictionWorldTangentialA, relPosB); } { // 2nd tangent Vector3 velA = bodyA.GetVelocityInLocalPoint(relPosA); Vector3 velB = bodyB.GetVelocityInLocalPoint(relPosB); Vector3 vel = velA - velB; float vrel = Vector3.Dot(cpd.FrictionWorldTangentialB, vel); // calculate j that moves us to zero relative velocity float j = -vrel * cpd.JacDiagABInvTangentB; float total = cpd.AccumulatedTangentImpulseB + j; if (limit < total) total = limit; if (total < -limit) total = -limit; j = total - cpd.AccumulatedTangentImpulseB; cpd.AccumulatedTangentImpulseB = total; bodyA.ApplyImpulse(j * cpd.FrictionWorldTangentialB, relPosA); bodyB.ApplyImpulse(j * -cpd.FrictionWorldTangentialB, relPosB); } } return cpd.AppliedImpulse; }
public override void AddRigidBody(RigidBody body) { if (!body.IsStaticOrKinematicObject) { body.Gravity = _gravity; } if (body.CollisionShape != null) { bool isDynamic = !(body.IsStaticObject || body.IsKinematicObject); BroadphaseProxy.CollisionFilterGroups collisionFilterGroup = isDynamic ? BroadphaseProxy.CollisionFilterGroups.Default : BroadphaseProxy.CollisionFilterGroups.Static; BroadphaseProxy.CollisionFilterGroups collisionFilterMask = isDynamic ? BroadphaseProxy.CollisionFilterGroups.All : (BroadphaseProxy.CollisionFilterGroups.All ^ BroadphaseProxy.CollisionFilterGroups.Static); AddCollisionObject(body, collisionFilterGroup, collisionFilterMask); } }
// solving 2x2 lcp problem (inequality, direct solution ) public void ResolveBilateralPairConstraint( RigidBody body1, RigidBody body2, Matrix world2A, Matrix world2B, Vector3 invInertiaADiag, float invMassA, Vector3 linvelA, Vector3 angvelA, Vector3 rel_posA1, Vector3 invInertiaBDiag, float invMassB, Vector3 linvelB, Vector3 angvelB, Vector3 rel_posA2, float depthA, Vector3 normalA, Vector3 rel_posB1, Vector3 rel_posB2, float depthB, Vector3 normalB, out float imp0, out float imp1) { imp0 = 0f; imp1 = 0f; float len = Math.Abs(normalA.Length()) - 1f; if (Math.Abs(len) >= float.Epsilon) return; BulletDebug.Assert(len < float.Epsilon); JacobianEntry jacA = new JacobianEntry(world2A, world2B, rel_posA1, rel_posA2, normalA, invInertiaADiag, invMassA, invInertiaBDiag, invMassB); JacobianEntry jacB = new JacobianEntry(world2A, world2B, rel_posB1, rel_posB2, normalB, invInertiaADiag, invMassA, invInertiaBDiag, invMassB); float vel0 = Vector3.Dot(normalA, body1.GetVelocityInLocalPoint(rel_posA1) - body2.GetVelocityInLocalPoint(rel_posA1)); float vel1 = Vector3.Dot(normalB, body1.GetVelocityInLocalPoint(rel_posB1) - body2.GetVelocityInLocalPoint(rel_posB1)); // calculate rhs (or error) terms float dv0 = depthA * _tau - vel0 * _damping; float dv1 = depthB * _tau - vel1 * _damping; float nonDiag = jacA.GetNonDiagonal(jacB, invMassA, invMassB); float invDet = 1.0f / (jacA.Diagonal * jacB.Diagonal - nonDiag * nonDiag); imp0 = dv0 * jacA.Diagonal * invDet + dv1 * -nonDiag * invDet; imp1 = dv1 * jacB.Diagonal * invDet + dv0 * -nonDiag * invDet; if (imp0 > 0.0f) { if (imp1 <= 0.0f) { imp1 = 0f; // now imp0>0 imp1<0 imp0 = dv0 / jacA.Diagonal; if (imp0 < 0.0f) imp0 = 0f; } } else { imp0 = 0f; imp1 = dv1 / jacB.Diagonal; if (imp1 <= 0.0f) { imp1 = 0f; // now imp0>0 imp1<0 imp0 = dv0 / jacA.Diagonal; if (imp0 > 0.0f) { } else { imp0 = 0f; } } } }
//velocity + friction //response between two dynamic objects with friction public static float ResolveSingleCollisionCombined( RigidBody bodyA, RigidBody bodyB, ManifoldPoint contactPoint, ContactSolverInfo solverInfo) { Vector3 posA = contactPoint.PositionWorldOnA; Vector3 posB = contactPoint.PositionWorldOnB; Vector3 normal = contactPoint.NormalWorldOnB; Vector3 relPosA = posA - bodyA.CenterOfMassPosition; Vector3 relPosB = posB - bodyB.CenterOfMassPosition; Vector3 velA = bodyA.GetVelocityInLocalPoint(relPosA); Vector3 velB = bodyB.GetVelocityInLocalPoint(relPosB); Vector3 vel = velA - velB; float relVel; relVel = Vector3.Dot(normal, vel); float Kfps = 1f / solverInfo.TimeStep; //float damping = solverInfo.m_damping; float Kerp = solverInfo.Erp; float Kcor = Kerp * Kfps; ConstraintPersistentData cpd = contactPoint.UserPersistentData as ConstraintPersistentData; if (cpd == null) throw new BulletException(); float distance = cpd.Penetration; float positionalError = Kcor * -distance; float velocityError = cpd.Restitution - relVel;// * damping; float penetrationImpulse = positionalError * cpd.JacDiagABInv; float velocityImpulse = velocityError * cpd.JacDiagABInv; float normalImpulse = penetrationImpulse + velocityImpulse; // See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse float oldNormalImpulse = cpd.AppliedImpulse; float sum = oldNormalImpulse + normalImpulse; cpd.AppliedImpulse = 0 > sum ? 0 : sum; normalImpulse = cpd.AppliedImpulse - oldNormalImpulse; if (bodyA.InverseMass != 0) { bodyA.InternalApplyImpulse(contactPoint.NormalWorldOnB * bodyA.InverseMass, cpd.AngularComponentA, normalImpulse); } if (bodyB.InverseMass != 0) { bodyB.InternalApplyImpulse(contactPoint.NormalWorldOnB * bodyB.InverseMass, cpd.AngularComponentB, -normalImpulse); } { //friction Vector3 vel12 = bodyA.GetVelocityInLocalPoint(relPosA); Vector3 vel22 = bodyB.GetVelocityInLocalPoint(relPosB); Vector3 vel3 = vel12 - vel22; relVel = Vector3.Dot(normal, vel3); Vector3 latVel = vel3 - normal * relVel; float lat_rel_vel = latVel.Length(); float combinedFriction = cpd.Friction; if (cpd.AppliedImpulse > 0) if (lat_rel_vel > float.Epsilon) { latVel /= lat_rel_vel; Vector3 temp1 = Vector3.TransformNormal(Vector3.Cross(relPosA, latVel), bodyA.InvInertiaTensorWorld); Vector3 temp2 = Vector3.TransformNormal(Vector3.Cross(relPosB, latVel), bodyB.InvInertiaTensorWorld); float friction_impulse = lat_rel_vel / (bodyA.InverseMass + bodyB.InverseMass + Vector3.Dot(latVel, Vector3.Cross(temp1, relPosA) + Vector3.Cross(temp2, relPosB))); float normal_impulse = cpd.AppliedImpulse * combinedFriction; MathHelper.SetMin(ref friction_impulse, normal_impulse); MathHelper.SetMin(ref friction_impulse, -normal_impulse); bodyA.ApplyImpulse(latVel * -friction_impulse, relPosA); bodyB.ApplyImpulse(latVel * friction_impulse, relPosB); } } return normalImpulse; }
public override void RemoveRigidBody(RigidBody body) { RemoveCollisionObject(body); }
public static float ResolveSingleFrictionEmpty( RigidBody bodyA, RigidBody bodyB, ManifoldPoint contactPoint, ContactSolverInfo solverInfo) { return 0; }
public ConstraintDemo() { CollisionShape shape = new BoxShape(new Vector3(HalfExtents, HalfExtents, HalfExtents)); Matrix trans = Matrix.Identity; trans.Translation = new Vector3(0, 20, 0); float mass = 1f; //Point to Point constraint (ball socket) { RigidBody bodyA = CreateRigidBody(mass, trans, shape); trans.Translation = new Vector3(2 * HalfExtents, 20, 0); mass = 1f; RigidBody bodyB = null; //RigidBody bodyB = CreateRigidBody(mass, trans, shape); //bodyB.ActivationState = ActivationState.DisableDeactivation; //bodyB.SetDamping(0.3f, 0.3f); Vector3 pivotInA = new Vector3(HalfExtents, -HalfExtents, -HalfExtents); Vector3 axisInA = new Vector3(0, 0, 1); Vector3 pivotInB = bodyB != null ? MathHelper.MatrixToVector(MathHelper.InvertMatrix(bodyB.CenterOfMassTransform), MathHelper.MatrixToVector(bodyA.CenterOfMassTransform, pivotInA)) : pivotInA; Vector3 axisInB = bodyB != null ? Vector3.TransformNormal(Vector3.TransformNormal(axisInA, bodyB.CenterOfMassTransform), MathHelper.InvertMatrix(bodyB.CenterOfMassTransform)) : Vector3.TransformNormal(axisInA, bodyA.CenterOfMassTransform); //TypedConstraint p2p = new Point2PointConstraint(bodyA, bodyB, pivotInA, pivotInB); //TypedConstraint hinge = new HingeConstraint(bodyA, bodyB, pivotInA, pivotInB, axisInA, axisInB); HingeConstraint hinge = new HingeConstraint(bodyA, pivotInA, axisInA); //use zero targetVelocity and a small maxMotorImpulse to simulate joint friction //float targetVelocity = 0.0f; //float maxMotorImpulse = 0.01; float targetVelocity = 1.0f; float maxMotorImpulse = 1.0f; hinge.EnableAngularMotor(true, targetVelocity, maxMotorImpulse); PhysicsWorld.AddConstraint(hinge); } // create a slider, using the generic D6 constraint { mass = 1f; Vector3 sliderWorldPos = new Vector3(0, 10, 0); Vector3 sliderAxis = new Vector3(1, 0, 0); float angle = 0; Matrix sliderOrientation = Matrix.CreateFromQuaternion(new Quaternion(sliderAxis, angle)); trans = Matrix.Identity; trans.Translation = sliderWorldPos; //trans.setBasis(sliderOrientation); _sliderTransform = trans; _d6BodyA = CreateRigidBody(mass, trans, shape); _d6BodyA.ActivationState = ActivationState.DisableDeactivation; RigidBody fixedBody1 = CreateRigidBody(0, trans, null); Matrix frameInA, frameInB; frameInA = Matrix.Identity; frameInB = Matrix.Identity; Generic6DofConstraint slider = new Generic6DofConstraint(_d6BodyA, fixedBody1, frameInA, frameInB); slider.SetLinearLowerLimit(_lowerSliderLimit); slider.SetLinearUpperLimit(_hiSliderLimit); //range should be small, otherwise singularities will 'explode' the constraint slider.SetAngularLowerLimit(new Vector3(10, 0, 0)); slider.SetAngularUpperLimit(new Vector3(0, 0, 0)); PhysicsWorld.AddConstraint(slider); } }
protected float SolveCombinedContactFriction(RigidBody bodyA, RigidBody bodyB, ManifoldPoint cp, ContactSolverInfo info, int iter, IDebugDraw debugDraw) { float maxImpulse = 0; Vector3 color = new Vector3(0, 1, 0); if (cp.Distance <= 0) { if (iter == 0) if (debugDraw != null) debugDraw.DrawContactPoint(cp.PositionWorldOnB, cp.NormalWorldOnB, cp.Distance, cp.LifeTime, color); float impulse = ContactConstraint.ResolveSingleCollisionCombined( bodyA, bodyB, cp, info); if (maxImpulse < impulse) maxImpulse = impulse; } return maxImpulse; }
protected internal void CreateRigidBody(BulletXScene parent_scene, IMesh mesh, OpenMetaverse.Vector3 pos, OpenMetaverse.Vector3 size) { //For RigidBody Constructor. The next values might change float _linearDamping = 0.0f; float _angularDamping = 0.0f; float _friction = 1.0f; float _restitution = 0.0f; Matrix _startTransform = Matrix.Identity; Matrix _centerOfMassOffset = Matrix.Identity; //added by jed zhu _mesh = mesh; lock (BulletXScene.BulletXLock) { _startTransform.Translation = BulletXMaths.PhysicsVectorToXnaVector3(pos); //For now all prims are boxes CollisionShape _collisionShape; if (mesh == null) { _collisionShape = new BoxShape(BulletXMaths.PhysicsVectorToXnaVector3(size)/2.0f); } else { int iVertexCount = mesh.getVertexList().Count; int[] indices = mesh.getIndexListAsInt(); Vector3[] v3Vertices = new Vector3[iVertexCount]; for (int i = 0; i < iVertexCount; i++) { OpenMetaverse.Vector3 v = mesh.getVertexList()[i]; if (v != null) // Note, null has special meaning. See meshing code for details v3Vertices[i] = BulletXMaths.PhysicsVectorToXnaVector3(v); else v3Vertices[i] = Vector3.Zero; } TriangleIndexVertexArray triMesh = new TriangleIndexVertexArray(indices, v3Vertices); _collisionShape = new TriangleMeshShape(triMesh); } DefaultMotionState _motionState = new DefaultMotionState(_startTransform, _centerOfMassOffset); Vector3 _localInertia = new Vector3(); if (_physical) _collisionShape.CalculateLocalInertia(Mass, out _localInertia); //Always when mass > 0 rigidBody = new RigidBody(Mass, _motionState, _collisionShape, _localInertia, _linearDamping, _angularDamping, _friction, _restitution); //rigidBody.ActivationState = ActivationState.DisableDeactivation; //It's seems that there are a bug with rigidBody constructor and its CenterOfMassPosition Vector3 _vDebugTranslation; _vDebugTranslation = _startTransform.Translation - rigidBody.CenterOfMassPosition; rigidBody.Translate(_vDebugTranslation); //--- parent_scene.ddWorld.AddRigidBody(rigidBody); } }
internal void AddForgottenRigidBody(RigidBody forgottenRigidBody) { _forgottenRigidBodies.Add(forgottenRigidBody); }
protected float Solve(RigidBody bodyA, RigidBody bodyB, ManifoldPoint cp, ContactSolverInfo info, int iter, IDebugDraw debugDraw) { float maxImpulse = 0; Vector3 color = new Vector3(0, 1, 0); if (cp.Distance <= 0) { if (iter == 0) if(debugDraw != null) debugDraw.DrawContactPoint(cp.PositionWorldOnB, cp.NormalWorldOnB, cp.Distance, cp.LifeTime, color); ConstraintPersistentData cpd = cp.UserPersistentData as ConstraintPersistentData; float impulse = cpd.ContactSolverFunc( bodyA, bodyB, cp, info); if (maxImpulse < impulse) maxImpulse = impulse; } return maxImpulse; }
private void InitSolverBody(out SolverBody solverBody, RigidBody rigidBody) { solverBody = new SolverBody(); solverBody.AngularVelocity = rigidBody.AngularVelocity; solverBody.CenterOfMassPosition = rigidBody.CenterOfMassPosition; solverBody.Friction = rigidBody.Friction; solverBody.InvMass = rigidBody.InverseMass; solverBody.LinearVelocity = rigidBody.LinearVelocity; solverBody.OriginalBody = rigidBody; solverBody.AngularFactor = rigidBody.AngularFactor; }
protected float SolveFriction(RigidBody bodyA, RigidBody bodyB, ManifoldPoint cp, ContactSolverInfo info, int iter, IDebugDraw debugDraw) { Vector3 color = new Vector3(0, 1, 0); if (cp.Distance <= 0) { ConstraintPersistentData cpd = cp.UserPersistentData as ConstraintPersistentData; cpd.FrictionSolverFunc( bodyA, bodyB, cp, info); } return 0; }
public abstract void AddRigidBody(RigidBody body);
public BulletXCharacter(String avName, BulletXScene parent_scene, OpenMetaverse.Vector3 pos, OpenMetaverse.Vector3 velocity, OpenMetaverse.Vector3 size, OpenMetaverse.Vector3 acceleration, OpenMetaverse.Quaternion orientation) : base(avName) { //This fields will be removed. They're temporal float _sizeX = 0.5f; float _sizeY = 0.5f; float _sizeZ = 1.6f; //. _position = pos; _velocity = velocity; _size = size; //--- _size.X = _sizeX; _size.Y = _sizeY; _size.Z = _sizeZ; //. _acceleration = acceleration; _orientation = orientation; _physical = true; float _mass = 50.0f; //This depends of avatar's dimensions //For RigidBody Constructor. The next values might change float _linearDamping = 0.0f; float _angularDamping = 0.0f; float _friction = 0.5f; float _restitution = 0.0f; Matrix _startTransform = Matrix.Identity; Matrix _centerOfMassOffset = Matrix.Identity; lock (BulletXScene.BulletXLock) { _startTransform.Translation = BulletXMaths.PhysicsVectorToXnaVector3(pos); //CollisionShape _collisionShape = new BoxShape(new MonoXnaCompactMaths.Vector3(1.0f, 1.0f, 1.60f)); //For now, like ODE, collisionShape = sphere of radious = 1.0 CollisionShape _collisionShape = new SphereShape(1.0f); DefaultMotionState _motionState = new DefaultMotionState(_startTransform, _centerOfMassOffset); Vector3 _localInertia = new Vector3(); _collisionShape.CalculateLocalInertia(_mass, out _localInertia); //Always when mass > 0 rigidBody = new RigidBody(_mass, _motionState, _collisionShape, _localInertia, _linearDamping, _angularDamping, _friction, _restitution); //rigidBody.ActivationState = ActivationState.DisableDeactivation; //It's seems that there are a bug with rigidBody constructor and its CenterOfMassPosition Vector3 _vDebugTranslation; _vDebugTranslation = _startTransform.Translation - rigidBody.CenterOfMassPosition; rigidBody.Translate(_vDebugTranslation); parent_scene.ddWorld.AddRigidBody(rigidBody); } }
protected float Solve(RigidBody bodyA, RigidBody bodyB, ManifoldPoint cp, ContactSolverInfo info, int iter) { return Solve(bodyA, bodyB, cp, info, iter, null); }
internal BulletXPlanet(BulletXScene parent_scene, float[] heightField) { _staticPosition = new OpenMetaverse.Vector3(BulletXScene.MaxXY / 2, BulletXScene.MaxXY / 2, 0); // _staticVelocity = new PhysicsVector(); // _staticOrientation = OpenMetaverse.Quaternion.Identity; _mass = 0; //No active // _parentscene = parent_scene; _heightField = heightField; float _linearDamping = 0.0f; float _angularDamping = 0.0f; float _friction = 0.5f; float _restitution = 0.0f; Matrix _startTransform = Matrix.Identity; Matrix _centerOfMassOffset = Matrix.Identity; lock (BulletXScene.BulletXLock) { try { _startTransform.Translation = BulletXMaths.PhysicsVectorToXnaVector3(_staticPosition); CollisionShape _collisionShape = new HeightfieldTerrainShape(BulletXScene.MaxXY, BulletXScene.MaxXY, _heightField, (float) BulletXScene.MaxZ, 2, true, false); DefaultMotionState _motionState = new DefaultMotionState(_startTransform, _centerOfMassOffset); Vector3 _localInertia = new Vector3(); //_collisionShape.CalculateLocalInertia(_mass, out _localInertia); //Always when mass > 0 _flatPlanet = new RigidBody(_mass, _motionState, _collisionShape, _localInertia, _linearDamping, _angularDamping, _friction, _restitution); //It's seems that there are a bug with rigidBody constructor and its CenterOfMassPosition Vector3 _vDebugTranslation; _vDebugTranslation = _startTransform.Translation - _flatPlanet.CenterOfMassPosition; _flatPlanet.Translate(_vDebugTranslation); parent_scene.ddWorld.AddRigidBody(_flatPlanet); } catch (Exception ex) { BulletXScene.BulletXMessage(ex.Message, true); } } BulletXScene.BulletXMessage("BulletXPlanet created.", false); }
/// <summary> /// Creates new rigid body and adds it to the world. /// </summary> /// <param name="mass">Body mass, if 0 body is static.</param> /// <param name="startTransform">Starting body transform.</param> /// <param name="shape">Body shape.</param> /// <returns>Created rigid body.</returns> public RigidBody CreateRigidBody(float mass, Matrix startTransform, CollisionShape shape) { //rigidbody is dynamic if and only if mass is non zero, otherwise static bool isDynamic = (mass != 0.0f); Vector3 localInertia = new Vector3(); if (isDynamic) shape.CalculateLocalInertia(mass, out localInertia); //using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects DefaultMotionState myMotionState = new DefaultMotionState(startTransform, Matrix.Identity); RigidBody body = new RigidBody(mass, myMotionState, shape, localInertia, 0.0f, 0.0f, 0.5f, 0.0f); _world.AddRigidBody(body); return body; }
public TypedConstraint(RigidBody rbA) : this(rbA, _fixed) { }
/// <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="chassis"></param> /// <param name="paramRaycastInfo">Not used!</param> public void UpdateWheel(RigidBody chassis, RaycastInfo paramRaycastInfo) { if (_raycastInfo.IsInContact) { float project = Vector3.Dot(_raycastInfo.ContactNormalWS, _raycastInfo.WheelDirectionWS); Vector3 chassisVelocityAtContactPoint = new Vector3(); Vector3 relpos = _raycastInfo.ContactPointWS - chassis.CenterOfMassPosition; chassisVelocityAtContactPoint = chassis.GetVelocityInLocalPoint(relpos); float projVel = Vector3.Dot(_raycastInfo.ContactNormalWS, chassisVelocityAtContactPoint); if (project >= -0.1f) { _suspensionRelativeVelocity = 0; _clippedInvContactDotSuspension = 1.0f / 0.1f; } else { float inv = -1 / project; _suspensionRelativeVelocity = projVel * inv; _clippedInvContactDotSuspension = inv; } } else { _raycastInfo.SuspensionLength = _suspensionRestLength; _suspensionRelativeVelocity = 0.0f; _raycastInfo.ContactNormalWS = -_raycastInfo.WheelDirectionWS; _clippedInvContactDotSuspension = 1.0f; } }
public override void AddRigidBody(RigidBody body) { body.Gravity = _gravity; if (body.CollisionShape != null) { AddCollisionObject(body); } }
public abstract void RemoveRigidBody(RigidBody body);
protected void SynchronizeMotionStates() { //debug vehicle wheels { //todo: iterate over awake simulation islands! for (int i = 0; i < CollisionObjects.Count; i++) { CollisionObject colObj = CollisionObjects[i]; if (DebugDrawer != null && (DebugDrawer.DebugMode & DebugDrawModes.DrawWireframe) != 0) { Vector3 color = new Vector3(255f, 255f, 255f); switch (colObj.ActivationState) { case ActivationState.Active: color = new Vector3(255f, 255f, 255f); break; case ActivationState.IslandSleeping: color = new Vector3(0f, 255f, 0f); break; case ActivationState.WantsDeactivation: color = new Vector3(0f, 255f, 255f); break; case ActivationState.DisableDeactivation: color = new Vector3(255f, 0f, 0f); break; case ActivationState.DisableSimulation: color = new Vector3(255f, 255f, 0f); break; default: color = new Vector3(255f, 0f, 0f); break; } DebugDrawObject(colObj.WorldTransform, colObj.CollisionShape, color); } RigidBody body = RigidBody.Upcast(colObj); if (body != null && body.MotionState != null && !body.IsStaticOrKinematicObject) { //if (body.ActivationState != ActivationState.IslandSleeping) { Matrix interpolatedTransform = new Matrix(); TransformUtil.IntegrateTransform(body.InterpolationWorldTransform, body.InterpolationLinearVelocity, body.InterpolationAngularVelocity, _localTime, ref interpolatedTransform); body.MotionState.SetWorldTransform(interpolatedTransform); } } } } if (DebugDrawer != null && (DebugDrawer.DebugMode & DebugDrawModes.DrawWireframe) != 0) { for (int i = 0; i < _vehicles.Count; i++) { for (int v = 0; v < _vehicles[i].getNumWheels(); v++) { Vector3 wheelColor = new Vector3(0, 255, 255); if (_vehicles[i].getWheelInfo(v).RaycastInfo.IsInContact) { wheelColor = new Vector3(0, 0, 255); } else { wheelColor = new Vector3(255, 0, 255); } //synchronize the wheels with the (interpolated) chassis worldtransform _vehicles[i].updateWheelTransform(v, true); Vector3 wheelPosWS = _vehicles[i].getWheelInfo(v).WorldTransform.Translation; Vector3 axle = new Vector3( MathHelper.GetElement(_vehicles[i].getWheelInfo(v).WorldTransform, 0, _vehicles[i].getRightAxis()), MathHelper.GetElement(_vehicles[i].getWheelInfo(v).WorldTransform, 1, _vehicles[i].getRightAxis()), MathHelper.GetElement(_vehicles[i].getWheelInfo(v).WorldTransform, 2, _vehicles[i].getRightAxis())); //m_vehicles[i]->getWheelInfo(v).m_raycastInfo.m_wheelAxleWS //debug wheels (cylinders) _debugDrawer.DrawLine(wheelPosWS, wheelPosWS + axle, wheelColor); _debugDrawer.DrawLine(wheelPosWS, _vehicles[i].getWheelInfo(v).RaycastInfo.ContactPointWS, wheelColor); } } } }