public void UpdateWheel(RigidBody chassis, ref WheelRaycastInfo raycastInfo) { if (m_raycastInfo.m_isInContact) { float project = IndexedVector3.Dot(m_raycastInfo.m_contactNormalWS, m_raycastInfo.m_wheelDirectionWS); IndexedVector3 chassis_velocity_at_contactPoint; IndexedVector3 relpos = m_raycastInfo.m_contactPointWS - chassis.GetCenterOfMassPosition(); chassis_velocity_at_contactPoint = chassis.GetVelocityInLocalPoint(ref relpos); float projVel = IndexedVector3.Dot(m_raycastInfo.m_contactNormalWS, chassis_velocity_at_contactPoint); if (project >= -0.1f) { m_suspensionRelativeVelocity = 0f; m_clippedInvContactDotSuspension = 1.0f / 0.1f; } else { float inv = -1f / project; m_suspensionRelativeVelocity = projVel * inv; m_clippedInvContactDotSuspension = inv; } } else // Not in contact : position wheel in a nice (rest length) position { m_raycastInfo.m_suspensionLength = this.GetSuspensionRestLength(); m_suspensionRelativeVelocity = 0f; m_raycastInfo.m_contactNormalWS = -m_raycastInfo.m_wheelDirectionWS; m_clippedInvContactDotSuspension = 1f; } }
///bilateral constraint between two dynamic objects ///positive distance = separation, negative distance = penetration public static void ResolveSingleBilateral(RigidBody body1, ref IndexedVector3 pos1, RigidBody body2, ref IndexedVector3 pos2, float distance, ref IndexedVector3 normal, ref float impulse, float timeStep) { float normalLenSqr = normal.LengthSquared(); Debug.Assert(Math.Abs(normalLenSqr) < 1.1f); if (normalLenSqr > 1.1f) { impulse = 0f; return; } IndexedVector3 rel_pos1 = pos1 - body1.GetCenterOfMassPosition(); IndexedVector3 rel_pos2 = pos2 - body2.GetCenterOfMassPosition(); //this jacobian entry could be re-used for all iterations IndexedVector3 vel1 = body1.GetVelocityInLocalPoint(ref rel_pos1); IndexedVector3 vel2 = body2.GetVelocityInLocalPoint(ref rel_pos2); IndexedVector3 vel = vel1 - vel2; IndexedBasisMatrix m1 = body1.GetCenterOfMassTransform()._basis.Transpose(); IndexedBasisMatrix m2 = body2.GetCenterOfMassTransform()._basis.Transpose(); JacobianEntry jac = new JacobianEntry(m1, m2, rel_pos1, rel_pos2, normal, body1.GetInvInertiaDiagLocal(), body1.GetInvMass(), body2.GetInvInertiaDiagLocal(), body2.GetInvMass()); float jacDiagAB = jac.GetDiagonal(); float jacDiagABInv = 1f / jacDiagAB; float rel_vel = jac.GetRelativeVelocity( body1.GetLinearVelocity(), body1.GetCenterOfMassTransform()._basis.Transpose() * body1.GetAngularVelocity(), body2.GetLinearVelocity(), body2.GetCenterOfMassTransform()._basis.Transpose() * body2.GetAngularVelocity()); float a = jacDiagABInv; rel_vel = normal.Dot(ref vel); //todo: move this into proper structure float contactDamping = 0.2f; if (ONLY_USE_LINEAR_MASS) { float massTerm = 1f / (body1.GetInvMass() + body2.GetInvMass()); impulse = -contactDamping * rel_vel * massTerm; } else { float velocityImpulse = -contactDamping * rel_vel * jacDiagABInv; impulse = velocityImpulse; } }
public virtual void UpdateFriction(float timeStep) { //calculate the impulse, so that the wheels don't move sidewards int numWheel = GetNumWheels(); if (numWheel == 0) { return; } //m_forwardWS.resize(numWheel); //m_axle.resize(numWheel); //m_forwardImpulse.resize(numWheel); //m_sideImpulse.resize(numWheel); int numWheelsOnGround = 0; //collapse all those loops into one! for (int i = 0; i < numWheel; i++) { WheelInfo wheelInfo = m_wheelInfo[i]; RigidBody groundObject = wheelInfo.m_raycastInfo.m_groundObject as RigidBody; if (groundObject != null) { numWheelsOnGround++; } m_sideImpulse[i] = 0f; m_forwardImpulse[i] = 0f; } if (numWheelsOnGround != 4) { int ibreak = 0; } { //foreach(WheelInfo wheelInfo in m_wheelInfo) for (int i = 0; i < numWheel; ++i) { WheelInfo wheelInfo = m_wheelInfo[i]; RigidBody groundObject = wheelInfo.m_raycastInfo.m_groundObject as RigidBody; if (groundObject != null) { IndexedMatrix wheelTrans = GetWheelTransformWS(i); IndexedBasisMatrix wheelBasis0 = wheelTrans._basis; m_axle[i] = new IndexedVector3( wheelBasis0._el0[m_indexRightAxis], wheelBasis0._el1[m_indexRightAxis], wheelBasis0._el2[m_indexRightAxis]); IndexedVector3 surfNormalWS = wheelInfo.m_raycastInfo.m_contactNormalWS; float proj = IndexedVector3.Dot(m_axle[i], surfNormalWS); m_axle[i] -= surfNormalWS * proj; m_axle[i].Normalize(); m_forwardWS[i] = IndexedVector3.Cross(surfNormalWS, m_axle[i]); m_forwardWS[i].Normalize(); IndexedVector3 tempAxle = m_axle[i]; float tempImpulse = m_sideImpulse[i]; ContactConstraint.ResolveSingleBilateral(m_chassisBody, ref wheelInfo.m_raycastInfo.m_contactPointWS, groundObject, ref wheelInfo.m_raycastInfo.m_contactPointWS, 0f, ref tempAxle, ref tempImpulse, timeStep); m_sideImpulse[i] = (tempImpulse * sideFrictionStiffness2); } } } float sideFactor = 1f; float fwdFactor = 0.5f; bool sliding = false; { for (int wheel = 0; wheel < numWheel; wheel++) { WheelInfo wheelInfo = m_wheelInfo[wheel]; RigidBody groundObject = wheelInfo.m_raycastInfo.m_groundObject as RigidBody; float rollingFriction = 0f; if (groundObject != null) { if (wheelInfo.m_engineForce != 0.0f) { rollingFriction = wheelInfo.m_engineForce * timeStep; } else { float defaultRollingFrictionImpulse = 0f; float maxImpulse = (wheelInfo.m_brake != 0f) ? wheelInfo.m_brake : defaultRollingFrictionImpulse; IndexedVector3 tempWheel = m_forwardWS[wheel]; WheelContactPoint contactPt = new WheelContactPoint(m_chassisBody, groundObject, ref wheelInfo.m_raycastInfo.m_contactPointWS, ref tempWheel, maxImpulse); m_forwardWS[wheel] = tempWheel; rollingFriction = CalcRollingFriction(contactPt); } } //switch between active rolling (throttle), braking and non-active rolling friction (no throttle/break) m_forwardImpulse[wheel] = 0f; m_wheelInfo[wheel].m_skidInfo = 1f; if (groundObject != null) { m_wheelInfo[wheel].m_skidInfo = 1f; float maximp = wheelInfo.m_wheelsSuspensionForce * timeStep * wheelInfo.m_frictionSlip; float maximpSide = maximp; float maximpSquared = maximp * maximpSide; m_forwardImpulse[wheel] = rollingFriction; //wheelInfo.m_engineForce* timeStep; float x = (m_forwardImpulse[wheel]) * fwdFactor; float y = (m_sideImpulse[wheel]) * sideFactor; float impulseSquared = (x * x + y * y); if (impulseSquared > maximpSquared) { sliding = true; float factor = (float)(maximp / Math.Sqrt(impulseSquared)); m_wheelInfo[wheel].m_skidInfo *= factor; } } } } if (sliding) { for (int wheel = 0; wheel < numWheel; wheel++) { if (m_sideImpulse[wheel] != 0f) { if (m_wheelInfo[wheel].m_skidInfo < 1f) { m_forwardImpulse[wheel] *= m_wheelInfo[wheel].m_skidInfo; m_sideImpulse[wheel] *= m_wheelInfo[wheel].m_skidInfo; } } } } // apply the impulses { for (int wheel = 0; wheel < numWheel; wheel++) { WheelInfo wheelInfo = m_wheelInfo[wheel]; IndexedVector3 rel_pos = wheelInfo.m_raycastInfo.m_contactPointWS - m_chassisBody.GetCenterOfMassPosition(); if (m_forwardImpulse[wheel] > 5f || m_sideImpulse[wheel] > 5f) { int ibreak = 0; } if (m_forwardImpulse[wheel] != 0f) { m_chassisBody.ApplyImpulse(m_forwardWS[wheel] * (m_forwardImpulse[wheel]), rel_pos); } if (m_sideImpulse[wheel] != 0f) { RigidBody groundObject = m_wheelInfo[wheel].m_raycastInfo.m_groundObject as RigidBody; IndexedVector3 rel_pos2 = wheelInfo.m_raycastInfo.m_contactPointWS - groundObject.GetCenterOfMassPosition(); IndexedVector3 sideImp = m_axle[wheel] * m_sideImpulse[wheel]; #if ROLLING_INFLUENCE_FIX // fix. It only worked if car's up was along Y - VT. IndexedVector3 vChassisWorldUp = GetRigidBody().GetCenterOfMassTransform()._basis.GetColumn(m_indexUpAxis); rel_pos -= vChassisWorldUp * (IndexedVector3.Dot(vChassisWorldUp, rel_pos) * (1.0f - wheelInfo.m_rollInfluence)); #else rel_pos[m_indexUpAxis] *= wheelInfo.m_rollInfluence; #endif m_chassisBody.ApplyImpulse(ref sideImp, ref rel_pos); //apply friction impulse on the ground IndexedVector3 temp = -sideImp; groundObject.ApplyImpulse(ref temp, ref rel_pos2); } } } }
public void UpdateWheel(RigidBody chassis, ref WheelRaycastInfo raycastInfo) { if (m_raycastInfo.m_isInContact) { float project= IndexedVector3.Dot(m_raycastInfo.m_contactNormalWS,m_raycastInfo.m_wheelDirectionWS ); IndexedVector3 chassis_velocity_at_contactPoint; IndexedVector3 relpos = m_raycastInfo.m_contactPointWS - chassis.GetCenterOfMassPosition(); chassis_velocity_at_contactPoint = chassis.GetVelocityInLocalPoint( ref relpos ); float projVel = IndexedVector3.Dot(m_raycastInfo.m_contactNormalWS,chassis_velocity_at_contactPoint ); if ( project >= -0.1f) { m_suspensionRelativeVelocity = 0f; m_clippedInvContactDotSuspension = 1.0f / 0.1f; } else { float inv = -1f / project; m_suspensionRelativeVelocity = projVel * inv; m_clippedInvContactDotSuspension = inv; } } else // Not in contact : position wheel in a nice (rest length) position { m_raycastInfo.m_suspensionLength = this.GetSuspensionRestLength(); m_suspensionRelativeVelocity = 0f; m_raycastInfo.m_contactNormalWS = -m_raycastInfo.m_wheelDirectionWS; m_clippedInvContactDotSuspension = 1f; } }
public float SolveLinearAxis( float timeStep, float jacDiagABInv, RigidBody body1, ref IndexedVector3 pointInA, RigidBody body2, ref IndexedVector3 pointInB, int limit_index, ref IndexedVector3 axis_normal_on_a, ref IndexedVector3 anchorPos) { ///find relative velocity // IndexedVector3 rel_pos1 = pointInA - body1.getCenterOfMassPosition(); // IndexedVector3 rel_pos2 = pointInB - body2.getCenterOfMassPosition(); IndexedVector3 rel_pos1 = anchorPos - body1.GetCenterOfMassPosition(); IndexedVector3 rel_pos2 = anchorPos - body2.GetCenterOfMassPosition(); IndexedVector3 vel1 = IndexedVector3.Zero; body1.InternalGetVelocityInLocalPointObsolete(ref rel_pos1, ref vel1); IndexedVector3 vel2 = IndexedVector3.Zero; ; body2.InternalGetVelocityInLocalPointObsolete(ref rel_pos2, ref vel2); IndexedVector3 vel = vel1 - vel2; float rel_vel = IndexedVector3.Dot(axis_normal_on_a, vel); /// apply displacement correction //positional error (zeroth order error) float depth = -IndexedVector3.Dot((pointInA - pointInB), axis_normal_on_a); float lo = float.MinValue; float hi = float.MaxValue; float minLimit = m_lowerLimit[limit_index]; float maxLimit = m_upperLimit[limit_index]; //handle the limits if (minLimit < maxLimit) { { if (depth > maxLimit) { depth -= maxLimit; lo = 0f; } else { if (depth < minLimit) { depth -= minLimit; hi = 0f; } else { return 0.0f; } } } } float normalImpulse = m_limitSoftness * (m_restitution * depth / timeStep - m_damping * rel_vel) * jacDiagABInv; float oldNormalImpulse = m_accumulatedImpulse[limit_index]; float sum = oldNormalImpulse + normalImpulse; m_accumulatedImpulse[limit_index] = (sum > hi ? 0f : sum < lo ? 0f : sum); normalImpulse = m_accumulatedImpulse[limit_index] - oldNormalImpulse; IndexedVector3 impulse_vector = axis_normal_on_a * normalImpulse; //body1.applyImpulse( impulse_vector, rel_pos1); //body2.applyImpulse(-impulse_vector, rel_pos2); IndexedVector3 ftorqueAxis1 = IndexedVector3.Cross(rel_pos1, axis_normal_on_a); IndexedVector3 ftorqueAxis2 = IndexedVector3.Cross(rel_pos2, axis_normal_on_a); body1.InternalApplyImpulse(axis_normal_on_a * body1.GetInvMass(), body1.GetInvInertiaTensorWorld() * ftorqueAxis1, normalImpulse, "Generic6DoF body1"); body2.InternalApplyImpulse(axis_normal_on_a * body2.GetInvMass(), body2.GetInvInertiaTensorWorld() * ftorqueAxis2, -normalImpulse, "Generic6DoF body2"); return normalImpulse; }
///bilateral constraint between two dynamic objects ///positive distance = separation, negative distance = penetration public static void ResolveSingleBilateral(RigidBody body1, ref IndexedVector3 pos1, RigidBody body2, ref IndexedVector3 pos2, float distance, ref IndexedVector3 normal, ref float impulse, float timeStep) { float normalLenSqr = normal.LengthSquared(); Debug.Assert(Math.Abs(normalLenSqr) < 1.1f); if (normalLenSqr > 1.1f) { impulse = 0f; return; } IndexedVector3 rel_pos1 = pos1 - body1.GetCenterOfMassPosition(); IndexedVector3 rel_pos2 = pos2 - body2.GetCenterOfMassPosition(); //this jacobian entry could be re-used for all iterations IndexedVector3 vel1 = body1.GetVelocityInLocalPoint(ref rel_pos1); IndexedVector3 vel2 = body2.GetVelocityInLocalPoint(ref rel_pos2); IndexedVector3 vel = vel1 - vel2; IndexedBasisMatrix m1 = body1.GetCenterOfMassTransform()._basis.Transpose(); IndexedBasisMatrix m2 = body2.GetCenterOfMassTransform()._basis.Transpose(); JacobianEntry jac = new JacobianEntry(m1, m2, rel_pos1, rel_pos2, normal, body1.GetInvInertiaDiagLocal(), body1.GetInvMass(), body2.GetInvInertiaDiagLocal(), body2.GetInvMass()); float jacDiagAB = jac.GetDiagonal(); float jacDiagABInv = 1f / jacDiagAB; float rel_vel = jac.GetRelativeVelocity( body1.GetLinearVelocity(), body1.GetCenterOfMassTransform()._basis.Transpose() * body1.GetAngularVelocity(), body2.GetLinearVelocity(), body2.GetCenterOfMassTransform()._basis.Transpose() * body2.GetAngularVelocity()); float a = jacDiagABInv; rel_vel = normal.Dot(ref vel); //todo: move this into proper structure float contactDamping = 0.2f; if (ONLY_USE_LINEAR_MASS) { float massTerm = 1f / (body1.GetInvMass() + body2.GetInvMass()); impulse = -contactDamping * rel_vel * massTerm; } else { float velocityImpulse = -contactDamping * rel_vel * jacDiagABInv; impulse = velocityImpulse; } }