//Think this is supposed doing the move and slide Riley reffered to. protected void UpdateTargetPositionBasedOnCollision(ref Vector3 hitNormal, float tangentMag, float normalMag) { Vector3 movementDirection = targetPosition - currentPosition; float movementLength = movementDirection.Length; if (movementLength > MathUtil.SIMD_EPSILON) { movementDirection.Normalize(); Vector3 reflectDir = ComputeReflectionDirection(ref movementDirection, ref hitNormal); reflectDir.Normalize(); Vector3 perpindicularDir; perpindicularDir = PerpindicularComponent(ref reflectDir, ref hitNormal); targetPosition = currentPosition; if (normalMag != 0.0f) { Vector3 perpComponent = perpindicularDir * (normalMag * movementLength); targetPosition += perpComponent; } } }
//Collision solving. public void PreStep(CollisionWorld collisionWorld) { int numPenetrationLoops = 0; touchingContact = false; while (RecoverFromPenetration(collisionWorld)) { numPenetrationLoops++; touchingContact = true; if (numPenetrationLoops > 4) { break; } } currentPosition = ghostObject.WorldTransform.Origin; targetPosition = currentPosition; }
public void SetVelocityForTimeInterval(ref BulletSharp.Math.Vector3 velocity, float timeInterval) { }
public void Warp(ref BulletSharp.Math.Vector3 origin) { ghostObject.WorldTransform = Matrix.Translation(origin); }
public virtual void SetWalkDirection(BulletSharp.Math.Vector3 walkDirection) { SetWalkDirection(ref walkDirection); }
//? public void SetWalkDirection(ref BulletSharp.Math.Vector3 walkDirection) { //this.walkDirection = walkDirection; //normalizedDirection = walkDirection.normalized(); }
protected void StepForwardAndStrafe(CollisionWorld collisionWorld, ref Vector3 walkMove) { Matrix start = Matrix.Identity, end = Matrix.Identity; targetPosition = currentPosition + walkMove; float fraction = 1.0f; float distance2 = (currentPosition - targetPosition).LengthSquared; // printf("distance2=%f\n",distance2); if (touchingContact) { float dot; Vector3.Dot(ref normalizedDirection, ref m_touchingNormal, out dot); if (dot > 0.0f) { //interferes with step movement //UpdateTargetPositionBasedOnCollision(ref m_touchingNormal, 0.0f, 1.0f); } } int maxIter = 10; while (fraction > 0.01f && maxIter-- > 0) { start.Origin = (currentPosition); end.Origin = (targetPosition); Vector3 sweepDirNegative = currentPosition - targetPosition; KinematicClosestNotMeConvexResultCallback callback = new KinematicClosestNotMeConvexResultCallback(ghostObject, sweepDirNegative, 0f); callback.CollisionFilterGroup = ghostObject.BroadphaseHandle.CollisionFilterGroup; callback.CollisionFilterMask = ghostObject.BroadphaseHandle.CollisionFilterMask; float margin = convexShape.Margin; convexShape.Margin = margin + .02f; ghostObject.ConvexSweepTestRef(convexShape, ref start, ref end, callback, collisionWorld.DispatchInfo.AllowedCcdPenetration); convexShape.Margin = margin; fraction -= callback.ClosestHitFraction; if (callback.HasHit) { // We moved only a fraction float hitDistance = (callback.HitPointWorld - currentPosition).Length; Vector3 hitNormalWorld = callback.HitNormalWorld; UpdateTargetPositionBasedOnCollision(ref hitNormalWorld, 0f, 1f); Vector3 currentDir = targetPosition - 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." */ float dot; Vector3.Dot(ref currentDir, ref normalizedDirection, out dot); if (dot <= 0.0f) { break; } } else { break; } } else { // we moved whole way currentPosition = targetPosition; } } }
protected bool RecoverFromPenetration(CollisionWorld collisionWorld) { Vector3 minAabb, maxAabb; convexShape.GetAabb(ghostObject.WorldTransform, out minAabb, out maxAabb); collisionWorld.Broadphase.SetAabbRef(ghostObject.BroadphaseHandle, ref minAabb, ref maxAabb, collisionWorld.Dispatcher); bool penetration = false; collisionWorld.Dispatcher.DispatchAllCollisionPairs(ghostObject.OverlappingPairCache, collisionWorld.DispatchInfo, collisionWorld.Dispatcher); currentPosition = ghostObject.WorldTransform.Origin; float maxPen = 0f; for (int i = 0; i < ghostObject.OverlappingPairCache.NumOverlappingPairs; i++) { localManifoldArray.Clear(); BroadphasePair collisionPair = ghostObject.OverlappingPairCache.OverlappingPairArray[i]; CollisionObject obj0 = collisionPair.Proxy0.ClientObject as CollisionObject; CollisionObject obj1 = collisionPair.Proxy1.ClientObject as CollisionObject; if ((obj0 != null && !obj0.HasContactResponse) || (obj1 != null && !obj1.HasContactResponse)) { continue; } if (collisionPair.Algorithm != null) { collisionPair.Algorithm.GetAllContactManifolds(localManifoldArray); } for (int j = 0; j < localManifoldArray.Count; j++) { PersistentManifold manifold = localManifoldArray[j]; float directionSign = manifold.Body0 == ghostObject ? -1f : 1f; for (int p = 0; p < manifold.NumContacts; p++) { ManifoldPoint pt = manifold.GetContactPoint(p); float dist = pt.Distance; if (dist < 0.0f) { if (dist < maxPen) { maxPen = dist; m_touchingNormal = pt.NormalWorldOnB * directionSign;//?? } currentPosition += pt.NormalWorldOnB * directionSign * dist * 0.2f; penetration = true; } } } } Matrix newTrans = ghostObject.WorldTransform; newTrans.Origin = currentPosition; ghostObject.WorldTransform = newTrans; return(penetration); }