Esempio n. 1
0
        public static int GetIslandId(PersistentManifold lhs)
        {
            CollisionObject rcolObj0 = lhs.GetBody0() as CollisionObject;
            int             islandId = rcolObj0.GetIslandTag();

            if (islandId >= 0)
            {
                return(islandId);
            }

            CollisionObject rcolObj1 = lhs.GetBody1() as CollisionObject;

            return(rcolObj1.GetIslandTag());
        }
		protected void ConvertContact(PersistentManifold manifold, ContactSolverInfo infoGlobal)
		{
			CollisionObject colObj0 = null, colObj1 = null;


			colObj0 = manifold.GetBody0() as CollisionObject;
			colObj1 = manifold.GetBody1() as CollisionObject;

			RigidBody solverBodyA = colObj0 as RigidBody;//RigidBody.Upcast(colObj0);
            RigidBody solverBodyB = colObj1 as RigidBody;//RigidBody.Upcast(colObj1);

			///avoid collision response between two static objects
			if ((solverBodyA == null || solverBodyA.GetInvMass() == 0f) && (solverBodyB == null || solverBodyB.GetInvMass() == 0f))
			{
				return;
			}

			for (int j = 0; j < manifold.GetNumContacts(); j++)
			{
				ManifoldPoint cp = manifold.GetContactPoint(j);
#if DEBUG				
                if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugSolver && false)
                {
                    String nameA = solverBodyA != null ? (String)solverBodyA.GetUserPointer() : "Null";
                    String nameB = solverBodyA != null ? (String)solverBodyB.GetUserPointer() : "Null";

                    BulletGlobals.g_streamWriter.WriteLine("ConvertContact [{0}][{1}][{2}][{3}][{4}]", j, nameA,nameB, cp.GetDistance() ,manifold.GetContactProcessingThreshold());
                    MathUtil.PrintContactPoint(BulletGlobals.g_streamWriter, cp);
                }
#endif

				if (cp.GetDistance() <= manifold.GetContactProcessingThreshold())
				{
					//                    IndexedVector3 pos1 = cp.getPositionWorldOnA();
					//                    IndexedVector3 pos2 = cp.getPositionWorldOnB();

					IndexedVector3 rel_pos1;
					IndexedVector3 rel_pos2;
					//;

					float relaxation = 1f;
					float rel_vel = 0f;
					IndexedVector3 vel = IndexedVector3.Zero;

					int frictionIndex = m_tmpSolverContactConstraintPool.Count;

                    // will create if needed.
                    SolverConstraint solverConstraint = m_tmpSolverContactConstraintPool[m_tmpSolverContactConstraintPool.Count];
                    solverConstraint.Reset();

                    RigidBody rb0 = solverBodyA;//RigidBody.Upcast(colObj0);
					RigidBody rb1 = solverBodyB;//RigidBody.Upcast(colObj1);
					solverConstraint.m_solverBodyA = rb0 != null ? rb0 : GetFixedBody();
					solverConstraint.m_solverBodyB = rb1 != null ? rb1 : GetFixedBody();

					solverConstraint.m_originalContactPoint = cp;

#if DEBUG
                    if (BulletGlobals.g_streamWriter != null && rb0 != null && BulletGlobals.debugSolver)
                    {
                        BulletGlobals.g_streamWriter.WriteLine("ConvertContact [{0}][{1}]", (String)solverConstraint.m_solverBodyA.GetUserPointer(), (String)solverConstraint.m_solverBodyB.GetUserPointer());
                        MathUtil.PrintContactPoint(BulletGlobals.g_streamWriter, cp);
                    }
#endif


                    SetupContactConstraint(ref solverConstraint, colObj0, colObj1, cp, infoGlobal, ref vel, ref rel_vel, ref relaxation, out rel_pos1, out rel_pos2);

#if DEBUG
					if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugSolver)
					{
						TypedConstraint.PrintSolverConstraint(BulletGlobals.g_streamWriter, solverConstraint, 99);
					}
#endif
					/////setup the friction constraints

					solverConstraint.m_frictionIndex = m_tmpSolverContactFrictionConstraintPool.Count;

					if (!(TestSolverMode(infoGlobal.m_solverMode, SolverMode.SOLVER_ENABLE_FRICTION_DIRECTION_CACHING)) || !cp.GetLateralFrictionInitialized())
					{
						cp.m_lateralFrictionDir1 = vel - cp.m_normalWorldOnB * rel_vel;
						float lat_rel_vel = cp.m_lateralFrictionDir1.LengthSquared();
						if (!TestSolverMode(infoGlobal.m_solverMode, SolverMode.SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION) && lat_rel_vel > MathUtil.SIMD_EPSILON)
						{
							cp.m_lateralFrictionDir1 /= (float)Math.Sqrt(lat_rel_vel);

							if (TestSolverMode(infoGlobal.m_solverMode, SolverMode.SOLVER_USE_2_FRICTION_DIRECTIONS))
							{
								cp.m_lateralFrictionDir2 = IndexedVector3.Cross(cp.m_lateralFrictionDir1, cp.m_normalWorldOnB);
								cp.m_lateralFrictionDir2.Normalize();//??
								ApplyAnisotropicFriction(colObj0, ref cp.m_lateralFrictionDir2);
								ApplyAnisotropicFriction(colObj1, ref cp.m_lateralFrictionDir2);
								AddFrictionConstraint(ref cp.m_lateralFrictionDir2, solverBodyA, solverBodyB, frictionIndex, cp, ref rel_pos1, ref rel_pos2, colObj0, colObj1, relaxation, 0f, 0f);
							}
							ApplyAnisotropicFriction(colObj0, ref cp.m_lateralFrictionDir1);
							ApplyAnisotropicFriction(colObj1, ref cp.m_lateralFrictionDir1);
							AddFrictionConstraint(ref cp.m_lateralFrictionDir1, solverBodyA, solverBodyB, frictionIndex, cp, ref rel_pos1, ref rel_pos2, colObj0, colObj1, relaxation, 0f, 0f);

							cp.m_lateralFrictionInitialized = true;
						}
						else
						{
							//re-calculate friction direction every frame, todo: check if this is really needed
                            IndexedVector3 temp = cp.m_normalWorldOnB;
							TransformUtil.PlaneSpace1(ref temp, out cp.m_lateralFrictionDir1, out cp.m_lateralFrictionDir2);
							if (TestSolverMode(infoGlobal.m_solverMode, SolverMode.SOLVER_USE_2_FRICTION_DIRECTIONS))
							{
								ApplyAnisotropicFriction(colObj0, ref cp.m_lateralFrictionDir2);
								ApplyAnisotropicFriction(colObj1, ref cp.m_lateralFrictionDir2);
								AddFrictionConstraint(ref cp.m_lateralFrictionDir2, solverBodyA, solverBodyB, frictionIndex, cp, ref rel_pos1, ref rel_pos2, colObj0, colObj1, relaxation, 0f, 0f);
							}
							ApplyAnisotropicFriction(colObj0, ref cp.m_lateralFrictionDir1);
							ApplyAnisotropicFriction(colObj1, ref cp.m_lateralFrictionDir1);
							AddFrictionConstraint(ref cp.m_lateralFrictionDir1, solverBodyA, solverBodyB, frictionIndex, cp, ref rel_pos1, ref rel_pos2, colObj0, colObj1, relaxation, 0f, 0f);

							cp.m_lateralFrictionInitialized = true;
						}

					}
					else
					{
						AddFrictionConstraint(ref cp.m_lateralFrictionDir1, solverBodyA, solverBodyB, frictionIndex, cp, ref rel_pos1, ref rel_pos2, colObj0, colObj1, relaxation, cp.m_contactMotion1, cp.m_contactCFM1);

						if (TestSolverMode(infoGlobal.m_solverMode, SolverMode.SOLVER_USE_2_FRICTION_DIRECTIONS))
						{
							AddFrictionConstraint(ref cp.m_lateralFrictionDir2, solverBodyA, solverBodyB, frictionIndex, cp, ref rel_pos1, ref rel_pos2, colObj0, colObj1, relaxation, cp.m_contactMotion2, cp.m_contactCFM2);
						}
					}
					SetFrictionConstraintImpulse(ref solverConstraint, rb0, rb1, cp, infoGlobal);
                    
				}
			}
		}
