public override void AddContactPoint(ref Vector3 normalOnBInWorld,ref Vector3 pointInWorld,float depth) { bool isSwapped = m_manifoldPtr.GetBody0() != m_body0; Vector3 pointA = pointInWorld + normalOnBInWorld * depth; Vector3 localA = Vector3.Zero; Vector3 localB = Vector3.Zero;; if (isSwapped) { localA = MathUtil.InverseTransform(ref m_rootTransB,ref pointA ); localB = MathUtil.InverseTransform(ref m_rootTransA,ref pointInWorld); } else { localA = MathUtil.InverseTransform(ref m_rootTransA,ref pointA ); localB = MathUtil.InverseTransform(ref m_rootTransB,ref pointInWorld); } ManifoldPoint newPt = new ManifoldPoint(ref localA,ref localB,ref normalOnBInWorld,depth); newPt.m_positionWorldOnA = pointA; newPt.m_positionWorldOnB = pointInWorld; //BP mod, store contact triangles. if (isSwapped) { newPt.m_partId0 = m_partId1; newPt.m_partId1 = m_partId0; newPt.m_index0 = m_index1; newPt.m_index1 = m_index0; } else { newPt.m_partId0 = m_partId0; newPt.m_partId1 = m_partId1; newPt.m_index0 = m_index0; newPt.m_index1 = m_index1; } //experimental feature info, for per-triangle material etc. CollisionObject obj0 = isSwapped? m_body1 : m_body0; CollisionObject obj1 = isSwapped? m_body0 : m_body1; m_resultCallback.AddSingleResult(newPt,obj0,newPt.m_partId0,newPt.m_index0,obj1,newPt.m_partId1,newPt.m_index1); }
public static void PrintContactPoint(StreamWriter streamWriter, ManifoldPoint mp) { if (streamWriter != null) { streamWriter.WriteLine("ContactPoint"); PrintVector3(streamWriter, "localPointA", mp.m_localPointA); PrintVector3(streamWriter, "localPointB", mp.m_localPointB); PrintVector3(streamWriter, "posWorldA", mp.m_positionWorldOnA); PrintVector3(streamWriter, "posWorldB", mp.m_positionWorldOnB); PrintVector3(streamWriter, "normalWorldB", mp.m_normalWorldOnB); } }
public abstract float AddSingleResult(ManifoldPoint cp, CollisionObject colObj0,int partId0,int index0,CollisionObject colObj1,int partId1,int index1);
public void SetupFrictionConstraint(ref SolverConstraint solverConstraint, ref Vector3 normalAxis, RigidBody solverBodyA, RigidBody solverBodyB, ManifoldPoint cp, ref Vector3 rel_pos1, ref Vector3 rel_pos2, CollisionObject colObj0, CollisionObject colObj1, float relaxation, float desiredVelocity, float cfmSlip) { RigidBody body0 = RigidBody.Upcast(colObj0); RigidBody body1 = RigidBody.Upcast(colObj1); solverConstraint.m_contactNormal = normalAxis; solverConstraint.m_solverBodyA = body0 != null ? body0 : GetFixedBody(); solverConstraint.m_solverBodyB = body1 != null ? body1 : GetFixedBody(); solverConstraint.m_friction = cp.GetCombinedFriction(); //solverConstraint.m_originalContactPoint = 0; solverConstraint.m_appliedImpulse = 0f; solverConstraint.m_appliedPushImpulse = 0f; { Vector3 ftorqueAxis1 = Vector3.Cross(rel_pos1, solverConstraint.m_contactNormal); solverConstraint.m_relpos1CrossNormal = ftorqueAxis1; solverConstraint.m_angularComponentA = body0 != null ? Vector3.TransformNormal(ftorqueAxis1, body0.GetInvInertiaTensorWorld()) * body0.GetAngularFactor() : Vector3.Zero; } { Vector3 ftorqueAxis1 = Vector3.Cross(rel_pos2, -solverConstraint.m_contactNormal); solverConstraint.m_relpos2CrossNormal = ftorqueAxis1; solverConstraint.m_angularComponentB = body1 != null ? Vector3.TransformNormal(ftorqueAxis1, body1.GetInvInertiaTensorWorld()) * body1.GetAngularFactor() : Vector3.Zero; } #if COMPUTE_IMPULSE_DENOM float denom0 = rb0.computeImpulseDenominator(pos1,solverConstraint.m_contactNormal); float denom1 = rb1.computeImpulseDenominator(pos2,solverConstraint.m_contactNormal); #else Vector3 vec; float denom0 = 0f; float denom1 = 0f; if (body0 != null) { vec = Vector3.Cross(solverConstraint.m_angularComponentA, rel_pos1); denom0 = body0.GetInvMass() + Vector3.Dot(normalAxis, vec); } if (body1 != null) { vec = Vector3.Cross(-solverConstraint.m_angularComponentB, rel_pos2); denom1 = body1.GetInvMass() + Vector3.Dot(normalAxis, vec); } #endif //COMPUTE_IMPULSE_DENOM float denom = relaxation / (denom0 + denom1); solverConstraint.m_jacDiagABInv = denom; MathUtil.SanityCheckFloat(solverConstraint.m_jacDiagABInv); #if _USE_JACOBIAN solverConstraint.m_jac = new JacobianEntry ( ref rel_pos1,ref rel_pos2,ref solverConstraint.m_contactNormal, body0.getInvInertiaDiagLocal(), body0.getInvMass(), body1.getInvInertiaDiagLocal(), body1.getInvMass()); #endif //_USE_JACOBIAN { float rel_vel; float vel1Dotn = Vector3.Dot(solverConstraint.m_contactNormal, body0 != null ? body0.GetLinearVelocity() : Vector3.Zero) + Vector3.Dot(solverConstraint.m_relpos1CrossNormal, body0 != null ? body0.GetAngularVelocity() : Vector3.Zero); float vel2Dotn = -Vector3.Dot(solverConstraint.m_contactNormal, body1 != null ? body1.GetLinearVelocity() : Vector3.Zero) + Vector3.Dot(solverConstraint.m_relpos2CrossNormal, body1 != null ? body1.GetAngularVelocity() : Vector3.Zero); rel_vel = vel1Dotn + vel2Dotn; //float positionalError = 0f; float velocityError = desiredVelocity - rel_vel; float damper = 1f; float velocityImpulse = (velocityError * solverConstraint.m_jacDiagABInv) * damper; solverConstraint.m_rhs = velocityImpulse; solverConstraint.m_cfm = cfmSlip; solverConstraint.m_lowerLimit = 0; solverConstraint.m_upperLimit = 1e10f; } }
public virtual void AddContactPoint(ref Vector3 normalOnBInWorld,ref Vector3 pointInWorld,float depth) { Debug.Assert(m_manifoldPtr != null); //order in manifold needs to match if (depth > m_manifoldPtr.GetContactBreakingThreshold()) { return; } bool isSwapped = m_manifoldPtr.GetBody0() != m_body0; Vector3 pointA = pointInWorld + normalOnBInWorld * depth; Vector3 localA = Vector3.Zero; Vector3 localB = Vector3.Zero; if (isSwapped) { localA = MathUtil.InverseTransform(ref m_rootTransB,ref pointA); localB = MathUtil.InverseTransform(ref m_rootTransA,ref pointInWorld); } else { localA = MathUtil.InverseTransform(ref m_rootTransA,ref pointA); localB = MathUtil.InverseTransform(ref m_rootTransB,ref pointInWorld); } ManifoldPoint newPt = new ManifoldPoint(ref localA,ref localB,ref normalOnBInWorld,depth); newPt.SetPositionWorldOnA(ref pointA); newPt.SetPositionWorldOnB(ref pointInWorld); int insertIndex = m_manifoldPtr.GetCacheEntry(newPt); newPt.SetCombinedFriction(CalculateCombinedFriction(m_body0,m_body1)); newPt.SetCombinedRestitution(CalculateCombinedRestitution(m_body0,m_body1)); //BP mod, store contact triangles. if (isSwapped) { newPt.m_partId0 = m_partId1; newPt.m_partId1 = m_partId0; newPt.m_index0 = m_index1; newPt.m_index1 = m_index0; } else { newPt.m_partId0 = m_partId0; newPt.m_partId1 = m_partId1; newPt.m_index0 = m_index0; newPt.m_index1 = m_index1; } //printf("depth=%f\n",depth); ///@todo, check this for any side effects if (insertIndex >= 0) { //const btManifoldPoint& oldPoint = m_manifoldPtr->getContactPoint(insertIndex); m_manifoldPtr.ReplaceContactPoint(newPt,insertIndex); } else { insertIndex = m_manifoldPtr.AddManifoldPoint(newPt); } //User can override friction and/or restitution if (BulletGlobals.gContactAddedCallback != null && //and if either of the two bodies requires custom material ((m_body0.GetCollisionFlags() & CollisionFlags.CF_CUSTOM_MATERIAL_CALLBACK) != 0 || (m_body1.GetCollisionFlags() & CollisionFlags.CF_CUSTOM_MATERIAL_CALLBACK)!=0)) { //experimental feature info, for per-triangle material etc. CollisionObject obj0 = isSwapped? m_body1 : m_body0; CollisionObject obj1 = isSwapped? m_body0 : m_body1; //gContactAddedCallback.callback(m_manifoldPtr.getContactPoint(insertIndex),obj0,m_partId0,m_index0,obj1,m_partId1,m_index1); BulletGlobals.gContactAddedCallback.Callback(m_manifoldPtr.GetContactPoint(insertIndex), obj0, newPt.m_partId0, newPt.m_index0, obj1, newPt.m_partId1, newPt.m_index1); } }
public void SetupFrictionConstraint(ref SolverConstraint solverConstraint, ref Vector3 normalAxis, RigidBody solverBodyA, RigidBody solverBodyB, ManifoldPoint cp, ref Vector3 rel_pos1, ref Vector3 rel_pos2, CollisionObject colObj0, CollisionObject colObj1, float relaxation) { SetupFrictionConstraint(ref solverConstraint, ref normalAxis, solverBodyA, solverBodyB, cp, ref rel_pos1, ref rel_pos2, colObj0, colObj1, relaxation, 0f, 0f); }
protected void SetFrictionConstraintImpulse(ref SolverConstraint solverConstraint, RigidBody rb0, RigidBody rb1, ManifoldPoint cp, ContactSolverInfo infoGlobal) { if (TestSolverMode(infoGlobal.m_solverMode, SolverMode.SOLVER_USE_FRICTION_WARMSTARTING)) { { SolverConstraint frictionConstraint1 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex]; if (TestSolverMode(infoGlobal.m_solverMode, SolverMode.SOLVER_USE_WARMSTARTING)) { frictionConstraint1.m_appliedImpulse = cp.m_appliedImpulseLateral1 * infoGlobal.m_warmstartingFactor; if (rb0 != null) { rb0.InternalApplyImpulse(frictionConstraint1.m_contactNormal * rb0.GetInvMass(), frictionConstraint1.m_angularComponentA, frictionConstraint1.m_appliedImpulse); } if (rb1 != null) { rb1.InternalApplyImpulse(frictionConstraint1.m_contactNormal * rb1.GetInvMass(), -frictionConstraint1.m_angularComponentB, -frictionConstraint1.m_appliedImpulse); } } else { frictionConstraint1.m_appliedImpulse = 0f; } m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex] = frictionConstraint1; } if (TestSolverMode(infoGlobal.m_solverMode, SolverMode.SOLVER_USE_2_FRICTION_DIRECTIONS)) { SolverConstraint frictionConstraint2 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex + 1]; if (TestSolverMode(infoGlobal.m_solverMode, SolverMode.SOLVER_USE_WARMSTARTING)) { frictionConstraint2.m_appliedImpulse = cp.m_appliedImpulseLateral2 * infoGlobal.m_warmstartingFactor; if (rb0 != null) { rb0.InternalApplyImpulse(frictionConstraint2.m_contactNormal * rb0.GetInvMass(), frictionConstraint2.m_angularComponentA, frictionConstraint2.m_appliedImpulse); } if (rb1 != null) { rb1.InternalApplyImpulse(frictionConstraint2.m_contactNormal * rb1.GetInvMass(), -frictionConstraint2.m_angularComponentB, -frictionConstraint2.m_appliedImpulse); } } else { frictionConstraint2.m_appliedImpulse = 0f; } m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex + 1] = frictionConstraint2; } } else { SolverConstraint frictionConstraint1 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex]; frictionConstraint1.m_appliedImpulse = 0f; if (TestSolverMode(infoGlobal.m_solverMode, SolverMode.SOLVER_USE_2_FRICTION_DIRECTIONS)) { SolverConstraint frictionConstraint2 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex + 1]; frictionConstraint2.m_appliedImpulse = 0f; m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex + 1] = frictionConstraint2; } m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex] = frictionConstraint1; } }
protected void SetupContactConstraint(ref SolverConstraint solverConstraint, CollisionObject colObj0, CollisionObject colObj1, ManifoldPoint cp, ContactSolverInfo infoGlobal, ref Vector3 vel, ref float rel_vel, ref float relaxation, ref Vector3 rel_pos1, ref Vector3 rel_pos2) { RigidBody rb0 = RigidBody.Upcast(colObj0); RigidBody rb1 = RigidBody.Upcast(colObj1); Vector3 pos1 = cp.GetPositionWorldOnA(); Vector3 pos2 = cp.GetPositionWorldOnB(); rel_pos1 = pos1 - colObj0.GetWorldTransform().Translation; rel_pos2 = pos2 - colObj1.GetWorldTransform().Translation; relaxation = 1f; Vector3 torqueAxis0 = Vector3.Cross(rel_pos1, cp.m_normalWorldOnB); solverConstraint.m_angularComponentA = rb0 != null ? Vector3.TransformNormal(torqueAxis0, rb0.GetInvInertiaTensorWorld()) * rb0.GetAngularFactor() : Vector3.Zero; Vector3 torqueAxis1 = Vector3.Cross(rel_pos2, cp.GetNormalWorldOnB()); solverConstraint.m_angularComponentB = rb1 != null ? Vector3.TransformNormal(-torqueAxis1, rb1.GetInvInertiaTensorWorld()) * rb1.GetAngularFactor() : Vector3.Zero; { #if COMPUTE_IMPULSE_DENOM float denom0 = rb0.computeImpulseDenominator(pos1,cp.m_normalWorldOnB); float denom1 = rb1.computeImpulseDenominator(pos2,cp.m_normalWorldOnB); #else Vector3 vec; float denom0 = 0f; float denom1 = 0f; if (rb0 != null) { vec = Vector3.Cross((solverConstraint.m_angularComponentA), rel_pos1); denom0 = rb0.GetInvMass() + Vector3.Dot(cp.GetNormalWorldOnB(), vec); } if (rb1 != null) { vec = Vector3.Cross((-solverConstraint.m_angularComponentB), rel_pos2); denom1 = rb1.GetInvMass() + Vector3.Dot(cp.GetNormalWorldOnB(), vec); } #endif //COMPUTE_IMPULSE_DENOM float denom = relaxation / (denom0 + denom1); MathUtil.SanityCheckFloat(denom); solverConstraint.m_jacDiagABInv = denom; } solverConstraint.m_contactNormal = cp.m_normalWorldOnB; solverConstraint.m_relpos1CrossNormal = Vector3.Cross(rel_pos1, cp.m_normalWorldOnB); solverConstraint.m_relpos2CrossNormal = Vector3.Cross(rel_pos2, -cp.m_normalWorldOnB); Vector3 vel1 = rb0 != null ? rb0.GetVelocityInLocalPoint(ref rel_pos1) : Vector3.Zero; Vector3 vel2 = rb1 != null ? rb1.GetVelocityInLocalPoint(ref rel_pos2) : Vector3.Zero; vel = vel1 - vel2; rel_vel = Vector3.Dot(cp.GetNormalWorldOnB(), vel); float penetration = cp.GetDistance() + infoGlobal.m_linearSlop; solverConstraint.m_friction = cp.GetCombinedFriction(); float restitution = 0f; if (cp.GetLifeTime() > infoGlobal.m_restingContactRestitutionThreshold) { restitution = 0f; } else { restitution = RestitutionCurve(rel_vel, cp.GetCombinedResitution()); if (restitution <= 0f) { restitution = 0f; } } ///warm starting (or zero if disabled) if (TestSolverMode(infoGlobal.m_solverMode, SolverMode.SOLVER_USE_WARMSTARTING)) { solverConstraint.m_appliedImpulse = cp.GetAppliedImpulse() * infoGlobal.m_warmstartingFactor; if (rb0 != null) { Vector3 contactNormalTemp = solverConstraint.m_contactNormal; Vector3.Multiply(ref contactNormalTemp, rb0.GetInvMass(), out contactNormalTemp); rb0.InternalApplyImpulse(ref contactNormalTemp, ref solverConstraint.m_angularComponentA, solverConstraint.m_appliedImpulse); } if (rb1 != null) { Vector3 contactNormalTemp = solverConstraint.m_contactNormal; Vector3.Multiply(ref contactNormalTemp, rb1.GetInvMass(), out contactNormalTemp); Vector3 negAngular = -solverConstraint.m_angularComponentB; rb1.InternalApplyImpulse(ref contactNormalTemp, ref negAngular, -solverConstraint.m_appliedImpulse); } } else { solverConstraint.m_appliedImpulse = 0f; } solverConstraint.m_appliedPushImpulse = 0f; { float rel_vel2 = 0f; float vel1Dotn = Vector3.Dot(solverConstraint.m_contactNormal, (rb0 != null ? rb0.GetLinearVelocity() : Vector3.Zero)) + Vector3.Dot(solverConstraint.m_relpos1CrossNormal, (rb0 != null ? rb0.GetAngularVelocity() : Vector3.Zero)); float vel2Dotn = -Vector3.Dot(solverConstraint.m_contactNormal, (rb1 != null ? rb1.GetLinearVelocity() : Vector3.Zero)) + Vector3.Dot(solverConstraint.m_relpos2CrossNormal, (rb1 != null ? rb1.GetAngularVelocity() : Vector3.Zero)); rel_vel2 = vel1Dotn + vel2Dotn; float positionalError = 0f; positionalError = -penetration * infoGlobal.m_erp / infoGlobal.m_timeStep; float velocityError = restitution - rel_vel2;// * damping; float penetrationImpulse = positionalError * solverConstraint.m_jacDiagABInv; float velocityImpulse = velocityError * solverConstraint.m_jacDiagABInv; if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold)) { //combine position and velocity into rhs solverConstraint.m_rhs = penetrationImpulse + velocityImpulse; solverConstraint.m_rhsPenetration = 0f; } else { //split position and velocity into rhs and m_rhsPenetration solverConstraint.m_rhs = velocityImpulse; solverConstraint.m_rhsPenetration = penetrationImpulse; } solverConstraint.m_cfm = 0f; solverConstraint.m_lowerLimit = 0; solverConstraint.m_upperLimit = 1e10f; } }
public SolverConstraint AddFrictionConstraint(ref Vector3 normalAxis, RigidBody solverBodyA, RigidBody solverBodyB, int frictionIndex, ManifoldPoint cp, ref Vector3 rel_pos1, ref Vector3 rel_pos2, CollisionObject colObj0, CollisionObject colObj1, float relaxation, float desiredVelocity, float cfmSlip) { SolverConstraint solverConstraint = new SolverConstraint(); solverConstraint.m_frictionIndex = frictionIndex; SetupFrictionConstraint(ref solverConstraint, ref normalAxis, solverBodyA, solverBodyB, cp, ref rel_pos1, ref rel_pos2, colObj0, colObj1, relaxation, desiredVelocity, cfmSlip); m_tmpSolverContactFrictionConstraintPool.Add(solverConstraint); return solverConstraint; }
public void Copy(ManifoldPoint other) { m_localPointA = other.m_localPointA; m_localPointB = other.m_localPointB; m_positionWorldOnA = other.m_positionWorldOnA; m_positionWorldOnB = other.m_positionWorldOnB; m_normalWorldOnB = other.m_normalWorldOnB; m_distance1 = other.m_distance1; m_combinedFriction = other.m_combinedFriction; m_partId0 = other.m_partId0; m_partId1 = other.m_partId1; m_index0 = other.m_index0; m_index1 = other.m_index1; m_userPersistentData = other.m_userPersistentData; m_appliedImpulse = other.m_appliedImpulse; m_lateralFrictionInitialized = other.m_lateralFrictionInitialized; m_appliedImpulseLateral1 = other.m_appliedImpulseLateral1; m_appliedImpulseLateral2 = other.m_appliedImpulseLateral2; m_lifeTime = other.m_lifeTime; m_lateralFrictionDir1 = other.m_lateralFrictionDir1; m_lateralFrictionDir2 = other.m_lateralFrictionDir2; }