protected bool RecoverFromPenetration(CollisionWorld collisionWorld)
{
bool penetration = false;
collisionWorld.GetDispatcher().DispatchAllCollisionPairs(m_ghostObject.GetOverlappingPairCache(), collisionWorld.GetDispatchInfo(), collisionWorld.GetDispatcher());
m_currentPosition = m_ghostObject.GetWorldTransform()._origin;
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);
float dist = pt.GetDistance();
if (dist < 0.0)
{
if (dist < maxPen)
{
maxPen = dist;
m_touchingNormal = pt.m_normalWorldOnB * directionSign;//??
}
m_currentPosition += pt.m_normalWorldOnB * directionSign * dist * 0.2f;
penetration = true;
}
else
{
//printf("touching %f\n", dist);
}
}
//manifold->clearManifold();
}
}
IndexedMatrix newTrans = m_ghostObject.GetWorldTransform();
newTrans._origin = 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 SingleContactCallback(CollisionObject collisionObject, CollisionWorld world, ContactResultCallback resultCallback)
{
m_collisionObject = collisionObject;
m_world = world;
m_resultCallback = resultCallback;
}
public void Initialize(ConvexShape castShape, ref IndexedMatrix convexFromTrans, ref IndexedMatrix 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;
IndexedVector3 unnormalizedRayDir = (m_convexToTrans._origin - m_convexFromTrans._origin);
IndexedVector3 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 = rayDir.Dot(ref unnormalizedRayDir);
}
public SingleSweepCallback() { } // for pool
public SingleSweepCallback(ConvexShape castShape, ref IndexedMatrix convexFromTrans, ref IndexedMatrix convexToTrans, CollisionWorld world, ConvexResultCallback resultCallback, float allowedPenetration)
{
Initialize(castShape, ref convexFromTrans, ref convexToTrans, world, resultCallback, allowedPenetration);
}
public void Initialize(ref IndexedVector3 rayFromWorld, ref IndexedVector3 rayToWorld, CollisionWorld world, RayResultCallback resultCallback)
{
m_rayFromWorld = rayFromWorld;
m_rayToWorld = rayToWorld;
m_world = world;
m_resultCallback = resultCallback;
m_rayFromTrans = IndexedMatrix.CreateTranslation(m_rayFromWorld);
m_rayToTrans = IndexedMatrix.CreateTranslation(m_rayToWorld);
IndexedVector3 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 = rayDir.Dot(m_rayToWorld - m_rayFromWorld);
}
public SingleRayCallback(ref IndexedVector3 rayFromWorld, ref IndexedVector3 rayToWorld, CollisionWorld world, RayResultCallback resultCallback)
{
Initialize(ref rayFromWorld,ref rayToWorld,world,resultCallback);
}
public void PlayerStep(CollisionWorld collisionWorld, float dt)
{
// quick check...
if (!m_useWalkDirection && m_velocityTimeInterval <= 0.0)
{
// printf("\n");
return; // no motion
}
m_wasOnGround = OnGround();
// Update fall velocity.
m_verticalVelocity -= m_gravity * dt;
if (m_verticalVelocity > 0.0f && m_verticalVelocity > m_jumpSpeed)
{
m_verticalVelocity = m_jumpSpeed;
}
if (m_verticalVelocity < 0.0f && Math.Abs(m_verticalVelocity) > Math.Abs(m_fallSpeed))
{
m_verticalVelocity = -Math.Abs(m_fallSpeed);
}
m_verticalOffset = m_verticalVelocity * dt;
IndexedMatrix 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?
IndexedVector3 move = m_walkDirection * dtMoving;
// printf(" dtMoving: %f", dtMoving);
// okay, step
StepForwardAndStrafe(collisionWorld, ref move);
}
StepDown(collisionWorld, dt);
xform._origin = m_currentPosition;
m_ghostObject.SetWorldTransform(ref xform);
}
///btActionInterface interface
public virtual void UpdateAction(CollisionWorld collisionWorld, float deltaTime)
{
PreStep(collisionWorld);
PlayerStep(collisionWorld, deltaTime);
}
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()._origin;
m_targetPosition = m_currentPosition;
}
protected void StepForwardAndStrafe(CollisionWorld collisionWorld, ref IndexedVector3 walkMove)
{
// printf("originalDir=%f,%f,%f\n",originalDir[0],originalDir[1],originalDir[2]);
// phase 2: forward and strafe
IndexedMatrix start = IndexedMatrix.Identity, end = IndexedMatrix.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 (IndexedVector3.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._origin = (m_currentPosition);
end._origin = (m_targetPosition);
IndexedVector3 sweepDirNegative = m_currentPosition - m_targetPosition;
KinematicClosestNotMeConvexResultCallback callback = new KinematicClosestNotMeConvexResultCallback(m_ghostObject, sweepDirNegative, 0f);
callback.m_collisionFilterGroup = GetGhostObject().GetBroadphaseHandle().m_collisionFilterGroup;
callback.m_collisionFilterMask = GetGhostObject().GetBroadphaseHandle().m_collisionFilterMask;
float margin = m_convexShape.GetMargin();
m_convexShape.SetMargin(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.SetMargin(margin);
fraction -= callback.m_closestHitFraction;
if (callback.HasHit())
{
// we moved only a fraction
float hitDistance = (callback.m_hitPointWorld - m_currentPosition).Length();
UpdateTargetPositionBasedOnCollision(ref callback.m_hitNormalWorld, 0f, 1f);
IndexedVector3 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 (IndexedVector3.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 StepDown(CollisionWorld collisionWorld, float dt)
{
IndexedMatrix start = IndexedMatrix.Identity, end = IndexedMatrix.Identity;
// phase 3: down
/*float additionalDownStep = (m_wasOnGround && !onGround()) ? m_stepHeight : 0.0;
btVector3 step_drop = getUpAxisDirections()[m_upAxis] * (m_currentStepOffset + additionalDownStep);
float downVelocity = (additionalDownStep == 0.0 && m_verticalVelocity<0.0?-m_verticalVelocity:0.0) * dt;
btVector3 gravity_drop = getUpAxisDirections()[m_upAxis] * downVelocity;
m_targetPosition -= (step_drop + gravity_drop);*/
float downVelocity = (m_verticalVelocity < 0.0f ? -m_verticalVelocity : 0.0f) * dt;
if (downVelocity > 0.0 && downVelocity < m_stepHeight
&& (m_wasOnGround || !m_wasJumping))
{
downVelocity = m_stepHeight;
}
IndexedVector3 step_drop = upAxisDirection[m_upAxis] * (m_currentStepOffset + downVelocity);
m_targetPosition -= step_drop;
start._origin = m_currentPosition;
end._origin = m_targetPosition;
KinematicClosestNotMeConvexResultCallback callback = new KinematicClosestNotMeConvexResultCallback(m_ghostObject, upAxisDirection[m_upAxis], m_maxSlopeCosine);
callback.m_collisionFilterGroup = GetGhostObject().GetBroadphaseHandle().m_collisionFilterGroup;
callback.m_collisionFilterMask = GetGhostObject().GetBroadphaseHandle().m_collisionFilterMask;
if (m_useGhostObjectSweepTest)
{
// this doesn't work....
m_ghostObject.ConvexSweepTest(m_convexShape, ref start, ref end, callback, collisionWorld.GetDispatchInfo().GetAllowedCcdPenetration());
}
else
{
// this works....
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);
m_verticalVelocity = 0.0f;
m_verticalOffset = 0.0f;
m_wasJumping = false;
}
else
{
// we dropped the full height
m_currentPosition = m_targetPosition;
}
}
protected void StepUp(CollisionWorld collisionWorld)
{
// phase 1: up
IndexedMatrix start = IndexedMatrix.Identity, end = IndexedMatrix.Identity;
m_targetPosition = m_currentPosition + upAxisDirection[m_upAxis] * (m_stepHeight + (m_verticalOffset > 0.0f ? m_verticalOffset : 0.0f));
/* FIXME: Handle penetration properly */
start._origin = (m_currentPosition + upAxisDirection[m_upAxis] * (m_convexShape.GetMargin() + m_addedMargin));
end._origin = (m_targetPosition);
KinematicClosestNotMeConvexResultCallback callback = new KinematicClosestNotMeConvexResultCallback(m_ghostObject, -upAxisDirection[m_upAxis], 0.7071f);
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())
{
// Only modify the position if the hit was a slope and not a wall or ceiling.
if (IndexedVector3.Dot(callback.m_hitNormalWorld, upAxisDirection[m_upAxis]) > 0.0)
{
// 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);
}
m_verticalVelocity = 0.0f;
m_verticalOffset = 0.0f;
}
else
{
m_currentStepOffset = m_stepHeight;
m_currentPosition = m_targetPosition;
}
}
private static void RecordCollision(CollisionWorld world, CollisionObject objA, CollisionObject objB, IndexedVector3 contact, IndexedVector3 norm)
{
IndexedVector3 contactNormal = norm;
if ((objA.GetCollisionFlags() & BulletXNA.BulletCollision.CollisionFlags.BS_WANTS_COLLISIONS) == 0 &&
(objB.GetCollisionFlags() & BulletXNA.BulletCollision.CollisionFlags.BS_WANTS_COLLISIONS) == 0)
{
return;
}
uint idA = (uint)objA.GetUserPointer();
uint idB = (uint)objB.GetUserPointer();
if (idA > idB)
{
uint temp = idA;
idA = idB;
idB = temp;
contactNormal = -contactNormal;
}
ulong collisionID = ((ulong) idA << 32) | idB;
BulletXNA.CollisionDesc cDesc = new BulletXNA.CollisionDesc()
{
aID = idA,
bID = idB,
point = contact,
normal = contactNormal
};
world.UpdatedCollisions.Add(cDesc);
m_collisionsThisFrame++;
}
public void cast (CollisionWorld cw)
{
#if USE_BT_CLOCK
frame_timer.reset ();
#endif //USE_BT_CLOCK
#if BATCH_RAYCASTER
if (gBatchRaycaster == null)
{
return;
}
gBatchRaycaster.clearRays ();
for (int i = 0; i < NUMRAYS_IN_BAR; i++)
{
gBatchRaycaster.addRay (source[i], dest[i]);
}
gBatchRaycaster.performBatchRaycast ();
for (int i = 0; i < gBatchRaycaster.getNumRays (); i++)
{
const SpuRaycastTaskWorkUnitOut& outResult = (*gBatchRaycaster)[i];
hit[i].setInterpolate3(source[i],dest[i],outResult.hitFraction);
normal[i] = outResult.hitNormal;
normal[i] = normal[i].Normalize();
}
#else
for (int i = 0; i < NUMRAYS_IN_BAR; i++)
{
using (ClosestRayResultCallback cb = BulletGlobals.ClosestRayResultCallbackPool.Get())
{
cb.Initialize(source[i], dest[i]);
cw.RayTest(ref source[i], ref dest[i], cb);
if (cb.HasHit())
{
hit[i] = cb.m_hitPointWorld;
normal[i] = cb.m_hitNormalWorld;
normal[i] = IndexedVector3.Normalize(normal[i]);
}
else
{
hit[i] = dest[i];
normal[i] = new IndexedVector3(1.0f, 0.0f, 0.0f);
}
}
}
#if USE_BT_CLOCK
ms += frame_timer.getTimeMilliseconds ();
#endif //USE_BT_CLOCK
frame_counter++;
if (frame_counter > 50)
{
min_ms = ms < min_ms ? ms : min_ms;
max_ms = ms > max_ms ? ms : max_ms;
sum_ms += ms;
sum_ms_samples++;
float mean_ms = (float)sum_ms/(float)sum_ms_samples;
//printf("%d rays in %d ms %d %d %f\n", NUMRAYS_IN_BAR * frame_counter, ms, min_ms, max_ms, mean_ms);
ms = 0;
frame_counter = 0;
}
#endif
}
public SimMotionState(CollisionWorld pWorld, uint id, IndexedMatrix starTransform, object frameUpdates)
{
m_properties = new EntityProperties()
{
ID = id,
Position = starTransform._origin,
Rotation = starTransform.GetRotation()
};
m_lastProperties = new EntityProperties()
{
ID = id,
Position = starTransform._origin,
Rotation = starTransform.GetRotation()
};
m_world = pWorld;
m_xform = starTransform;
}
///btActionInterface interface
public virtual void UpdateAction(CollisionWorld collisionWorld, float step)
{
UpdateVehicle(step);
}
