예제 #1
0
        public int GetCacheEntry(ManifoldPoint newPoint)
        {
            //float shortestDist = GetContactBreakingThreshold() * GetContactBreakingThreshold();
            float shortestDist = GetContactBreakingThreshold();

            shortestDist *= shortestDist;
            int size         = GetNumContacts();
            int nearestPoint = -1;

            for (int i = 0; i < size; i++)
            {
                IndexedVector3 diffA           = m_pointCache[i].GetLocalPointA() - newPoint.GetLocalPointA();
                float          distToManiPoint = diffA.LengthSquared();
                if (distToManiPoint < shortestDist)
                {
                    shortestDist = distToManiPoint;
                    nearestPoint = i;
                }
            }
#if DEBUG
            if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugPersistentManifold)
            {
                BulletGlobals.g_streamWriter.WriteLine("getCacheEntry [{0}]", nearestPoint);
            }
#endif

            return(nearestPoint);
        }
예제 #2
0
        public void ClearUserCache(ref ManifoldPoint pt)
        {
            Object oldPtr = pt.m_userPersistentData;

            if (oldPtr != null)
            {
#if DEBUG
                if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugPersistentManifold)
                {
                    int occurance = 0;
                    for (int i = 0; i < m_cachedPoints; i++)
                    {
                        if (m_pointCache[i].m_userPersistentData == oldPtr)
                        {
                            occurance++;
                            if (occurance > 1)
                            {
                                BulletGlobals.g_streamWriter.WriteLine("error in clearUserCache\n");
                            }
                        }
                    }
                    //Debug.Assert(occurance<=0);
                }
#endif
                if (pt.m_userPersistentData != null && gContactDestroyedCallback != null)
                {
                    gContactDestroyedCallback.Callback(pt.m_userPersistentData);
                    pt.m_userPersistentData = null;
                }

                DebugPersistency();
            }
            BulletGlobals.ManifoldPointPool.Free(pt);
            pt = null;
        }
예제 #3
0
        public void ReplaceContactPoint(ManifoldPoint newPoint, int insertIndex)
        {
            Debug.Assert(ValidContactDistance(newPoint));


#if MAINTAIN_PERSISTENCY
            int   lifeTime               = m_pointCache[insertIndex].GetLifeTime();
            float appliedImpulse         = m_pointCache[insertIndex].m_appliedImpulse;
            float appliedLateralImpulse1 = m_pointCache[insertIndex].m_appliedImpulseLateral1;
            float appliedLateralImpulse2 = m_pointCache[insertIndex].m_appliedImpulseLateral2;

            Debug.Assert(lifeTime >= 0);
            Object cache = m_pointCache[insertIndex].GetUserPersistentData();

            BulletGlobals.ManifoldPointPool.Free(m_pointCache[insertIndex]);

            m_pointCache[insertIndex] = newPoint;

            m_pointCache[insertIndex].SetUserPersistentData(cache);
            m_pointCache[insertIndex].SetAppliedImpulse(appliedImpulse);
            m_pointCache[insertIndex].SetAppliedImpulseLateral1(appliedLateralImpulse1);
            m_pointCache[insertIndex].SetAppliedImpulseLateral2(appliedLateralImpulse2);

            m_pointCache[insertIndex].m_constraintRow[0].m_accumImpulse = appliedImpulse;
            m_pointCache[insertIndex].m_constraintRow[1].m_accumImpulse = appliedLateralImpulse1;
            m_pointCache[insertIndex].m_constraintRow[2].m_accumImpulse = appliedLateralImpulse2;

            m_pointCache[insertIndex].SetLifeTime(lifeTime);
#else
            ClearUserCache(ref m_pointCache[insertIndex]);
            m_pointCache[insertIndex] = newPoint;
#endif
        }
