//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);
}