public void UpdateWheel(RigidBody chassis, RaycastInfo raycastInfo) { if (raycastInfo.IsInContact) { float project = Vector3.Dot(raycastInfo.ContactNormalWS, raycastInfo.WheelDirectionWS); Vector3 chassis_velocity_at_contactPoint; Vector3 relpos = raycastInfo.ContactPointWS - chassis.CenterOfMassPosition; chassis_velocity_at_contactPoint = chassis.GetVelocityInLocalPoint(relpos); float projVel = Vector3.Dot(raycastInfo.ContactNormalWS, chassis_velocity_at_contactPoint); if (project >= -0.1f) { SuspensionRelativeVelocity = 0; ClippedInvContactDotSuspension = 1.0f / 0.1f; } else { float inv = -1.0f / project; SuspensionRelativeVelocity = projVel * inv; ClippedInvContactDotSuspension = inv; } } else // Not in contact : position wheel in a nice (rest length) position { RaycastInfo.SuspensionLength = SuspensionRestLength; SuspensionRelativeVelocity = 0; RaycastInfo.ContactNormalWS = -raycastInfo.WheelDirectionWS; ClippedInvContactDotSuspension = 1.0f; } }
public void UpdateWheel(RigidBody chassis, RaycastInfo raycastInfo) { if (raycastInfo.IsInContact) { float project = Vector3.Dot(raycastInfo.ContactNormalWS, raycastInfo.WheelDirectionWS); Vector3 chassis_velocity_at_contactPoint; Vector3 relpos = raycastInfo.ContactPointWS - chassis.CenterOfMassPosition; chassis_velocity_at_contactPoint = chassis.GetVelocityInLocalPoint(relpos); float projVel = Vector3.Dot(raycastInfo.ContactNormalWS, chassis_velocity_at_contactPoint); if (project >= -0.1f) { SuspensionRelativeVelocity = 0; ClippedInvContactDotSuspension = 1.0f / 0.1f; } else { float inv = -1.0f / project; SuspensionRelativeVelocity = projVel * inv; ClippedInvContactDotSuspension = inv; } } else // Not in contact : position wheel in a nice (rest length) position { RaycastInfo.SuspensionLength = SuspensionRestLength; SuspensionRelativeVelocity = 0; RaycastInfo.ContactNormalWS = -raycastInfo.WheelDirectionWS; ClippedInvContactDotSuspension = 1.0f; } }
public unsafe static OpenTK.Vector3 GetVelocityInLocalPoint(this RigidBody obj, ref OpenTK.Vector3 rel_pos) { fixed(OpenTK.Vector3 *rel_posPtr = &rel_pos) { return(obj.GetVelocityInLocalPoint(ref *(BulletSharp.Math.Vector3 *)rel_posPtr).ToOpenTK()); } }
double CalcRollingFriction(RigidBody body0, RigidBody body1, Vector3 contactPosWorld, Vector3 frictionDirectionWorld, double maxImpulse) { double denom0 = body0.ComputeImpulseDenominator(contactPosWorld, frictionDirectionWorld); double denom1 = body1.ComputeImpulseDenominator(contactPosWorld, frictionDirectionWorld); const double relaxation = 1.0f; double jacDiagABInv = relaxation / (denom0 + denom1); double j1; Vector3 rel_pos1 = contactPosWorld - body0.CenterOfMassPosition; Vector3 rel_pos2 = contactPosWorld - body1.CenterOfMassPosition; Vector3 vel1 = body0.GetVelocityInLocalPoint(rel_pos1); Vector3 vel2 = body1.GetVelocityInLocalPoint(rel_pos2); Vector3 vel = vel1 - vel2; double vrel; Vector3.Dot(ref frictionDirectionWorld, ref vel, out vrel); // calculate j that moves us to zero relative velocity j1 = -vrel * jacDiagABInv; j1 = System.Math.Min(j1, maxImpulse); j1 = System.Math.Max(j1, -maxImpulse); return(j1); }
private void ResolveSingleBilateral(RigidBody body1, Vector3 pos1, RigidBody body2, Vector3 pos2, float distance, Vector3 normal, ref float impulse, float timeStep) { float normalLenSqr = normal.LengthSquared; Debug.Assert(System.Math.Abs(normalLenSqr) < 1.1f); if (normalLenSqr > 1.1f) { impulse = 0; return; } Vector3 rel_pos1 = pos1 - body1.CenterOfMassPosition; Vector3 rel_pos2 = pos2 - body2.CenterOfMassPosition; Vector3 vel1 = body1.GetVelocityInLocalPoint(rel_pos1); Vector3 vel2 = body2.GetVelocityInLocalPoint(rel_pos2); Vector3 vel = vel1 - vel2; Matrix world2A = Matrix.Transpose(body1.CenterOfMassTransform.Basis); Matrix world2B = Matrix.Transpose(body2.CenterOfMassTransform.Basis); Vector3 m_aJ = Vector3.TransformCoordinate(Vector3.Cross(rel_pos1, normal), world2A); Vector3 m_bJ = Vector3.TransformCoordinate(Vector3.Cross(rel_pos2, -normal), world2B); Vector3 m_0MinvJt = body1.InvInertiaDiagLocal * m_aJ; Vector3 m_1MinvJt = body2.InvInertiaDiagLocal * m_bJ; float dot0, dot1; Vector3.Dot(ref m_0MinvJt, ref m_aJ, out dot0); Vector3.Dot(ref m_1MinvJt, ref m_bJ, out dot1); float jacDiagAB = body1.InvMass + dot0 + body2.InvMass + dot1; float jacDiagABInv = 1.0f / jacDiagAB; float rel_vel; Vector3.Dot(ref normal, ref vel, out rel_vel); //todo: move this into proper structure const float contactDamping = 0.2f; #if ONLY_USE_LINEAR_MASS float massTerm = 1.0f / (body1.InvMass + body2.InvMass); impulse = -contactDamping * rel_vel * massTerm; #else float velocityImpulse = -contactDamping * rel_vel * jacDiagABInv; impulse = velocityImpulse; #endif }
private void ResolveSingleBilateral(RigidBody body1, Vector3 pos1, RigidBody body2, Vector3 pos2, float distance, Vector3 normal, ref float impulse, float timeStep) { float normalLenSqr = normal.LengthSquared(); Debug.Assert(Math.Abs(normalLenSqr) < 1.1f); if (normalLenSqr > 1.1f) { impulse = 0; return; } Vector3 rel_pos1 = pos1 - body1.CenterOfMassPosition; Vector3 rel_pos2 = pos2 - body2.CenterOfMassPosition; Vector3 vel1 = body1.GetVelocityInLocalPoint(rel_pos1); Vector3 vel2 = body2.GetVelocityInLocalPoint(rel_pos2); Vector3 vel = vel1 - vel2; Matrix world2A = body1.CenterOfMassTransform; world2A.Origin = Vector3.Zero; world2A = Matrix.Transpose(world2A); Matrix world2B = body2.CenterOfMassTransform; world2B.Origin = Vector3.Zero; world2B = Matrix.Transpose(world2B); Vector3 m_aJ = Vector3.TransformCoordinate(Vector3.Cross(rel_pos1, normal), world2A); Vector3 m_bJ = Vector3.TransformCoordinate(Vector3.Cross(rel_pos2, -normal), world2B); Vector3 m_0MinvJt = body1.InvInertiaDiagLocal * m_aJ; Vector3 m_1MinvJt = body2.InvInertiaDiagLocal * m_bJ; float jacDiagAB = body1.InvMass + Vector3.Dot(m_0MinvJt, m_aJ) + body2.InvMass + Vector3.Dot(m_1MinvJt, m_bJ); float jacDiagABInv = 1.0f / jacDiagAB; float rel_vel = Vector3.Dot(normal, vel); //todo: move this into proper structure const float contactDamping = 0.2f; #if ONLY_USE_LINEAR_MASS float massTerm = 1.0f / (body1.InvMass + body2.InvMass); impulse = - contactDamping * rel_vel * massTerm; #else float velocityImpulse = -contactDamping * rel_vel * jacDiagABInv; impulse = velocityImpulse; #endif }
float CalcRollingFriction(RigidBody body0, RigidBody body1, Vector3 contactPosWorld, Vector3 frictionDirectionWorld, float maxImpulse) { float denom0 = body0.ComputeImpulseDenominator(contactPosWorld, frictionDirectionWorld); float denom1 = body1.ComputeImpulseDenominator(contactPosWorld, frictionDirectionWorld); const float relaxation = 1.0f; float jacDiagABInv = relaxation / (denom0 + denom1); float j1; Vector3 rel_pos1 = contactPosWorld - body0.CenterOfMassPosition; Vector3 rel_pos2 = contactPosWorld - body1.CenterOfMassPosition; Vector3 vel1 = body0.GetVelocityInLocalPoint(rel_pos1); Vector3 vel2 = body1.GetVelocityInLocalPoint(rel_pos2); Vector3 vel = vel1 - vel2; float vrel = Vector3.Dot(frictionDirectionWorld, vel); // calculate j that moves us to zero relative velocity j1 = -vrel * jacDiagABInv; j1 = Math.Min(j1, maxImpulse); j1 = Math.Max(j1, -maxImpulse); return j1; }