Esempio n. 3
0
        public void BuildIslands(IDispatcher dispatcher, CollisionWorld collisionWorld)
        {
            BulletGlobals.StartProfile("islandUnionFindAndQuickSort");

            ObjectArray <CollisionObject> collisionObjects = collisionWorld.GetCollisionObjectArray();

            m_islandmanifold.Clear();

            //we are going to sort the unionfind array, and store the element id in the size
            //afterwards, we clean unionfind, to make sure no-one uses it anymore

            GetUnionFind().sortIslands();
            int numElem = GetUnionFind().GetNumElements();

            int endIslandIndex = 1;


            //update the sleeping state for bodies, if all are sleeping
            for (int startIslandIndex = 0; startIslandIndex < numElem; startIslandIndex = endIslandIndex)
            {
                int islandId = GetUnionFind().GetElement(startIslandIndex).m_id;

                if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugIslands)
                {
                    BulletGlobals.g_streamWriter.WriteLine(String.Format("buildIslands start[{0}] end[{1}] id[{2}]", startIslandIndex, endIslandIndex, islandId));
                }

                for (endIslandIndex = startIslandIndex + 1; (endIslandIndex < numElem) && (GetUnionFind().GetElement(endIslandIndex).m_id == islandId); endIslandIndex++)
                {
                }

                //int numSleeping = 0;

                bool allSleeping = true;

                for (int idx = startIslandIndex; idx < endIslandIndex; idx++)
                {
                    int i = GetUnionFind().GetElement(idx).m_sz;

                    CollisionObject colObj0 = collisionObjects[i];
                    if ((colObj0.GetIslandTag() != islandId) && (colObj0.GetIslandTag() != -1))
                    {
                        //				printf("error in island management\n");
                    }

                    Debug.Assert((colObj0.GetIslandTag() == islandId) || (colObj0.GetIslandTag() == -1));
                    if (colObj0.GetIslandTag() == islandId)
                    {
                        if (colObj0.GetActivationState() == ActivationState.ACTIVE_TAG)
                        {
                            allSleeping = false;
                        }
                        if (colObj0.GetActivationState() == ActivationState.DISABLE_DEACTIVATION)
                        {
                            allSleeping = false;
                        }
                    }
                }


                if (allSleeping)
                {
                    for (int idx = startIslandIndex; idx < endIslandIndex; idx++)
                    {
                        int             i         = GetUnionFind().GetElement(idx).m_sz;
                        CollisionObject colObj0   = collisionObjects[i];
                        int             islandTag = colObj0.GetIslandTag();
                        if (islandTag != islandId && islandTag != -1)
                        {
                            //					printf("error in island management\n");
                        }

                        Debug.Assert((islandTag == islandId) || (islandTag == -1));

                        if (islandTag == islandId)
                        {
                            colObj0.SetActivationState(ActivationState.ISLAND_SLEEPING);
                        }
                    }
                }
                else
                {
                    for (int idx = startIslandIndex; idx < endIslandIndex; idx++)
                    {
                        int i = GetUnionFind().GetElement(idx).m_sz;

                        CollisionObject colObj0   = collisionObjects[i];
                        int             islandTag = colObj0.GetIslandTag();
                        if (islandTag != islandId && islandTag != -1)
                        {
                            //					printf("error in island management\n");
                        }

                        Debug.Assert((islandTag == islandId) || (islandTag == -1));

                        if (islandTag == islandId)
                        {
                            if (colObj0.GetActivationState() == ActivationState.ISLAND_SLEEPING)
                            {
                                colObj0.SetActivationState(ActivationState.WANTS_DEACTIVATION);
                                colObj0.SetDeactivationTime(0f);
                            }
                        }
                    }
                }
            }

            int maxNumManifolds = dispatcher.GetNumManifolds();

            //#definef SPLIT_ISLANDS 1
            //#ifdef SPLIT_ISLANDS


            //#endif //SPLIT_ISLANDS


            for (int i = 0; i < maxNumManifolds; i++)
            {
                PersistentManifold manifold = dispatcher.GetManifoldByIndexInternal(i);

                CollisionObject colObj0 = manifold.GetBody0() as CollisionObject;
                CollisionObject colObj1 = manifold.GetBody1() as CollisionObject;

                ///@todo: check sleeping conditions!
                if (((colObj0 != null) && colObj0.GetActivationState() != ActivationState.ISLAND_SLEEPING) ||
                    ((colObj1 != null) && colObj1.GetActivationState() != ActivationState.ISLAND_SLEEPING))
                {
                    //kinematic objects don't merge islands, but wake up all connected objects
                    if (colObj0.IsKinematicObject() && colObj0.GetActivationState() != ActivationState.ISLAND_SLEEPING)
                    {
                        if (colObj0.HasContactResponse())
                        {
                            colObj1.Activate();
                        }
                    }
                    if (colObj1.IsKinematicObject() && colObj1.GetActivationState() != ActivationState.ISLAND_SLEEPING)
                    {
                        if (colObj1.HasContactResponse())
                        {
                            colObj0.Activate();
                        }
                    }
                    if (m_splitIslands)
                    {
                        //filtering for response
                        if (dispatcher.NeedsResponse(colObj0, colObj1))
                        {
                            m_islandmanifold.Add(manifold);
                        }
                    }
                }
            }
            BulletGlobals.StopProfile();
        }
        private static int getIslandId(PersistentManifold lhs)
        {
            CollisionObject rcolObj0 = lhs.GetBody0() as CollisionObject;
            int islandId = rcolObj0.GetIslandTag();
            if (islandId >= 0)
                return rcolObj0.GetIslandTag();

            CollisionObject rcolObj1 = lhs.GetBody1() as CollisionObject;
            return rcolObj1.GetIslandTag();
        }