예제 #4
0
        /// calculated new worldspace coordinates and depth, and reject points that exceed the collision margin
        public void RefreshContactPoints(ref IndexedMatrix trA, ref IndexedMatrix trB)
        {
            if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugPersistentManifold)
            {
                MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "refreshContactPoints trA", trA._origin);
                MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "refreshContactPoints trB", trB._origin);
            }


            /// first refresh worldspace positions and distance
            int numContacts = GetNumContacts() - 1;

            for (int i = numContacts; i >= 0; i--)
            {
                ManifoldPoint  manifoldPoint = m_pointCache[i];
                IndexedVector3 pointA        = trA * manifoldPoint.GetLocalPointA();
                IndexedVector3 pointB        = trB * manifoldPoint.GetLocalPointB();

                manifoldPoint.SetPositionWorldOnA(ref pointA);
                manifoldPoint.SetPositionWorldOnB(ref pointB);

                manifoldPoint.SetDistance(IndexedVector3.Dot((manifoldPoint.GetPositionWorldOnA() - manifoldPoint.GetPositionWorldOnB()), manifoldPoint.GetNormalWorldOnB()));
                manifoldPoint.SetLifeTime(manifoldPoint.GetLifeTime() + 1);
                m_pointCache[i] = manifoldPoint;
            }

            /// then
            float          distance2d;
            IndexedVector3 projectedDifference, projectedPoint;

            for (int i = numContacts; i >= 0; i--)
            {
                ManifoldPoint manifoldPoint = m_pointCache[i];
                //contact becomes invalid when signed distance exceeds margin (projected on contactnormal direction)
                if (!ValidContactDistance(manifoldPoint))
                {
                    RemoveContactPoint(i);
                }
                else
                {
                    //contact also becomes invalid when relative movement orthogonal to normal exceeds margin
                    projectedPoint      = manifoldPoint.GetPositionWorldOnA() - manifoldPoint.GetNormalWorldOnB() * manifoldPoint.GetDistance();
                    projectedDifference = manifoldPoint.GetPositionWorldOnB() - projectedPoint;
                    distance2d          = IndexedVector3.Dot(projectedDifference, projectedDifference);
                    if (distance2d > GetContactBreakingThreshold() * GetContactBreakingThreshold())
                    {
                        RemoveContactPoint(i);
                    }
                    else
                    {
                        //contact point processed callback
                        if (gContactProcessedCallback != null)
                        {
                            gContactProcessedCallback.Callback(manifoldPoint, m_body0, m_body1);
                        }
                    }
                }
            }
            DebugPersistency();
        }
예제 #5
0
        public int AddManifoldPoint(ManifoldPoint newPoint)
        {
            Debug.Assert(ValidContactDistance(newPoint));

            int insertIndex = GetNumContacts();

            if (insertIndex == MANIFOLD_CACHE_SIZE)
            {
                if (MANIFOLD_CACHE_SIZE >= 4)
                {
                    //sort cache so best points come first, based on area
                    insertIndex = SortCachedPoints(newPoint);
                }
                else
                {
                    insertIndex = 0;
                }
                ClearUserCache(ref m_pointCache[insertIndex]);
            }
            else
            {
                m_cachedPoints++;
            }
            if (insertIndex < 0)
            {
                insertIndex = 0;
            }

            //Debug.Assert(m_pointCache[insertIndex].GetUserPersistentData() == null);
            m_pointCache[insertIndex] = newPoint;

#if DEBUG
            if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugPersistentManifold)
            {
                BulletGlobals.g_streamWriter.WriteLine("addManifoldPoint[{0}][{1}]", insertIndex, m_cachedPoints);
                MathUtil.PrintContactPoint(BulletGlobals.g_streamWriter, newPoint);
            }
