protected bool RecoverFromPenetration(CollisionWorld collisionWorld) { bool penetration = false; collisionWorld.GetDispatcher().DispatchAllCollisionPairs(m_ghostObject.GetOverlappingPairCache(), collisionWorld.GetDispatchInfo(), collisionWorld.GetDispatcher()); m_currentPosition = m_ghostObject.GetWorldTransform().Translation; float maxPen = 0f; for (int i = 0; i < m_ghostObject.GetOverlappingPairCache().GetNumOverlappingPairs(); i++) { m_manifoldArray.Clear(); BroadphasePair collisionPair = m_ghostObject.GetOverlappingPairCache().GetOverlappingPairArray()[i]; if (collisionPair.m_algorithm != null) { collisionPair.m_algorithm.GetAllContactManifolds(m_manifoldArray); } for (int j=0;j<m_manifoldArray.Count;j++) { PersistentManifold manifold = m_manifoldArray[j]; float directionSign = manifold.GetBody0() == m_ghostObject ? -1f : 1f; for (int p=0;p<manifold.GetNumContacts();p++) { ManifoldPoint pt = manifold.GetContactPoint(p); if (pt.GetDistance() < 0.0f) { if (pt.GetDistance() < maxPen) { maxPen = pt.GetDistance(); m_touchingNormal = pt.GetNormalWorldOnB() * directionSign;//?? } m_currentPosition += pt.GetNormalWorldOnB() * directionSign * pt.GetDistance() * 0.2f; penetration = true; } else { //printf("touching %f\n", pt.getDistance()); } } //manifold->clearManifold(); } } Matrix newTrans = m_ghostObject.GetWorldTransform(); newTrans.Translation = m_currentPosition; m_ghostObject.SetWorldTransform(ref newTrans); // printf("m_touchingNormal = %f,%f,%f\n",m_touchingNormal[0],m_touchingNormal[1],m_touchingNormal[2]); return penetration; }
public virtual void StoreIslandActivationState(CollisionWorld collisionWorld) { int index = 0; ObjectArray<CollisionObject> list = collisionWorld.GetCollisionObjectArray(); foreach (CollisionObject collisionObject in list) { if (!collisionObject.IsStaticOrKinematicObject()) { collisionObject.SetIslandTag(m_unionFind.Find(index)); collisionObject.SetCompanionId(-1); } else { collisionObject.SetIslandTag(-1); collisionObject.SetCompanionId(-2); } index++; } }
public virtual void UpdateActivationState(CollisionWorld collisionWorld, IDispatcher dispatcher) { InitUnionFind(collisionWorld.GetCollisionObjectArray().Count); // put the index into m_controllers into m_tag { int index = 0; ObjectArray<CollisionObject> list = collisionWorld.GetCollisionObjectArray(); foreach(CollisionObject collisionObject in list) { collisionObject.SetIslandTag(index); collisionObject.SetCompanionId(-1); collisionObject.SetHitFraction(1f); index++; } } // do the union find FindUnions(dispatcher, collisionWorld); }
public SingleSweepCallback(ConvexShape castShape, ref Matrix convexFromTrans,ref Matrix convexToTrans,CollisionWorld world,ConvexResultCallback resultCallback,float allowedPenetration) { m_convexFromTrans = convexFromTrans; m_convexToTrans = convexToTrans; m_world = world; m_resultCallback = resultCallback; m_allowedCcdPenetration = allowedPenetration; m_castShape = castShape; Vector3 unnormalizedRayDir = (m_convexToTrans.Translation-m_convexFromTrans.Translation); Vector3 rayDir = unnormalizedRayDir; rayDir.Normalize(); ///what about division by zero? -. just set rayDirection[i] to INF/1e30 m_rayDirectionInverse.X = MathUtil.CompareFloat(rayDir.X ,0.0f) ? float.MaxValue : 1f / rayDir.X; m_rayDirectionInverse.Y = MathUtil.CompareFloat(rayDir.Y ,0.0f) ? float.MaxValue : 1f / rayDir.Y; m_rayDirectionInverse.Z = MathUtil.CompareFloat(rayDir.Z ,0.0f) ? float.MaxValue : 1f / rayDir.Z; m_signs[0] = m_rayDirectionInverse.X < 0.0; m_signs[1] = m_rayDirectionInverse.Y < 0.0; m_signs[2] = m_rayDirectionInverse.Z < 0.0; m_lambda_max = Vector3.Dot(rayDir,unnormalizedRayDir); }
public SingleRayCallback(ref Vector3 rayFromWorld,ref Vector3 rayToWorld,CollisionWorld world,RayResultCallback resultCallback) { m_rayFromWorld = rayFromWorld; m_rayToWorld = rayToWorld; m_world = world; m_resultCallback = resultCallback; m_rayFromTrans = Matrix.Identity; m_rayFromTrans.Translation = m_rayFromWorld; m_rayToTrans = Matrix.Identity; m_rayToTrans.Translation = m_rayToWorld; Vector3 rayDir = (rayToWorld-rayFromWorld); rayDir.Normalize(); ///what about division by zero? -. just set rayDirection[i] to INF/1e30 m_rayDirectionInverse.X = MathUtil.FuzzyZero(rayDir.X) ? float.MaxValue : 1f / rayDir.X; m_rayDirectionInverse.Y = MathUtil.FuzzyZero(rayDir.Y) ? float.MaxValue : 1f / rayDir.Y; m_rayDirectionInverse.Z = MathUtil.FuzzyZero(rayDir.Z) ? float.MaxValue : 1f / rayDir.Z; m_signs[0] = m_rayDirectionInverse.X < 0.0f; m_signs[1] = m_rayDirectionInverse.Y < 0.0f; m_signs[2] = m_rayDirectionInverse.Z < 0.0f; m_lambda_max = Vector3.Dot(rayDir,(m_rayToWorld-m_rayFromWorld)); }
public void BuildIslands(IDispatcher dispatcher, CollisionWorld collisionWorld) { //BT_PROFILE("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 && 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]; 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) { colObj0.SetActivationState(ActivationState.ISLAND_SLEEPING); } } } else { 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.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 = (CollisionObject)(manifold.GetBody0()); CollisionObject colObj1 = (CollisionObject)(manifold.GetBody1()); ///@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) { colObj1.Activate(); } if (colObj1.IsKinematicObject() && colObj1.GetActivationState() != ActivationState.ISLAND_SLEEPING) { colObj0.Activate(); } if (m_splitIslands) { //filtering for response if (dispatcher.NeedsResponse(colObj0, colObj1)) { m_islandmanifold.Add(manifold); } } } } }
public void BuildAndProcessIslands(IDispatcher dispatcher, CollisionWorld collisionWorld, IIslandCallback callback) { ObjectArray<CollisionObject> collisionObjects = collisionWorld.GetCollisionObjectArray(); BuildIslands(dispatcher,collisionWorld); int endIslandIndex=1; int startIslandIndex; int numElem = GetUnionFind().GetNumElements(); //BT_PROFILE("processIslands"); if(!m_splitIslands) { ObjectArray<PersistentManifold> manifolds = dispatcher.GetInternalManifoldPointer(); int maxNumManifolds = dispatcher.GetNumManifolds(); callback.ProcessIsland(collisionObjects,collisionObjects.Count,manifolds,maxNumManifolds, -1); } else { // Sort manifolds, based on islands // Sort the vector using predicate and std::sort //std::sort(islandmanifold.begin(), islandmanifold.end(), btPersistentManifoldSortPredicate); int numManifolds = m_islandmanifold.Count; if (numManifolds != 0 && m_islandmanifold[0].GetNumContacts() > 0) { int ibreak = 0; } //we should do radix sort, it it much faster (O(n) instead of O (n log2(n)) //m_islandmanifold.quickSort(btPersistentManifoldSortPredicate()); //((ObjectArray<PersistentManifold>)m_islandmanifold).Sort(); m_islandmanifold.Sort(new Comparison<PersistentManifold>(PersistentManifoldSortPredicate)); //now process all active islands (sets of manifolds for now) int startManifoldIndex = 0; int endManifoldIndex = 1; //int islandId; // printf("Start Islands\n"); //traverse the simulation islands, and call the solver, unless all objects are sleeping/deactivated for ( startIslandIndex=0;startIslandIndex<numElem;startIslandIndex = endIslandIndex) { int islandId = GetUnionFind().GetElement(startIslandIndex).m_id; if (BulletGlobals.g_streamWriter != null && debugIslands) { BulletGlobals.g_streamWriter.WriteLine(String.Format("buildAndProcessIslands start[{0}] end[{1}] id[{2}]", startIslandIndex, endIslandIndex, islandId)); } bool islandSleeping = false; for (endIslandIndex = startIslandIndex;(endIslandIndex<numElem) && (GetUnionFind().GetElement(endIslandIndex).m_id == islandId);endIslandIndex++) { int i = GetUnionFind().GetElement(endIslandIndex).m_sz; CollisionObject colObj0 = collisionObjects[i]; m_islandBodies.Add(colObj0); if (!colObj0.IsActive()) { islandSleeping = true; //islandSleeping = false; } } //find the accompanying contact manifold for this islandId int numIslandManifolds = 0; PersistentManifold startManifold = null; if (startManifoldIndex<numManifolds) { int curIslandId = GetIslandId(m_islandmanifold[startManifoldIndex]); if (curIslandId == islandId) { startManifold = m_islandmanifold[startManifoldIndex]; for (endManifoldIndex = startManifoldIndex+1;(endManifoldIndex<numManifolds) && (islandId == GetIslandId(m_islandmanifold[endManifoldIndex]));endManifoldIndex++) { } /// Process the actual simulation, only if not sleeping/deactivated numIslandManifolds = endManifoldIndex-startManifoldIndex; } } if (!islandSleeping) { // pass shortedned list to callback. ObjectArray<PersistentManifold> subList = new ObjectArray<PersistentManifold>(); for(int i=0;i<numIslandManifolds;++i) { subList.Add(m_islandmanifold[startManifoldIndex+i]); } callback.ProcessIsland(m_islandBodies,m_islandBodies.Count,subList,numIslandManifolds, islandId); // printf("Island callback of size:%d bodies, %d manifolds\n",islandBodies.size(),numIslandManifolds); } if (numIslandManifolds != 0) { startManifoldIndex = endManifoldIndex; } m_islandBodies.Clear(); } } // else if(!splitIslands) }
public void FindUnions(IDispatcher dispatcher, CollisionWorld collisionWorld) { ObjectArray<BroadphasePair> broadphaseList = collisionWorld.GetPairCache().GetOverlappingPairArray(); foreach(BroadphasePair collisionPair in broadphaseList) { CollisionObject colObj0 = (CollisionObject)collisionPair.m_pProxy0.m_clientObject; CollisionObject colObj1 = (CollisionObject)collisionPair.m_pProxy1.m_clientObject; if (((colObj0 != null) && ((colObj0).MergesSimulationIslands())) && ((colObj1 != null) && ((colObj1).MergesSimulationIslands()))) { m_unionFind.Unite((colObj0).GetIslandTag(), (colObj1).GetIslandTag()); } } }
public void PlayerStep(CollisionWorld collisionWorld, float dt) { // quick check... if (!m_useWalkDirection && m_velocityTimeInterval <= 0.0) { // printf("\n"); return; // no motion } Matrix xform = m_ghostObject.GetWorldTransform(); // printf("walkDirection(%f,%f,%f)\n",walkDirection[0],walkDirection[1],walkDirection[2]); // printf("walkSpeed=%f\n",walkSpeed); StepUp(collisionWorld); if (m_useWalkDirection) { StepForwardAndStrafe(collisionWorld, ref m_walkDirection); } else { //printf(" time: %f", m_velocityTimeInterval); // still have some time left for moving! float dtMoving = (dt < m_velocityTimeInterval) ? dt : m_velocityTimeInterval; m_velocityTimeInterval -= dt; // how far will we move while we are moving? Vector3 move = m_walkDirection * dtMoving; // printf(" dtMoving: %f", dtMoving); // okay, step StepForwardAndStrafe(collisionWorld, ref move); } StepDown(collisionWorld, dt); xform.Translation = m_currentPosition; m_ghostObject.SetWorldTransform(ref xform); }
public void PreStep(CollisionWorld collisionWorld) { int numPenetrationLoops = 0; m_touchingContact = false; while (RecoverFromPenetration(collisionWorld)) { numPenetrationLoops++; m_touchingContact = true; if (numPenetrationLoops > 4) { // printf("character could not recover from penetration = %d\n", numPenetrationLoops); break; } } m_currentPosition = m_ghostObject.GetWorldTransform().Translation; m_targetPosition = m_currentPosition; }
///btActionInterface interface public virtual void UpdateAction( CollisionWorld collisionWorld,float deltaTime) { PreStep ( collisionWorld); PlayerStep (collisionWorld, deltaTime); }
protected void StepDown(CollisionWorld collisionWorld, float dt) { Matrix start= Matrix.Identity, end = Matrix.Identity; // phase 3: down Vector3 step_drop = upAxisDirection[m_upAxis] * m_currentStepOffset; Vector3 gravity_drop = upAxisDirection[m_upAxis] * m_stepHeight; m_targetPosition -= (step_drop + gravity_drop); start.Translation = m_currentPosition; end.Translation = m_targetPosition; KinematicClosestNotMeConvexResultCallback callback = new KinematicClosestNotMeConvexResultCallback(m_ghostObject); callback.m_collisionFilterGroup = GetGhostObject().GetBroadphaseHandle().m_collisionFilterGroup; callback.m_collisionFilterMask = GetGhostObject().GetBroadphaseHandle().m_collisionFilterMask; if (m_useGhostObjectSweepTest) { m_ghostObject.ConvexSweepTest (m_convexShape, ref start, ref end, callback, collisionWorld.GetDispatchInfo().GetAllowedCcdPenetration()); } else { collisionWorld.ConvexSweepTest(m_convexShape, start, end, callback, collisionWorld.GetDispatchInfo().GetAllowedCcdPenetration()); } if (callback.hasHit()) { // we dropped a fraction of the height -> hit floor m_currentPosition = MathUtil.Interpolate3(ref m_currentPosition, ref m_targetPosition, callback.m_closestHitFraction); } else { // we dropped the full height m_currentPosition = m_targetPosition; } }
protected void StepForwardAndStrafe(CollisionWorld collisionWorld, ref Vector3 walkMove) { // printf("originalDir=%f,%f,%f\n",originalDir[0],originalDir[1],originalDir[2]); // phase 2: forward and strafe Matrix start = Matrix.Identity, end = Matrix.Identity; m_targetPosition = m_currentPosition + walkMove; float fraction = 1.0f; float distance2 = (m_currentPosition-m_targetPosition).LengthSquared(); // printf("distance2=%f\n",distance2); if (m_touchingContact) { if (Vector3.Dot(m_normalizedDirection,m_touchingNormal) > 0.0f) { UpdateTargetPositionBasedOnCollision (ref m_touchingNormal,0.0f,1.0f); } } int maxIter = 10; while (fraction > 0.01f && maxIter-- > 0) { start.Translation = (m_currentPosition); end.Translation = (m_targetPosition); KinematicClosestNotMeConvexResultCallback callback = new KinematicClosestNotMeConvexResultCallback(m_ghostObject); callback.m_collisionFilterGroup = GetGhostObject().GetBroadphaseHandle().m_collisionFilterGroup; callback.m_collisionFilterMask = GetGhostObject().GetBroadphaseHandle().m_collisionFilterMask; float margin = m_convexShape.Margin; m_convexShape.Margin = margin + m_addedMargin; if (m_useGhostObjectSweepTest) { m_ghostObject.ConvexSweepTest (m_convexShape, ref start, ref end, callback, collisionWorld.GetDispatchInfo().GetAllowedCcdPenetration()); } else { collisionWorld.ConvexSweepTest (m_convexShape, ref start, ref end, callback, collisionWorld.GetDispatchInfo().GetAllowedCcdPenetration()); } m_convexShape.Margin = margin; fraction -= callback.m_closestHitFraction; if (callback.hasHit()) { // we moved only a fraction float hitDistance = (callback.m_hitPointWorld - m_currentPosition).Length(); if (hitDistance<0.0f) { // printf("neg dist?\n"); } /* If the distance is farther than the collision margin, move */ if (hitDistance > m_addedMargin) { // printf("callback.m_closestHitFraction=%f\n",callback.m_closestHitFraction); m_currentPosition = MathUtil.Interpolate3(ref m_currentPosition, ref m_targetPosition, callback.m_closestHitFraction); } UpdateTargetPositionBasedOnCollision (ref callback.m_hitNormalWorld,0f,1f); Vector3 currentDir = m_targetPosition - m_currentPosition; distance2 = currentDir.LengthSquared(); if (distance2 > MathUtil.SIMD_EPSILON) { currentDir.Normalize(); /* See Quake2: "If velocity is against original velocity, stop ead to avoid tiny oscilations in sloping corners." */ if (Vector3.Dot(currentDir, m_normalizedDirection) <= 0.0f) { break; } } else { // printf("currentDir: don't normalize a zero vector\n"); break; } } else { // we moved whole way m_currentPosition = m_targetPosition; } // if (callback.m_closestHitFraction == 0.f) // break; } }
protected void StepUp(CollisionWorld collisionWorld) { // phase 1: up Matrix start = Matrix.Identity, end = Matrix.Identity; m_targetPosition = m_currentPosition + upAxisDirection[m_upAxis] * m_stepHeight; /* FIXME: Handle penetration properly */ start.Translation = (m_currentPosition + upAxisDirection[m_upAxis] * 0.1f); end.Translation = m_targetPosition; KinematicClosestNotMeConvexResultCallback callback = new KinematicClosestNotMeConvexResultCallback(m_ghostObject); callback.m_collisionFilterGroup = GetGhostObject().GetBroadphaseHandle().m_collisionFilterGroup; callback.m_collisionFilterMask = GetGhostObject().GetBroadphaseHandle().m_collisionFilterMask; if (m_useGhostObjectSweepTest) { m_ghostObject.ConvexSweepTest(m_convexShape, ref start, ref end, callback, collisionWorld.GetDispatchInfo().GetAllowedCcdPenetration()); } else { collisionWorld.ConvexSweepTest(m_convexShape, ref start, ref end, callback,0f); } if (callback.hasHit()) { // we moved up only a fraction of the step height m_currentStepOffset = m_stepHeight * callback.m_closestHitFraction; m_currentPosition = MathUtil.Interpolate3(ref m_currentPosition, ref m_targetPosition, callback.m_closestHitFraction); } else { m_currentStepOffset = m_stepHeight; m_currentPosition = m_targetPosition; } }
///btActionInterface interface public virtual void UpdateAction(CollisionWorld collisionWorld, float step) { UpdateVehicle(step); }
public SingleContactCallback(CollisionObject collisionObject, CollisionWorld world,ContactResultCallback resultCallback) { m_collisionObject = collisionObject; m_world = world; m_resultCallback = resultCallback; }