/// <summary> /// Solve the constraint for position. /// </summary> /// <returns>Returns a value indicating whether the constraint has been satisfied.</returns> public override bool ProcessPosition() { RigidBody a = BodyA, b = BodyB; // get offsets and distance in world coordinates Vector3 impulse; Vector3.Transform(ref _bodyPointA, ref a.World.Combined, out _worldOffsetA); Vector3.Transform(ref _bodyPointB, ref b.World.Combined, out _worldOffsetB); Vector3.Subtract(ref _worldOffsetA, ref _worldOffsetB, out impulse); Vector3.Subtract(ref _worldOffsetA, ref a.World.Position, out _worldOffsetA); Vector3.Subtract(ref _worldOffsetB, ref b.World.Position, out _worldOffsetB); float error = impulse.Length(); if (error <= this.Manager.LinearErrorTolerance) { return(true); } // need normalized direction to calculate effective mass Vector3 n; Vector3.Divide(ref impulse, error, out n); float mass = MassProperties.EffectiveMass(ref a.MassWorld, ref b.MassWorld, ref _worldOffsetA, ref _worldOffsetB, ref n); Vector3.Multiply(ref impulse, mass * this.Manager.PositionCorrectionFactor, out impulse); // apply impulse b.ApplyFlashImpulse(ref impulse, ref _worldOffsetB); Vector3.Negate(ref impulse, out impulse); a.ApplyFlashImpulse(ref impulse, ref _worldOffsetA); return(false); }
/// <summary> /// Prepares the constraint for iterative processing in the current frame. /// </summary> public override void PreProcess() { RigidBody a = BodyA, b = BodyB; // get world points and normal Vector3.Transform(ref _bodyPointA, ref a.World.Combined, out _worldOffsetA); Vector3.Transform(ref _bodyPointB, ref b.World.Combined, out _worldOffsetB); Vector3.Subtract(ref _worldOffsetA, ref _worldOffsetB, out _normal); Vector3.Subtract(ref _worldOffsetA, ref a.World.Position, out _worldOffsetA); Vector3.Subtract(ref _worldOffsetB, ref b.World.Position, out _worldOffsetB); _distance = _normal.Length(); if (_distance < Constants.Epsilon) { _normal = Vector3.Zero; } else { Vector3.Divide(ref _normal, _distance, out _normal); } _mass = MassProperties.EffectiveMass(ref a.MassWorld, ref b.MassWorld, ref _worldOffsetA, ref _worldOffsetB, ref _normal); // determine the constraint behavior for this frame var prevState = _state; if (Math.Abs(_maxDistance - _minDistance) < 2f * this.Manager.LinearErrorTolerance) { _state = LimitState.Equal; } else if (_distance <= _minDistance) { _state = LimitState.Min; } else if (_distance >= _maxDistance) { _state = LimitState.Max; } else { _state = LimitState.Between; } if (this.Manager.IsSolverWarmStarted && prevState == _state) { Vector3 impulse; Vector3.Multiply(ref _impulse, this.Manager.TimeStep, out impulse); b.ApplyImpulse(ref impulse, ref _worldOffsetB); Vector3.Negate(ref impulse, out impulse); a.ApplyImpulse(ref impulse, ref _worldOffsetA); } else { _impulse = Vector3.Zero; } _pImpulse = Vector3.Zero; }
private void ComputeVitals() { RigidBody a = this.BodyA, b = this.BodyB; // compute relative basis between the two objects in world position Frame.Transform(ref _bodyBasisA, ref this.BodyA.World, out _worldBasisA); Frame.Transform(ref _bodyBasisB, ref this.BodyB.World, out _worldBasisB); Frame rel; Frame.Subtract(ref _worldBasisA, ref _worldBasisB, out rel); // compute current positions and angles Vector3.Subtract(ref _worldBasisA.Origin, ref _worldBasisB.Origin, out _positions); Matrix m; _worldBasisB.ToMatrix(out m); Matrix.Transpose(ref m, out m); Vector3.Transform(ref _positions, ref m, out _positions); rel.ComputeEulerAnglesXYZ(out _angles); // borrowed from Bullet; construct the axes about which we actually restrict rotation Vector3.Cross(ref _worldBasisB.Z, ref _worldBasisA.X, out _axisY); Vector3.Cross(ref _axisY, ref _worldBasisB.Z, out _axisX); Vector3.Cross(ref _worldBasisA.X, ref _axisY, out _axisZ); _axisX.Normalize(); _axisY.Normalize(); _axisZ.Normalize(); // calculate effective inertia along each axis Matrix inertia = new Matrix(); if (a.IsMovable) { Matrix.Add(ref inertia, ref a.MassWorld.InertiaInverse, out inertia); } if (b.IsMovable) { Matrix.Add(ref inertia, ref b.MassWorld.InertiaInverse, out inertia); } inertia.M44 = 1f; Matrix.Invert(ref inertia, out inertia); Vector3 d; Vector3.Transform(ref _axisX, ref inertia, out d); Vector3.Dot(ref d, ref _axisX, out _inertia.X); Vector3.Transform(ref _axisY, ref inertia, out d); Vector3.Dot(ref d, ref _axisY, out _inertia.Y); Vector3.Transform(ref _axisZ, ref inertia, out d); Vector3.Dot(ref d, ref _axisZ, out _inertia.Z); // calculate effective mass along each axis of basis B //float w = b.MassWorld.MassInverse / (a.MassWorld.MassInverse = b.MassWorld.MassInverse); //Vector3.Multiply(ref _worldBasisA.Origin, 1f - w, out _anchorA); //Vector3.Multiply(ref _worldBasisB.Origin, w, out _anchorB); //Vector3.Add(ref _anchorA, ref _anchorB, out _anchorB); //Vector3.Subtract(ref _anchorB, ref a.World.Position, out _anchorA); //Vector3.Subtract(ref _anchorB, ref b.World.Position, out _anchorB); Vector3.Subtract(ref _worldBasisA.Origin, ref a.World.Position, out _anchorA); Vector3.Subtract(ref _worldBasisB.Origin, ref b.World.Position, out _anchorB); _mass.X = MassProperties.EffectiveMass(ref a.MassWorld, ref b.MassWorld, ref _anchorA, ref _anchorB, ref _worldBasisB.X); _mass.Y = MassProperties.EffectiveMass(ref a.MassWorld, ref b.MassWorld, ref _anchorA, ref _anchorB, ref _worldBasisB.Y); _mass.Z = MassProperties.EffectiveMass(ref a.MassWorld, ref b.MassWorld, ref _anchorA, ref _anchorB, ref _worldBasisB.Z); }
/// <summary> /// Solve the constraint for position. /// </summary> /// <returns>Returns a value indicating whether the constraint has been satisfied.</returns> public override bool ProcessPosition() { RigidBody a = BodyA, b = BodyB; if (_state == LimitState.Between) { return(true); } // recalculate vitals Vector3.Transform(ref _bodyPointA, ref a.World.Combined, out _worldOffsetA); Vector3.Transform(ref _bodyPointB, ref b.World.Combined, out _worldOffsetB); Vector3.Subtract(ref _worldOffsetA, ref _worldOffsetB, out _normal); Vector3.Subtract(ref _worldOffsetA, ref a.World.Position, out _worldOffsetA); Vector3.Subtract(ref _worldOffsetB, ref b.World.Position, out _worldOffsetB); _distance = _normal.Length(); Vector3.Divide(ref _normal, _distance, out _normal); _mass = MassProperties.EffectiveMass(ref a.MassWorld, ref b.MassWorld, ref _worldOffsetA, ref _worldOffsetB, ref _normal); // the error depends on the current limit state float error = 0f; switch (_state) { case LimitState.Equal: if (_distance > _maxDistance) { error = _distance - _maxDistance; } else if (_distance < _minDistance) { error = _distance - _minDistance; } break; case LimitState.Min: error = MathHelper.Min(_distance - _minDistance, 0f); break; case LimitState.Max: error = MathHelper.Max(_distance - _maxDistance, 0f); break; } if (Math.Abs(error) <= this.Manager.LinearErrorTolerance) { return(true); } // clamp impulse Vector3 impulse, oldImpulse = _pImpulse; Vector3.Multiply(ref _normal, error * _mass * this.Manager.PositionCorrectionFactor, out impulse); Vector3.Add(ref _pImpulse, ref impulse, out _pImpulse); float d; Vector3.Dot(ref _normal, ref _pImpulse, out d); if (_state == LimitState.Min && d > 0f || _state == LimitState.Max && d < 0f) { _pImpulse = Vector3.Zero; } Vector3.Subtract(ref _pImpulse, ref oldImpulse, out impulse); // apply impulse b.ApplyFlashImpulse(ref impulse, ref _worldOffsetB); Vector3.Negate(ref impulse, out impulse); a.ApplyFlashImpulse(ref impulse, ref _worldOffsetA); return(false); }
/// <summary> /// Prepare the contact constraint for iterative processing within a single frame. Computes the desired target velocities /// to attempt to prevent inter-penetration. /// </summary> public override void PreProcess() { if (this.IsCollisionSuppressed) { return; } RigidBody a = BodyA, b = BodyB; var cached = b.Manager.ContactCache.Get(a, b); bool isWarmStarted = cached != null && cached.Count == _count; // calculate relative force applied during this frame Vector3 va = Vector3.Zero, vb = Vector3.Zero; float forceMag; if (a.IsMovable) { Vector3.Multiply(ref a.Force, this.Manager.TimeStep * a.Mass.MassInverse, out va); } if (b.IsMovable) { Vector3.Multiply(ref b.Force, this.Manager.TimeStep * b.Mass.MassInverse, out vb); } Vector3.Add(ref va, ref vb, out va); Vector3.Dot(ref _normal, ref va, out forceMag); forceMag = MathHelper.Min(forceMag, 0f); for (int i = 0; i < _count; i++) { var p = _points[i]; // calculate movement along normal and tangent float normalDelta; a.GetVelocityAtPoint(ref p.OffsetA, out va); b.GetVelocityAtPoint(ref p.OffsetB, out vb); Vector3.Subtract(ref va, ref vb, out va); Vector3.Dot(ref _normal, ref va, out normalDelta); float tangentDelta; Vector3.Multiply(ref _normal, normalDelta, out p.Tangent); Vector3.Subtract(ref va, ref p.Tangent, out p.Tangent); Vector3.Dot(ref p.Tangent, ref va, out tangentDelta); if (p.Tangent.LengthSquared() >= Constants.Epsilon) { p.Tangent.Normalize(); Vector3.Negate(ref p.Tangent, out p.Tangent); } else { p.Tangent = Vector3.Zero; } // calculate effective mass along tangent and normal p.NormalMass = MassProperties.EffectiveMass(ref a.MassWorld, ref b.MassWorld, ref p.OffsetA, ref p.OffsetB, ref _normal); p.TangentMass = p.Tangent != Vector3.Zero ? MassProperties.EffectiveMass(ref a.MassWorld, ref b.MassWorld, ref p.OffsetA, ref p.OffsetB, ref p.Tangent) : 0f; // calculate target velocity float restitution = Math.Max(_restitution * -(normalDelta - forceMag), 0f); float penetration = (p.Depth - this.Manager.LinearErrorTolerance); if (restitution < this.Manager.MinRestitution) { if (penetration > 0f) { p.Target = penetration * this.Manager.PenetrationBias; } else { float scale = MathHelper.Clamp(-0.1f * penetration / this.Manager.LinearErrorTolerance, Constants.Epsilon, 1f); p.Target = scale * (p.Depth - this.Manager.LinearErrorTolerance) * this.Manager.TimeStepInverse; } } else { p.Target = Math.Max(Math.Max(penetration * this.Manager.PenetrationBias, 0f), restitution); } p.Impulse = 0f; p.TangentImpulse = 0f; p.PositionImpulse = 0f; if (isWarmStarted) { Vector3 impulse, tangentImpulse; // find the best cached point var bestPoint = new CachedContactPoint(); float bestDistance = float.MaxValue; for (int j = 0; j < cached.Points.Length; j++) { float d1, d2; Vector3.DistanceSquared(ref cached.Points[j].OffsetA, ref p.OffsetA, out d1); Vector3.DistanceSquared(ref cached.Points[j].OffsetB, ref p.OffsetB, out d2); if (d1 + d2 < bestDistance) { bestDistance = d1 + d2; bestPoint = cached.Points[j]; } } p.Impulse = bestPoint.NormalImpulse; float tangentImpulseMag = MathHelper.Clamp(-tangentDelta * p.TangentMass, 0f, _friction * p.Impulse); p.TangentImpulse = tangentImpulseMag * p.NormalMass; if (Math.Abs(p.Impulse) >= Constants.Epsilon) { Vector3.Multiply(ref _normal, p.Impulse, out impulse); Vector3.Multiply(ref p.Tangent, p.TangentImpulse, out tangentImpulse); Vector3.Add(ref impulse, ref tangentImpulse, out impulse); a.ApplyImpulse(ref impulse, ref p.OffsetA); Vector3.Negate(ref impulse, out impulse); b.ApplyImpulse(ref impulse, ref p.OffsetB); } } _points[i] = p; } // calculate an averaged contact point for help with stabilization during position correction if (_count > 2 && _count < _points.Length) { var ap = _points[_count]; ap.Depth = float.MaxValue; for (int i = 0; i < _count; i++) { var p = _points[i]; float depth; Vector3 pa, pb; Vector3.Add(ref a.World.Position, ref p.OffsetA, out pa); Vector3.Add(ref b.World.Position, ref p.OffsetB, out pb); Vector3.Add(ref ap.OffsetA, ref pa, out ap.OffsetA); Vector3.Add(ref ap.OffsetB, ref pb, out ap.OffsetB); Vector3.Subtract(ref pb, ref pa, out pa); Vector3.Dot(ref _normal, ref pa, out depth); if (depth < ap.Depth) { ap.Depth = depth; } } Vector3.Divide(ref ap.OffsetA, _count, out ap.OffsetA); Vector3.Divide(ref ap.OffsetB, _count, out ap.OffsetB); ap.NormalMass = MassProperties.EffectiveMass(ref a.MassWorld, ref b.MassWorld, ref ap.OffsetA, ref ap.OffsetB, ref _normal); ap.PositionImpulse = 0f; _points[_count] = ap; } }