#endif

            return(insertIndex);
        }
		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,"SetupFriction-rb0");
						}
						if (rb1 != null)
						{
                            rb1.InternalApplyImpulse(frictionConstraint1.m_contactNormal * rb1.GetInvMass(), -frictionConstraint1.m_angularComponentB, -frictionConstraint1.m_appliedImpulse, "SetupFriction-rb1");
						}
					}
					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,"SetFriction-rb0");
						}
						if (rb1 != null)
						{
							rb1.InternalApplyImpulse(frictionConstraint2.m_contactNormal * rb1.GetInvMass(), -frictionConstraint2.m_angularComponentB, -frictionConstraint2.m_appliedImpulse,"SetFriction-rb1");
						}
					}
					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;

			}
		}
		public void SetupFrictionConstraint(ref SolverConstraint solverConstraint, ref IndexedVector3 normalAxis, RigidBody solverBodyA, RigidBody solverBodyB,
						ManifoldPoint cp, ref IndexedVector3 rel_pos1, ref IndexedVector3 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);
		}
		public SolverConstraint AddFrictionConstraint(ref IndexedVector3 normalAxis, RigidBody solverBodyA, RigidBody solverBodyB, int frictionIndex, ManifoldPoint cp, ref IndexedVector3 rel_pos1, ref IndexedVector3 rel_pos2, CollisionObject colObj0, CollisionObject colObj1, float relaxation, float desiredVelocity, float cfmSlip)
		{
            // will create the object
            SolverConstraint solverConstraint = m_tmpSolverContactFrictionConstraintPool[m_tmpSolverContactFrictionConstraintPool.Count];
            solverConstraint.Reset();
			solverConstraint.m_frictionIndex = frictionIndex;
			SetupFrictionConstraint(ref solverConstraint, ref normalAxis, solverBodyA, solverBodyB, cp, ref rel_pos1, ref rel_pos2,
									colObj0, colObj1, relaxation, desiredVelocity, cfmSlip);
			return solverConstraint;
		}
		protected void SetupContactConstraint(ref SolverConstraint solverConstraint, CollisionObject colObj0, CollisionObject colObj1, ManifoldPoint cp,
								ContactSolverInfo infoGlobal, ref IndexedVector3 vel, ref float rel_vel, ref float relaxation,
								out IndexedVector3 rel_pos1, out IndexedVector3 rel_pos2)
		{
            RigidBody rb0 = colObj0 as RigidBody;// RigidBody.Upcast(colObj0);
            RigidBody rb1 = colObj1 as RigidBody;//RigidBody.Upcast(colObj1);

			IndexedVector3 pos1 = cp.GetPositionWorldOnA();
			IndexedVector3 pos2 = cp.GetPositionWorldOnB();

			rel_pos1 = pos1 - colObj0.m_worldTransform._origin;
			rel_pos2 = pos2 - colObj1.m_worldTransform._origin;

			relaxation = 1f;

            // cross

            IndexedVector3 torqueAxis0 = new IndexedVector3(rel_pos1.Y *cp.m_normalWorldOnB.Z - rel_pos1.Z * cp.m_normalWorldOnB.Y,
                rel_pos1.Z *cp.m_normalWorldOnB.X - rel_pos1.X * cp.m_normalWorldOnB.Z,
                rel_pos1.X *cp.m_normalWorldOnB.Y - rel_pos1.Y * cp.m_normalWorldOnB.X);

            IndexedVector3 torqueAxis1 = new IndexedVector3(rel_pos2.Y *cp.m_normalWorldOnB.Z - rel_pos2.Z * cp.m_normalWorldOnB.Y,
                rel_pos2.Z *cp.m_normalWorldOnB.X - rel_pos2.X * cp.m_normalWorldOnB.Z,
                rel_pos2.X *cp.m_normalWorldOnB.Y - rel_pos2.Y * cp.m_normalWorldOnB.X);


            solverConstraint.m_angularComponentA = rb0 != null ? rb0.GetInvInertiaTensorWorld() * torqueAxis0 * rb0.GetAngularFactor() : IndexedVector3.Zero;
            //IndexedVector3 torqueAxis1 = IndexedVector3.Cross(ref rel_pos2, ref cp.m_normalWorldOnB);
            solverConstraint.m_angularComponentB = rb1 != null ? rb1.GetInvInertiaTensorWorld() * -torqueAxis1 * rb1.GetAngularFactor() : IndexedVector3.Zero;


            //IndexedVector3 torqueAxis0 = IndexedVector3.Cross(ref rel_pos1, ref cp.m_normalWorldOnB);
            //solverConstraint.m_angularComponentA = rb0 != null ? rb0.GetInvInertiaTensorWorld() * torqueAxis0 * rb0.GetAngularFactor() : IndexedVector3.Zero;
            //IndexedVector3 torqueAxis1 = IndexedVector3.Cross(ref rel_pos2, ref cp.m_normalWorldOnB);
            //solverConstraint.m_angularComponentB = rb1 != null ? rb1.GetInvInertiaTensorWorld() * -torqueAxis1 * rb1.GetAngularFactor() : IndexedVector3.Zero;

			{
#if COMPUTE_IMPULSE_DENOM
		        float denom0 = rb0.computeImpulseDenominator(pos1,cp.m_normalWorldOnB);
		        float denom1 = rb1.computeImpulseDenominator(pos2,cp.m_normalWorldOnB);
#else
				IndexedVector3 vec;
				float denom0 = 0f;
				float denom1 = 0f;
				if (rb0 != null)
				{
					vec = IndexedVector3.Cross(ref solverConstraint.m_angularComponentA, ref rel_pos1);
                    denom0 = rb0.GetInvMass() + IndexedVector3.Dot(cp.m_normalWorldOnB, vec);
				}
				if (rb1 != null)
				{
					vec = IndexedVector3.Cross((-solverConstraint.m_angularComponentB), rel_pos2);
                    denom1 = rb1.GetInvMass() + IndexedVector3.Dot(cp.m_normalWorldOnB, 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 = IndexedVector3.Cross(rel_pos1, cp.m_normalWorldOnB);
			solverConstraint.m_relpos2CrossNormal = IndexedVector3.Cross(rel_pos2, -cp.m_normalWorldOnB);



			IndexedVector3 vel1 = rb0 != null ? rb0.GetVelocityInLocalPoint(ref rel_pos1) : IndexedVector3.Zero;
			IndexedVector3 vel2 = rb1 != null ? rb1.GetVelocityInLocalPoint(ref rel_pos2) : IndexedVector3.Zero;

			vel = vel1 - vel2;

            rel_vel = IndexedVector3.Dot(cp.m_normalWorldOnB, 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)
				{
					IndexedVector3 contactNormalTemp = solverConstraint.m_contactNormal;
                    rb0.InternalApplyImpulse(solverConstraint.m_contactNormal * rb0.GetInvMass() * rb0.GetLinearFactor(), solverConstraint.m_angularComponentA, solverConstraint.m_appliedImpulse, "SetupContactConstraint-rb0");
                }
				if (rb1 != null)
				{
                    rb1.InternalApplyImpulse(solverConstraint.m_contactNormal * rb1.GetInvMass() * rb1.GetLinearFactor(), -solverConstraint.m_angularComponentB, -solverConstraint.m_appliedImpulse,"SetupContactConstraint-rb1");
                }
			}
			else
			{
				solverConstraint.m_appliedImpulse = 0f;
			}
			solverConstraint.m_appliedPushImpulse = 0f;
			{
				float rel_vel2 = 0f;
				float vel1Dotn = IndexedVector3.Dot(solverConstraint.m_contactNormal, (rb0 != null ? rb0.GetLinearVelocity() : IndexedVector3.Zero))
					+ IndexedVector3.Dot(solverConstraint.m_relpos1CrossNormal, (rb0 != null ? rb0.GetAngularVelocity() : IndexedVector3.Zero));
				float vel2Dotn = -IndexedVector3.Dot(solverConstraint.m_contactNormal, (rb1 != null ? rb1.GetLinearVelocity() : IndexedVector3.Zero))
					+ IndexedVector3.Dot(solverConstraint.m_relpos2CrossNormal, (rb1 != null ? rb1.GetAngularVelocity() : IndexedVector3.Zero));

				rel_vel2 = vel1Dotn + vel2Dotn;

				float positionalError = 0f;

                if (rel_vel2 > 20)
                {
                    int ibreak = 0;
                }

				float velocityError = restitution - rel_vel2;// * damping;

				if (penetration > 0f)
				{
					positionalError = 0f;
					velocityError -= penetration / infoGlobal.m_timeStep;
				}
				else
				{
					positionalError = -penetration * infoGlobal.m_erp / infoGlobal.m_timeStep;
				}
				
				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 void ReplaceContactPoint(ManifoldPoint newPoint, int insertIndex)
        {
            Debug.Assert(ValidContactDistance(newPoint));


#if MAINTAIN_PERSISTENCY
            int lifeTime = m_pointCache[insertIndex].GetLifeTime();
            float appliedImpulse = m_pointCache[insertIndex].m_appliedImpulse;
            float appliedLateralImpulse1 = m_pointCache[insertIndex].m_appliedImpulseLateral1;
            float appliedLateralImpulse2 = m_pointCache[insertIndex].m_appliedImpulseLateral2;

            Debug.Assert(lifeTime >= 0);
            Object cache = m_pointCache[insertIndex].GetUserPersistentData();

            BulletGlobals.ManifoldPointPool.Free(m_pointCache[insertIndex]);

            m_pointCache[insertIndex] = newPoint;

            m_pointCache[insertIndex].SetUserPersistentData(cache);
            m_pointCache[insertIndex].SetAppliedImpulse(appliedImpulse);
            m_pointCache[insertIndex].SetAppliedImpulseLateral1(appliedLateralImpulse1);
            m_pointCache[insertIndex].SetAppliedImpulseLateral2(appliedLateralImpulse2);

            m_pointCache[insertIndex].m_constraintRow[0].m_accumImpulse = appliedImpulse;
            m_pointCache[insertIndex].m_constraintRow[1].m_accumImpulse = appliedLateralImpulse1;
            m_pointCache[insertIndex].m_constraintRow[2].m_accumImpulse = appliedLateralImpulse2;

            m_pointCache[insertIndex].SetLifeTime(lifeTime);
#else
		    ClearUserCache(ref m_pointCache[insertIndex]);
		    m_pointCache[insertIndex] = newPoint;
#endif
        }
예제 #11
0
        /// sort cached points so most isolated points come first
        private int SortCachedPoints(ManifoldPoint pt)
        {
            //calculate 4 possible cases areas, and take biggest area
            //also need to keep 'deepest'

            int maxPenetrationIndex = -1;

#if KEEP_DEEPEST_POINT
            float maxPenetration = pt.GetDistance();
            for (int i = 0; i < m_pointCache.Length; i++)
            {
                if (m_pointCache[i].GetDistance() < maxPenetration)
                {
                    maxPenetrationIndex = i;
                    maxPenetration      = m_pointCache[i].GetDistance();
                }
            }
#endif //KEEP_DEEPEST_POINT

            float res0 = 0f, res1 = 0f, res2 = 0f, res3 = 0f;
            if (maxPenetrationIndex != 0)
            {
                IndexedVector3 a0    = pt.GetLocalPointA() - m_pointCache[1].GetLocalPointA();
                IndexedVector3 b0    = m_pointCache[3].GetLocalPointA() - m_pointCache[2].GetLocalPointA();
                IndexedVector3 cross = IndexedVector3.Cross(a0, b0);
                res0 = cross.LengthSquared();
            }
            if (maxPenetrationIndex != 1)
            {
                IndexedVector3 a1    = pt.GetLocalPointA() - m_pointCache[0].GetLocalPointA();
                IndexedVector3 b1    = m_pointCache[3].GetLocalPointA() - m_pointCache[2].GetLocalPointA();
                IndexedVector3 cross = IndexedVector3.Cross(a1, b1);
                res1 = cross.LengthSquared();
            }

            if (maxPenetrationIndex != 2)
            {
                IndexedVector3 a2    = pt.GetLocalPointA() - m_pointCache[0].GetLocalPointA();
                IndexedVector3 b2    = m_pointCache[3].GetLocalPointA() - m_pointCache[1].GetLocalPointA();
                IndexedVector3 cross = IndexedVector3.Cross(a2, b2);
                res2 = cross.LengthSquared();
            }

            if (maxPenetrationIndex != 3)
            {
                IndexedVector3 a3    = pt.GetLocalPointA() - m_pointCache[0].GetLocalPointA();
                IndexedVector3 b3    = m_pointCache[2].GetLocalPointA() - m_pointCache[1].GetLocalPointA();
                IndexedVector3 cross = IndexedVector3.Cross(a3, b3);
                res3 = cross.LengthSquared();
            }

            IndexedVector4 maxvec      = new IndexedVector4(res0, res1, res2, res3);
            int            biggestarea = MathUtil.ClosestAxis(ref maxvec);

#if DEBUG
            if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugPersistentManifold)
            {
                BulletGlobals.g_streamWriter.WriteLine("sortCachedPoints [{0}]", biggestarea);
            }
#endif

            return(biggestarea);
        }
예제 #12
0
			public bool Callback(ref ManifoldPoint cp, CollisionObject colObj0, int partId0, int index0, CollisionObject colObj1, int partId1, int index1)
			{
				float friction0 = colObj0.GetFriction();
				float friction1 = colObj1.GetFriction();
				float restitution0 = colObj0.GetRestitution();
				float restitution1 = colObj1.GetRestitution();

				if ((colObj0.GetCollisionFlags() & CollisionFlags.CF_CUSTOM_MATERIAL_CALLBACK) != 0)
				{
					friction0 = 1.0f;//partId0,index0
					restitution0 = 0.0f;
				}
				if ((colObj1.GetCollisionFlags() & CollisionFlags.CF_CUSTOM_MATERIAL_CALLBACK) != 0)
				{
					if ((index1&1) != 0)
					{
						friction1 = 1.0f;//partId1,index1
					} 
					else
					{
						friction1 = 0f;
					}
					restitution1 = 0f;
				}

				cp.m_combinedFriction = CalculateCombinedFriction(friction0,friction1);
				cp.m_combinedRestitution = calculateCombinedRestitution(restitution0,restitution1);

				//this return value is currently ignored, but to be on the safe side: return false if you don't calculate friction
				return true;
			}
예제 #13
0
 public abstract float AddSingleResult(ref ManifoldPoint cp, CollisionObject colObj0, int partId0, int index0, CollisionObject colObj1, int partId1, int index1);
        public int GetCacheEntry(ManifoldPoint newPoint)
        {
            //float shortestDist = GetContactBreakingThreshold() * GetContactBreakingThreshold();
            float shortestDist = GetContactBreakingThreshold(); 
            shortestDist *= shortestDist;
            int size = GetNumContacts();
            int nearestPoint = -1;
            for (int i = 0; i < size; i++)
            {
                IndexedVector3 diffA = m_pointCache[i].GetLocalPointA() - newPoint.GetLocalPointA();
                float distToManiPoint = diffA.LengthSquared();
                if (distToManiPoint < shortestDist)
                {
                    shortestDist = distToManiPoint;
                    nearestPoint = i;
                }
            }
			if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugPersistentManifold)
            {
                BulletGlobals.g_streamWriter.WriteLine("getCacheEntry [{0}]", nearestPoint);
            }


            return nearestPoint;
        }
        public void ClearUserCache(ref ManifoldPoint pt)
        {
            Object oldPtr = pt.m_userPersistentData;
            if (oldPtr != null)
            {
                if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugPersistentManifold)
                {

                    int occurance = 0;
                    for (int i = 0; i < m_cachedPoints; i++)
                    {
                        if (m_pointCache[i].m_userPersistentData == oldPtr)
                        {
                            occurance++;
                            if (occurance > 1)
                            {
                                BulletGlobals.g_streamWriter.WriteLine("error in clearUserCache\n");
                            }
                        }
                    }
                    //Debug.Assert(occurance<=0);
                }
                if (pt.m_userPersistentData != null && gContactDestroyedCallback != null)
                {
                    gContactDestroyedCallback.Callback(pt.m_userPersistentData);
                    pt.m_userPersistentData = null;
                }

		        DebugPersistency();
            }
            BulletGlobals.ManifoldPointPool.Free(pt);
            pt = null;
        }
예제 #16
0
 public int FindContactPoint(ref ManifoldPoint unUsed, int numUnused, ref ManifoldPoint pt)
 {
     return(0);
 }
        /// sort cached points so most isolated points come first
        private int SortCachedPoints(ManifoldPoint pt)
        {
            //calculate 4 possible cases areas, and take biggest area
            //also need to keep 'deepest'

            int maxPenetrationIndex = -1;
#if KEEP_DEEPEST_POINT
            float maxPenetration = pt.GetDistance();
            for (int i = 0; i < m_pointCache.Length; i++)
            {
                if (m_pointCache[i].GetDistance() < maxPenetration)
                {
                    maxPenetrationIndex = i;
                    maxPenetration = m_pointCache[i].GetDistance();
                }
            }
#endif //KEEP_DEEPEST_POINT

            float res0 = 0f, res1 = 0f, res2 = 0f, res3 = 0f;
            if (maxPenetrationIndex != 0)
            {
                IndexedVector3 a0 = pt.GetLocalPointA() - m_pointCache[1].GetLocalPointA();
                IndexedVector3 b0 = m_pointCache[3].GetLocalPointA() - m_pointCache[2].GetLocalPointA();
                IndexedVector3 cross = IndexedVector3.Cross(a0, b0);
                res0 = cross.LengthSquared();
            }
            if (maxPenetrationIndex != 1)
            {
                IndexedVector3 a1 = pt.GetLocalPointA() - m_pointCache[0].GetLocalPointA();
                IndexedVector3 b1 = m_pointCache[3].GetLocalPointA() - m_pointCache[2].GetLocalPointA();
                IndexedVector3 cross = IndexedVector3.Cross(a1, b1);
                res1 = cross.LengthSquared();
            }

            if (maxPenetrationIndex != 2)
            {
                IndexedVector3 a2 = pt.GetLocalPointA() - m_pointCache[0].GetLocalPointA();
                IndexedVector3 b2 = m_pointCache[3].GetLocalPointA() - m_pointCache[1].GetLocalPointA();
                IndexedVector3 cross = IndexedVector3.Cross(a2, b2);
                res2 = cross.LengthSquared();
            }

            if (maxPenetrationIndex != 3)
            {
                IndexedVector3 a3 = pt.GetLocalPointA() - m_pointCache[0].GetLocalPointA();
                IndexedVector3 b3 = m_pointCache[2].GetLocalPointA() - m_pointCache[1].GetLocalPointA();
                IndexedVector3 cross = IndexedVector3.Cross(a3, b3);
                res3 = cross.LengthSquared();
            }

            IndexedVector4 maxvec = new IndexedVector4(res0, res1, res2, res3);
            int biggestarea = MathUtil.ClosestAxis(ref maxvec);

            if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugPersistentManifold)
            {
                BulletGlobals.g_streamWriter.WriteLine("sortCachedPoints [{0}]", biggestarea);
            }


            return biggestarea;

        }
 public bool ValidContactDistance(ManifoldPoint pt)
 {
     return pt.m_distance1 <= GetContactBreakingThreshold();
 }
		public void SetupFrictionConstraint(ref SolverConstraint solverConstraint, ref IndexedVector3 normalAxis, RigidBody solverBodyA, RigidBody solverBodyB,
								ManifoldPoint cp, ref IndexedVector3 rel_pos1, ref IndexedVector3 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();
#if DEBUG
            if (BulletGlobals.g_streamWriter != null && (body0 != null  || body1 != null) && BulletGlobals.debugSolver)
            {
                BulletGlobals.g_streamWriter.WriteLine("SetupFrictionConstraint[{0}][{1}]", (String)solverConstraint.m_solverBodyA.GetUserPointer(), (String)solverConstraint.m_solverBodyB.GetUserPointer());
                MathUtil.PrintContactPoint(BulletGlobals.g_streamWriter, cp);
            }
#endif

			solverConstraint.m_originalContactPoint = null;
            //solverConstraint.m_originalContactPointConstraint = null;
			solverConstraint.m_appliedImpulse = 0f;
			solverConstraint.m_appliedPushImpulse = 0f;




			{
				IndexedVector3 ftorqueAxis1 = IndexedVector3.Cross(rel_pos1, solverConstraint.m_contactNormal);
				solverConstraint.m_relpos1CrossNormal = ftorqueAxis1;
                solverConstraint.m_angularComponentA = body0 != null ? body0.GetInvInertiaTensorWorld() * ftorqueAxis1 * body0.GetAngularFactor() : IndexedVector3.Zero;
            }
			{
				IndexedVector3 ftorqueAxis1 = IndexedVector3.Cross(rel_pos2, -solverConstraint.m_contactNormal);
				solverConstraint.m_relpos2CrossNormal = ftorqueAxis1;
                solverConstraint.m_angularComponentB = body1 != null ? body1.GetInvInertiaTensorWorld() * ftorqueAxis1 * body1.GetAngularFactor() : IndexedVector3.Zero;
            }

#if COMPUTE_IMPULSE_DENOM
	        float denom0 = rb0.computeImpulseDenominator(pos1,solverConstraint.m_contactNormal);
	        float denom1 = rb1.computeImpulseDenominator(pos2,solverConstraint.m_contactNormal);
#else
			IndexedVector3 vec;
			float denom0 = 0f;
			float denom1 = 0f;
			if (body0 != null)
			{
				vec = IndexedVector3.Cross(solverConstraint.m_angularComponentA, rel_pos1);
				denom0 = body0.GetInvMass() + IndexedVector3.Dot(normalAxis, vec);
			}
			if (body1 != null)
			{
				vec = IndexedVector3.Cross(-solverConstraint.m_angularComponentB, rel_pos2);
				denom1 = body1.GetInvMass() + IndexedVector3.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 = IndexedVector3.Dot(solverConstraint.m_contactNormal, body0 != null ? body0.GetLinearVelocity() : IndexedVector3.Zero)
					+ IndexedVector3.Dot(solverConstraint.m_relpos1CrossNormal, body0 != null ? body0.GetAngularVelocity() : IndexedVector3.Zero);
				float vel2Dotn = -IndexedVector3.Dot(solverConstraint.m_contactNormal, body1 != null ? body1.GetLinearVelocity() : IndexedVector3.Zero)
					+ IndexedVector3.Dot(solverConstraint.m_relpos2CrossNormal, body1 != null ? body1.GetAngularVelocity() : IndexedVector3.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 int FindContactPoint(ref ManifoldPoint unUsed, int numUnused, ref ManifoldPoint pt)
 {
     return 0;
 }
예제 #21
0
        public virtual void AddContactPoint(ref IndexedVector3 normalOnBInWorld, ref IndexedVector3 pointInWorld, float depth)
        {
            Debug.Assert(m_manifoldPtr != null);
            //order in manifold needs to match

#if DEBUG
            if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugManifoldResult)
            {
                BulletGlobals.g_streamWriter.WriteLine("AddContactPoint depth[{0}]", depth);
                MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "normalOnBInWorld", normalOnBInWorld);
                MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "pointInWorld", pointInWorld);
            }
#endif

            //if (depth > m_manifoldPtr.GetContactProcessingThreshold())
            if (depth > m_manifoldPtr.GetContactBreakingThreshold())
            {
                return;
            }

            bool isSwapped = m_manifoldPtr.GetBody0() != m_body0;

            IndexedVector3 pointA = pointInWorld + normalOnBInWorld * depth;

            IndexedVector3 localA;
            IndexedVector3 localB;

            if (isSwapped)
            {
                MathUtil.InverseTransform(ref m_rootTransB, ref pointA, out localA);
                MathUtil.InverseTransform(ref m_rootTransA, ref pointInWorld, out localB);
            }
            else
            {
                MathUtil.InverseTransform(ref m_rootTransA, ref pointA, out localA);
                MathUtil.InverseTransform(ref m_rootTransB, ref pointInWorld, out localB);
            }

            ManifoldPoint newPt = BulletGlobals.ManifoldPointPool.Get();
            newPt.Initialise(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;
            }

#if DEBUG
            if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugManifoldResult)
            {
                MathUtil.PrintContactPoint(BulletGlobals.g_streamWriter, newPt);
            }
