public override void SolveConstraint(float timeStep) { Vector3 pivotAInW = MathHelper.Transform(_pivotInA, RigidBodyA.CenterOfMassTransform); Vector3 pivotBInW = MathHelper.Transform(_pivotInB, RigidBodyB.CenterOfMassTransform); Vector3 normal = new Vector3(); for (int i = 0; i < 3; i++) { MathHelper.SetElement(ref normal, i, 1); float jacDiagABInv = 1.0f / _jacobian[i].Diagonal; Vector3 rel_pos1 = pivotAInW - RigidBodyA.CenterOfMassPosition; Vector3 rel_pos2 = pivotBInW - RigidBodyB.CenterOfMassPosition; Vector3 vel1 = RigidBodyA.GetVelocityInLocalPoint(rel_pos1); Vector3 vel2 = RigidBodyB.GetVelocityInLocalPoint(rel_pos2); Vector3 vel = vel1 - vel2; float rel_vel = Vector3.Dot(normal, vel); float depth = -Vector3.Dot((pivotAInW - pivotBInW), normal); float impulse = depth * _setting.Tau / timeStep * jacDiagABInv - _setting.Damping * rel_vel * jacDiagABInv; AppliedImpulse += impulse; Vector3 impulseVector = normal * impulse; RigidBodyA.ApplyImpulse(impulseVector, pivotAInW - RigidBodyA.CenterOfMassPosition); RigidBodyB.ApplyImpulse(-impulseVector, pivotBInW - RigidBodyB.CenterOfMassPosition); MathHelper.SetElement(ref normal, i, 0); } }
public override void SolveConstraint(float timeStep) { Vector3 pivotAInW = MathHelper.Transform(_pivotInA, RigidBodyA.CenterOfMassTransform); Vector3 pivotBInW = MathHelper.Transform(_pivotInB, RigidBodyB.CenterOfMassTransform); Vector3 normal = new Vector3(0, 0, 0); float tau = 0.3f; float damping = 1f; //linear part if (!_angularOnly) { for (int i = 0; i < 3; i++) { if (i == 0) { normal = new Vector3(1, 0, 0); } else if (i == 1) { normal = new Vector3(0, 1, 0); } else { normal = new Vector3(0, 0, 1); } float jacDiagABInv = 1f / _jac[i].Diagonal; Vector3 rel_pos1 = pivotAInW - RigidBodyA.CenterOfMassPosition; Vector3 rel_pos2 = pivotBInW - RigidBodyB.CenterOfMassPosition; Vector3 vel1 = RigidBodyA.GetVelocityInLocalPoint(rel_pos1); Vector3 vel2 = RigidBodyB.GetVelocityInLocalPoint(rel_pos2); Vector3 vel = vel1 - vel2; float rel_vel; rel_vel = Vector3.Dot(normal, vel); //positional error (zeroth order error) float depth = -Vector3.Dot(pivotAInW - pivotBInW, normal); //this is the error projected on the normal float impulse = depth * tau / timeStep * jacDiagABInv - damping * rel_vel * jacDiagABInv * damping; AppliedImpulse += impulse; Vector3 impulse_vector = normal * impulse; RigidBodyA.ApplyImpulse(impulse_vector, pivotAInW - RigidBodyA.CenterOfMassPosition); RigidBodyB.ApplyImpulse(-impulse_vector, pivotBInW - RigidBodyB.CenterOfMassPosition); } } //solve angular part // get axes in world space Vector3 axisA = MathHelper.TransformNormal(_axisInA, RigidBodyA.CenterOfMassTransform); Vector3 axisB = MathHelper.TransformNormal(_axisInB, RigidBodyB.CenterOfMassTransform); Vector3 angVelA = RigidBodyA.AngularVelocity; Vector3 angVelB = RigidBodyB.AngularVelocity; Vector3 angVelAroundHingeAxisA = axisA * Vector3.Dot(axisA, angVelA); Vector3 angVelAroundHingeAxisB = axisB * Vector3.Dot(axisB, angVelB); Vector3 angAOrthog = angVelA - angVelAroundHingeAxisA; Vector3 angBOrthog = angVelB - angVelAroundHingeAxisB; Vector3 velrelOrthog = angAOrthog - angBOrthog; //solve angular velocity correction float relaxation = 1f; float len = velrelOrthog.Length(); if (len > 0.00001f) { Vector3 normal2 = Vector3.Normalize(velrelOrthog); float denom = RigidBodyA.ComputeAngularImpulseDenominator(normal2) + RigidBodyB.ComputeAngularImpulseDenominator(normal2); // scale for mass and relaxation velrelOrthog *= (1f / denom) * 0.9f; } //solve angular positional correction Vector3 angularError = -Vector3.Cross(axisA, axisB) * (1f / timeStep); float len2 = angularError.Length(); if (len2 > 0.00001f) { Vector3 normal2 = Vector3.Normalize(angularError); float denom2 = RigidBodyA.ComputeAngularImpulseDenominator(normal2) + RigidBodyB.ComputeAngularImpulseDenominator(normal2); angularError *= (1f / denom2) * relaxation; } RigidBodyA.ApplyTorqueImpulse(-velrelOrthog + angularError); RigidBodyB.ApplyTorqueImpulse(velrelOrthog - angularError); //apply motor if (_enableAngularMotor) { //todo: add limits too Vector3 angularLimit = Vector3.Zero; Vector3 velrel = angVelAroundHingeAxisA - angVelAroundHingeAxisB; float projRelVel = Vector3.Dot(velrel, axisA); float desiredMotorVel = _motorTargetVelocity; float motorRelvel = desiredMotorVel - projRelVel; float denom3 = RigidBodyA.ComputeAngularImpulseDenominator(axisA) + RigidBodyB.ComputeAngularImpulseDenominator(axisA); float unclippedMotorImpulse = (1f / denom3) * motorRelvel; //todo: should clip against accumulated impulse float clippedMotorImpulse = unclippedMotorImpulse > _maxMotorImpulse ? _maxMotorImpulse : unclippedMotorImpulse; clippedMotorImpulse = clippedMotorImpulse < -_maxMotorImpulse ? -_maxMotorImpulse : clippedMotorImpulse; Vector3 motorImp = clippedMotorImpulse * axisA; RigidBodyA.ApplyTorqueImpulse(motorImp + angularLimit); RigidBodyB.ApplyTorqueImpulse(-motorImp - angularLimit); } }