#endif

            //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);
                ManifoldPoint point = m_manifoldPtr.GetContactPoint(insertIndex);
                BulletGlobals.gContactAddedCallback.Callback(ref point, obj0, newPt.m_partId0, newPt.m_index0, obj1, newPt.m_partId1, newPt.m_index1);
            }
        }
예제 #22
0
 public bool ValidContactDistance(ManifoldPoint pt)
 {
     return(pt.m_distance1 <= GetContactBreakingThreshold());
 }
        public bool Callback(ref ManifoldPoint cp, CollisionObject colObj0, int partId0, int index0, CollisionObject colObj1, int partId1, int index1)
        {
            bool enable = true;
           	if (enable)
	        {
		        InternalEdgeUtility.AdjustInternalEdgeContacts(cp,colObj1,colObj0, partId1,index1,BulletXNA.BulletCollision.CollisionDispatch.InternalEdgeUtility.InternalEdgeAdjustFlags.BT_TRIANGLE_CONVEX_BACKFACE_NONE);
		        //btAdjustInternalEdgeContacts(cp,colObj1,colObj0, partId1,index1, BT_TRIANGLE_CONVEX_BACKFACE_MODE);
		        //btAdjustInternalEdgeContacts(cp,colObj1,colObj0, partId1,index1, BT_TRIANGLE_CONVEX_DOUBLE_SIDED+BT_TRIANGLE_CONCAVE_DOUBLE_SIDED);
	        }

	        float friction0 = colObj0.GetFriction();
	        float friction1 = colObj1.GetFriction();
	        float restitution0 = colObj0.GetRestitution();
	        float restitution1 = colObj1.GetRestitution();

	        if ((colObj0.GetCollisionFlags() & CollisionFlags.CF_CUSTOM_MATERIAL_CALLBACK) != 0)
	        {
		        friction0 = 1.0f;//partId0,index0
		        restitution0 = 0.0f;
	        }
	        if ((colObj1.GetCollisionFlags() & CollisionFlags.CF_CUSTOM_MATERIAL_CALLBACK) != 0)
	        {
		        if ((index1&1) != 0)
		        {
			        friction1 = 1.0f;//partId1,index1
		        } 
                else
		        {
			        friction1 = 0.0f;
		        }
		        restitution1 = 0.0f;
	        }

	        cp.m_combinedFriction = CalculateCombinedFriction(friction0,friction1);
	        cp.m_combinedRestitution = CalculateCombinedRestitution(restitution0,restitution1);

	        //this return value is currently ignored, but to be on the safe side: return false if you don't calculate friction
	        return true;
 
        }
        public int AddManifoldPoint(ManifoldPoint newPoint)
        {
            Debug.Assert(ValidContactDistance(newPoint));

            int insertIndex = GetNumContacts();
            if (insertIndex == MANIFOLD_CACHE_SIZE)
            {
                if (MANIFOLD_CACHE_SIZE >= 4)
                {
                    //sort cache so best points come first, based on area
                    insertIndex = SortCachedPoints(newPoint);
                }
                else
                {
                    insertIndex = 0;
                }
                ClearUserCache(ref m_pointCache[insertIndex]);

            }
            else
            {
                m_cachedPoints++;
            }
            if (insertIndex < 0)
            {
                insertIndex = 0;
            }

            //Debug.Assert(m_pointCache[insertIndex].GetUserPersistentData() == null);
            m_pointCache[insertIndex] = newPoint;


            if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugPersistentManifold)
            {
                BulletGlobals.g_streamWriter.WriteLine("addManifoldPoint[{0}][{1}]",insertIndex,m_cachedPoints);
                MathUtil.PrintContactPoint(BulletGlobals.g_streamWriter, newPoint);
            }


            return insertIndex;
        }