protected void StepUp(CollisionWorld collisionWorld)
{
// phase 1: up
Matrix start, end;
m_targetPosition = m_currentPosition + upAxisDirection[m_upAxis] * (m_stepHeight + (m_verticalOffset > 0.0f ? m_verticalOffset : 0.0f));
/* FIXME: Handle penetration properly */
start = Matrix.Translation(m_currentPosition + upAxisDirection[m_upAxis] * (m_convexShape.Margin + m_addedMargin));
end = Matrix.Translation(m_targetPosition);
KinematicClosestNotMeConvexResultCallback callback = new KinematicClosestNotMeConvexResultCallback(m_ghostObject, -upAxisDirection[m_upAxis], 0.7071f);
callback.CollisionFilterGroup = GhostObject.BroadphaseHandle.CollisionFilterGroup;
callback.CollisionFilterMask = GhostObject.BroadphaseHandle.CollisionFilterMask;
if (m_useGhostObjectSweepTest)
{
m_ghostObject.ConvexSweepTestRef(m_convexShape, ref start, ref end, callback, collisionWorld.DispatchInfo.AllowedCcdPenetration);
}
else
{
collisionWorld.ConvexSweepTestRef(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 (Vector3.Dot(callback.HitNormalWorld, upAxisDirection[m_upAxis]) > 0.0)
{
// we moved up only a fraction of the step height
m_currentStepOffset = m_stepHeight * callback.ClosestHitFraction;
if (m_interpolateUp)
{
Vector3.Lerp(ref m_currentPosition, ref m_targetPosition, callback.ClosestHitFraction, out m_currentPosition);
}
else
{
m_currentPosition = m_targetPosition;
}
}
m_verticalVelocity = 0.0f;
m_verticalOffset = 0.0f;
}
else
{
m_currentStepOffset = m_stepHeight;
m_currentPosition = m_targetPosition;
}
}
public KinematicCharacterController(PairCachingGhostObject ghostObject, ConvexShape convexShape, float stepHeight, int upAxis = 1)
{
m_addedMargin = 0.02f;
m_walkDirection = Vector3.Zero;
m_useGhostObjectSweepTest = true;
m_ghostObject = ghostObject;
m_stepHeight = stepHeight;
m_convexShape = convexShape;
m_useWalkDirection = true; // use walk direction by default, legacy behavior
m_velocityTimeInterval = 0.0f;
m_verticalVelocity = 0.0f;
m_verticalOffset = 0.0f;
Gravity = 9.8f * 3; // 3G acceleration.
m_fallSpeed = 55.0f; // Terminal velocity of a sky diver in m/s.
m_jumpSpeed = 10.0f; // ?
m_wasOnGround = false;
m_wasJumping = false;
m_interpolateUp = true;
MaxSlope = MathUtil.DegToRadians(45.0f);
m_currentStepOffset = 0;
full_drop = false;
bounce_fix = false;
_callback = new KinematicClosestNotMeConvexResultCallback(m_ghostObject, -Vector3.UnitY, 0.7071f);
}
protected void StepDown(CollisionWorld collisionWorld, float dt)
{
Matrix start, end, end_double;
bool runonce = false;
// phase 3: down
/*float additionalDownStep = (m_wasOnGround && !OnGround) ? m_stepHeight : 0;
* Vector3 step_drop = m_up * (m_currentStepOffset + additionalDownStep);
* float downVelocity = (additionalDownStep == 0.0 && m_verticalVelocity < 0 ? -m_verticalVelocity : 0) * dt;
* Vector3 gravity_drop = m_up * downVelocity;
* m_targetPosition -= (step_drop + gravity_drop);*/
Vector3 orig_position = m_targetPosition;
float downVelocity = (m_verticalVelocity < 0f ? -m_verticalVelocity : 0f) * dt;
if (m_verticalVelocity > 0.0)
{
return;
}
if (downVelocity > 0.0 && downVelocity > m_fallSpeed && (m_wasOnGround || !m_wasJumping))
{
downVelocity = m_fallSpeed;
}
Vector3 step_drop = m_up * (m_currentStepOffset + downVelocity);
m_targetPosition -= step_drop;
using (KinematicClosestNotMeConvexResultCallback callback = new KinematicClosestNotMeConvexResultCallback(m_ghostObject, m_up, m_maxSlopeCosine))
using (KinematicClosestNotMeConvexResultCallback callback2 = new KinematicClosestNotMeConvexResultCallback(m_ghostObject, m_up, m_maxSlopeCosine))
{
callback.CollisionFilterGroup = GhostObject.BroadphaseHandle.CollisionFilterGroup;
callback.CollisionFilterMask = GhostObject.BroadphaseHandle.CollisionFilterMask;
callback2.CollisionFilterGroup = GhostObject.BroadphaseHandle.CollisionFilterGroup;
callback2.CollisionFilterMask = GhostObject.BroadphaseHandle.CollisionFilterMask;
while (true)
{
Matrix.Translation(ref m_currentPosition, out start);
Matrix.Translation(ref m_targetPosition, out end);
start.SetRotation(m_currentOrientation, out start);
end.SetRotation(m_targetOrientation, out end);
//set double test for 2x the step drop, to check for a large drop vs small drop
end_double = Matrix.Translation(m_targetPosition - step_drop);
if (m_useGhostObjectSweepTest)
{
m_ghostObject.ConvexSweepTest(m_convexShape, start, end, callback, collisionWorld.DispatchInfo.AllowedCcdPenetration);
if (!callback.HasHit && m_ghostObject.HasContactResponse)
{
//test a double fall height, to see if the character should interpolate it's fall (full) or not (partial)
m_ghostObject.ConvexSweepTest(m_convexShape, start, end_double, callback2, collisionWorld.DispatchInfo.AllowedCcdPenetration);
}
}
else
{
collisionWorld.ConvexSweepTest(m_convexShape, start, end, callback, collisionWorld.DispatchInfo.AllowedCcdPenetration);
if (!callback.HasHit && m_ghostObject.HasContactResponse)
{
//test a double fall height, to see if the character should interpolate it's fall (large) or not (small)
collisionWorld.ConvexSweepTest(m_convexShape, start, end_double, callback2, collisionWorld.DispatchInfo.AllowedCcdPenetration);
}
}
float downVelocity2 = (m_verticalVelocity < 0f ? -m_verticalVelocity : 0f) * dt;
bool has_hit;
if (bounce_fix == true)
{
has_hit = (callback.HasHit || callback2.HasHit) && m_ghostObject.HasContactResponse && NeedsCollision(m_ghostObject, callback.HitCollisionObject);
}
else
{
has_hit = callback2.HasHit && m_ghostObject.HasContactResponse && NeedsCollision(m_ghostObject, callback2.HitCollisionObject);
}
float stepHeight = 0.0f;
if (m_verticalVelocity < 0.0)
{
stepHeight = m_stepHeight;
}
if (downVelocity2 > 0.0 && downVelocity2 < stepHeight && has_hit == true && runonce == false && (m_wasOnGround || !m_wasJumping))
{
//redo the velocity calculation when falling a small amount, for fast stairs motion
//for larger falls, use the smoother/slower interpolated movement by not touching the target position
m_targetPosition = orig_position;
downVelocity = stepHeight;
step_drop = m_up * (m_currentStepOffset + downVelocity);
m_targetPosition -= step_drop;
runonce = true;
continue; //re-run previous tests
}
break;
}
if ((m_ghostObject.HasContactResponse && (callback.HasHit && NeedsCollision(m_ghostObject, callback.HitCollisionObject))) || runonce == true)
{
// we dropped a fraction of the height -> hit floor
float fraction = (m_currentPosition.Y - callback.HitPointWorld.Y) / 2;
//System.Console.WriteLine("hitpoint: {0} - pos {1}", callback.HitPointWorld.Y, m_currentPosition.Y);
if (bounce_fix == true)
{
if (full_drop == true)
{
m_currentPosition = Vector3.Lerp(m_currentPosition, m_targetPosition, callback.ClosestHitFraction);
}
else
{
//due to errors in the closestHitFraction variable when used with large polygons, calculate the hit fraction manually
m_currentPosition = Vector3.Lerp(m_currentPosition, m_targetPosition, fraction);
}
}
else
{
m_currentPosition = Vector3.Lerp(m_currentPosition, m_targetPosition, callback.ClosestHitFraction);
}
full_drop = false;
m_verticalVelocity = 0.0f;
m_verticalOffset = 0.0f;
m_wasJumping = false;
}
else
{
// we dropped the full height
full_drop = true;
if (bounce_fix == true)
{
downVelocity = (m_verticalVelocity < 0f ? -m_verticalVelocity : 0f) * dt;
if (downVelocity > m_fallSpeed && (m_wasOnGround || !m_wasJumping))
{
m_targetPosition += step_drop; //undo previous target change
downVelocity = m_fallSpeed;
step_drop = m_up * (m_currentStepOffset + downVelocity);
m_targetPosition -= step_drop;
}
}
//System.Console.WriteLine("full drop - {0}, {1}", m_currentPosition.Y, m_targetPosition.Y);
m_currentPosition = m_targetPosition;
}
}
}
protected void StepForwardAndStrafe(CollisionWorld collisionWorld, ref Vector3 walkMove)
{
//System.Console.WriteLine("m_normalizedDirection=" + m_normalizedDirection);
// phase 2: forward and strafe
Matrix start = Matrix.Identity;
Matrix end = Matrix.Identity;
m_targetPosition = m_currentPosition + walkMove;
float fraction = 1.0f;
float distance2 = (m_currentPosition - m_targetPosition).LengthSquared;
//System.Console.WriteLine("distance2=" + distance2);
int maxIter = 10;
while (fraction > 0.01f && maxIter-- > 0)
{
start.Origin = m_currentPosition;
end.Origin = m_targetPosition;
Vector3 sweepDirNegative = m_currentPosition - m_targetPosition;
start.SetRotation(m_currentOrientation, out start);
end.SetRotation(m_targetOrientation, out end);
using (KinematicClosestNotMeConvexResultCallback callback = new KinematicClosestNotMeConvexResultCallback(m_ghostObject, sweepDirNegative, 0.0f))
{
callback.CollisionFilterGroup = GhostObject.BroadphaseHandle.CollisionFilterGroup;
callback.CollisionFilterMask = GhostObject.BroadphaseHandle.CollisionFilterMask;
float margin = m_convexShape.Margin;
m_convexShape.Margin = margin + m_addedMargin;
if (start != end)
{
if (m_useGhostObjectSweepTest)
{
m_ghostObject.ConvexSweepTest(m_convexShape, start, end, callback, collisionWorld.DispatchInfo.AllowedCcdPenetration);
}
else
{
collisionWorld.ConvexSweepTest(m_convexShape, start, end, callback, collisionWorld.DispatchInfo.AllowedCcdPenetration);
}
}
m_convexShape.Margin = margin;
fraction -= callback.ClosestHitFraction;
if (callback.HasHit && GhostObject.HasContactResponse && NeedsCollision(m_ghostObject, callback.HitCollisionObject))
{
// we moved only a fraction
//float hitDistance = (callback.HitPointWorld - m_currentPosition).Length;
//Vector3.Lerp(ref m_currentPosition, ref m_targetPosition, callback.ClosestHitFraction, out m_currentPosition);
Vector3 hitNormalWorld = callback.HitNormalWorld;
UpdateTargetPositionBasedOnCollision(ref hitNormalWorld);
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 (currentDir.Dot(m_normalizedDirection) <= 0.0f)
{
break;
}
}
else
{
//System.Console.WriteLine("currentDir: don't normalize a zero vector");
break;
}
}
else
{
m_currentPosition = m_targetPosition;
}
}
}
}
protected void StepUp(CollisionWorld world)
{
float stepHeight = 0.0f;
if (m_verticalVelocity < 0.0f)
{
stepHeight = m_stepHeight;
}
// phase 1: up
Matrix start, end;
start = Matrix.Identity;
end = Matrix.Identity;
/* FIX ME: Handle penetration properly */
start.Origin = m_currentPosition;
m_targetPosition = m_currentPosition + m_up * (stepHeight) + m_jumpAxis * ((m_verticalOffset > 0f ? m_verticalOffset : 0f));
m_currentPosition = m_targetPosition;
end.Origin = m_targetPosition;
start.SetRotation(m_currentOrientation, out start);
end.SetRotation(m_targetOrientation, out end);
using (KinematicClosestNotMeConvexResultCallback callback = new KinematicClosestNotMeConvexResultCallback(m_ghostObject, -m_up, m_maxSlopeCosine))
{
callback.CollisionFilterGroup = GhostObject.BroadphaseHandle.CollisionFilterGroup;
callback.CollisionFilterMask = GhostObject.BroadphaseHandle.CollisionFilterMask;
if (m_useGhostObjectSweepTest)
{
m_ghostObject.ConvexSweepTest(m_convexShape, start, end, callback, world.DispatchInfo.AllowedCcdPenetration);
}
else
{
world.ConvexSweepTest(m_convexShape, start, end, callback, world.DispatchInfo.AllowedCcdPenetration);
}
if (callback.HasHit && m_ghostObject.HasContactResponse && NeedsCollision(m_ghostObject, callback.HitCollisionObject))
{
// Only modify the position if the hit was a slope and not a wall or ceiling.
if (callback.HitNormalWorld.Dot(m_up) > 0.0)
{
// we moved up only a fraction of the step height
m_currentStepOffset = stepHeight * callback.ClosestHitFraction;
if (m_interpolateUp == true)
{
m_currentPosition = Vector3.Lerp(m_currentPosition, m_targetPosition, callback.ClosestHitFraction);
}
else
{
m_currentPosition = m_targetPosition;
}
}
Matrix xform = m_ghostObject.WorldTransform;
xform.Origin = m_currentPosition;
m_ghostObject.WorldTransform = xform;
// fix penetration if we hit a ceiling for example
int numPenetrationLoops = 0;
m_touchingContact = false;
while (RecoverFromPenetration(world))
{
numPenetrationLoops++;
m_touchingContact = true;
if (numPenetrationLoops > 4)
{
//System.Console.WriteLine("character could not recover from penetration = " + numPenetrationLoops);
break;
}
}
m_targetPosition = m_ghostObject.WorldTransform.Origin;
m_currentPosition = m_targetPosition;
if (m_verticalOffset > 0)
{
m_verticalOffset = 0.0f;
m_verticalVelocity = 0.0f;
m_currentStepOffset = m_stepHeight;
}
}
else
{
m_currentStepOffset = stepHeight;
m_currentPosition = m_targetPosition;
}
}
}
protected void StepUp(CollisionWorld collisionWorld)
{
// phase 1: up
Matrix start, end;
m_targetPosition = m_currentPosition + upAxisDirection[m_upAxis] * (m_stepHeight + (m_verticalOffset > 0.0f ? m_verticalOffset : 0.0f));
/* FIXME: Handle penetration properly */
start = Matrix.Translation(m_currentPosition + upAxisDirection[m_upAxis] * (m_convexShape.Margin + m_addedMargin));
end = Matrix.Translation(m_targetPosition);
KinematicClosestNotMeConvexResultCallback callback = new KinematicClosestNotMeConvexResultCallback(m_ghostObject, -upAxisDirection[m_upAxis], 0.7071f);
callback.CollisionFilterGroup = GhostObject.BroadphaseHandle.CollisionFilterGroup;
callback.CollisionFilterMask = GhostObject.BroadphaseHandle.CollisionFilterMask;
if (m_useGhostObjectSweepTest)
{
m_ghostObject.ConvexSweepTestRef(m_convexShape, ref start, ref end, callback, collisionWorld.DispatchInfo.AllowedCcdPenetration);
}
else
{
collisionWorld.ConvexSweepTestRef(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 (Vector3.Dot(callback.HitNormalWorld, upAxisDirection[m_upAxis]) > 0.0)
{
// we moved up only a fraction of the step height
m_currentStepOffset = m_stepHeight * callback.ClosestHitFraction;
if (m_interpolateUp)
{
Vector3.Lerp(ref m_currentPosition, ref m_targetPosition, callback.ClosestHitFraction, out m_currentPosition);
}
else
{
m_currentPosition = m_targetPosition;
}
}
m_verticalVelocity = 0.0f;
m_verticalOffset = 0.0f;
}
else
{
m_currentStepOffset = m_stepHeight;
m_currentPosition = m_targetPosition;
}
}
protected void StepDown(CollisionWorld collisionWorld, float dt)
{
Matrix start, end, end_double;
bool runonce = false;
// 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);*/
Vector3 orig_position = m_targetPosition;
float downVelocity = (m_verticalVelocity < 0.0f ? -m_verticalVelocity : 0.0f) * dt;
if (downVelocity > 0.0 && downVelocity > m_fallSpeed
&& (m_wasOnGround || !m_wasJumping))
{
downVelocity = m_fallSpeed;
}
Vector3 step_drop = upAxisDirection[m_upAxis] * (m_currentStepOffset + downVelocity);
m_targetPosition -= step_drop;
KinematicClosestNotMeConvexResultCallback callback = new KinematicClosestNotMeConvexResultCallback(m_ghostObject, upAxisDirection[m_upAxis], m_maxSlopeCosine);
callback.CollisionFilterGroup = GhostObject.BroadphaseHandle.CollisionFilterGroup;
callback.CollisionFilterMask = GhostObject.BroadphaseHandle.CollisionFilterMask;
KinematicClosestNotMeConvexResultCallback callback2 = new KinematicClosestNotMeConvexResultCallback(m_ghostObject, upAxisDirection[m_upAxis], m_maxSlopeCosine);
callback2.CollisionFilterGroup = GhostObject.BroadphaseHandle.CollisionFilterGroup;
callback2.CollisionFilterMask = GhostObject.BroadphaseHandle.CollisionFilterMask;
while (true)
{
start = Matrix.Translation(m_currentPosition);
end = Matrix.Translation(m_targetPosition);
//set double test for 2x the step drop, to check for a large drop vs small drop
end_double = Matrix.Translation(m_targetPosition - step_drop);
if (m_useGhostObjectSweepTest)
{
m_ghostObject.ConvexSweepTestRef(m_convexShape, ref start, ref end, callback, collisionWorld.DispatchInfo.AllowedCcdPenetration);
if (!callback.HasHit)
{
//test a double fall height, to see if the character should interpolate it's fall (full) or not (partial)
m_ghostObject.ConvexSweepTest(m_convexShape, start, end_double, callback2, collisionWorld.DispatchInfo.AllowedCcdPenetration);
}
}
else
{
// this works....
collisionWorld.ConvexSweepTestRef(m_convexShape, ref start, ref end, callback, collisionWorld.DispatchInfo.AllowedCcdPenetration);
if (!callback.HasHit)
{
//test a double fall height, to see if the character should interpolate it's fall (large) or not (small)
m_ghostObject.ConvexSweepTest(m_convexShape, start, end_double, callback2, collisionWorld.DispatchInfo.AllowedCcdPenetration);
}
}
float downVelocity2 = (m_verticalVelocity < 0.0f ? -m_verticalVelocity : 0.0f) * dt;
bool has_hit = false;
if (bounce_fix == true)
has_hit = callback.HasHit || callback2.HasHit;
else
has_hit = callback2.HasHit;
if (downVelocity2 > 0.0f && downVelocity2 < m_stepHeight && has_hit == true && runonce == false
&& (m_wasOnGround || !m_wasJumping))
{
//redo the velocity calculation when falling a small amount, for fast stairs motion
//for larger falls, use the smoother/slower interpolated movement by not touching the target position
m_targetPosition = orig_position;
downVelocity = m_stepHeight;
Vector3 step_drop2 = upAxisDirection[m_upAxis] * (m_currentStepOffset + downVelocity);
m_targetPosition -= step_drop2;
runonce = true;
continue; //re-run previous tests
}
break;
}
if (callback.HasHit || runonce == true)
{
// we dropped a fraction of the height -> hit floor
float fraction = (m_currentPosition.Y - callback.HitPointWorld.Y) / 2;
//printf("hitpoint: %g - pos %g\n", callback.m_hitPointWorld.getY(), m_currentPosition.getY());
if (bounce_fix == true)
{
if (full_drop == true)
{
Vector3.Lerp(ref m_currentPosition, ref m_targetPosition, callback.ClosestHitFraction, out m_currentPosition);
}
else
{
//due to errors in the closestHitFraction variable when used with large polygons, calculate the hit fraction manually
Vector3.Lerp(ref m_currentPosition, ref m_targetPosition, fraction, out m_currentPosition);
}
}
else
{
Vector3.Lerp(ref m_currentPosition, ref m_targetPosition, callback.ClosestHitFraction, out m_currentPosition);
}
full_drop = false;
m_verticalVelocity = 0.0f;
m_verticalOffset = 0.0f;
m_wasJumping = false;
}
else
{
// we dropped the full height
full_drop = true;
if (bounce_fix == true)
{
downVelocity = (m_verticalVelocity < 0.0f ? -m_verticalVelocity : 0.0f) * dt;
if (downVelocity > m_fallSpeed && (m_wasOnGround || !m_wasJumping))
{
m_targetPosition += step_drop; //undo previous target change
downVelocity = m_fallSpeed;
step_drop = upAxisDirection[m_upAxis] * (m_currentStepOffset + downVelocity);
m_targetPosition -= step_drop;
}
}
//printf("full drop - %g, %g\n", m_currentPosition.getY(), m_targetPosition.getY());
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)
{
float dot;
Vector3.Dot(ref m_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 = (m_currentPosition);
end.Origin = (m_targetPosition);
Vector3 sweepDirNegative = m_currentPosition - m_targetPosition;
KinematicClosestNotMeConvexResultCallback callback = new KinematicClosestNotMeConvexResultCallback(m_ghostObject, sweepDirNegative, 0f);
callback.CollisionFilterGroup = GhostObject.BroadphaseHandle.CollisionFilterGroup;
callback.CollisionFilterMask = GhostObject.BroadphaseHandle.CollisionFilterMask;
float margin = m_convexShape.Margin;
m_convexShape.Margin = margin + m_addedMargin;
if (m_useGhostObjectSweepTest)
{
m_ghostObject.ConvexSweepTestRef(m_convexShape, ref start, ref end, callback, collisionWorld.DispatchInfo.AllowedCcdPenetration);
}
else
{
collisionWorld.ConvexSweepTestRef(m_convexShape, ref start, ref end, callback, collisionWorld.DispatchInfo.AllowedCcdPenetration);
}
m_convexShape.Margin = margin;
fraction -= callback.ClosestHitFraction;
if (callback.HasHit)
{
// we moved only a fraction
float hitDistance = (callback.HitPointWorld - m_currentPosition).Length;
Vector3 hitNormalWorld = callback.HitNormalWorld;
UpdateTargetPositionBasedOnCollision(ref 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." */
float dot;
Vector3.Dot(ref currentDir, ref m_normalizedDirection, out dot);
if (dot <= 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)
{
Matrix start, end, end_double;
bool runonce = false;
// 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);*/
Vector3 orig_position = m_targetPosition;
float downVelocity = (m_verticalVelocity < 0.0f ? -m_verticalVelocity : 0.0f) * dt;
if (downVelocity > 0.0 && downVelocity > m_fallSpeed &&
(m_wasOnGround || !m_wasJumping))
{
downVelocity = m_fallSpeed;
}
Vector3 step_drop = upAxisDirection[m_upAxis] * (m_currentStepOffset + downVelocity);
m_targetPosition -= step_drop;
KinematicClosestNotMeConvexResultCallback callback = new KinematicClosestNotMeConvexResultCallback(m_ghostObject, upAxisDirection[m_upAxis], m_maxSlopeCosine);
callback.CollisionFilterGroup = GhostObject.BroadphaseHandle.CollisionFilterGroup;
callback.CollisionFilterMask = GhostObject.BroadphaseHandle.CollisionFilterMask;
KinematicClosestNotMeConvexResultCallback callback2 = new KinematicClosestNotMeConvexResultCallback(m_ghostObject, upAxisDirection[m_upAxis], m_maxSlopeCosine);
callback2.CollisionFilterGroup = GhostObject.BroadphaseHandle.CollisionFilterGroup;
callback2.CollisionFilterMask = GhostObject.BroadphaseHandle.CollisionFilterMask;
while (true)
{
start = Matrix.Translation(m_currentPosition);
end = Matrix.Translation(m_targetPosition);
//set double test for 2x the step drop, to check for a large drop vs small drop
end_double = Matrix.Translation(m_targetPosition - step_drop);
if (m_useGhostObjectSweepTest)
{
m_ghostObject.ConvexSweepTestRef(m_convexShape, ref start, ref end, callback, collisionWorld.DispatchInfo.AllowedCcdPenetration);
if (!callback.HasHit)
{
//test a double fall height, to see if the character should interpolate it's fall (full) or not (partial)
m_ghostObject.ConvexSweepTest(m_convexShape, start, end_double, callback2, collisionWorld.DispatchInfo.AllowedCcdPenetration);
}
}
else
{
// this works....
collisionWorld.ConvexSweepTestRef(m_convexShape, ref start, ref end, callback, collisionWorld.DispatchInfo.AllowedCcdPenetration);
if (!callback.HasHit)
{
//test a double fall height, to see if the character should interpolate it's fall (large) or not (small)
m_ghostObject.ConvexSweepTest(m_convexShape, start, end_double, callback2, collisionWorld.DispatchInfo.AllowedCcdPenetration);
}
}
float downVelocity2 = (m_verticalVelocity < 0.0f ? -m_verticalVelocity : 0.0f) * dt;
bool has_hit = false;
if (bounce_fix == true)
{
has_hit = callback.HasHit || callback2.HasHit;
}
else
{
has_hit = callback2.HasHit;
}
if (downVelocity2 > 0.0f && downVelocity2 < m_stepHeight && has_hit == true && runonce == false &&
(m_wasOnGround || !m_wasJumping))
{
//redo the velocity calculation when falling a small amount, for fast stairs motion
//for larger falls, use the smoother/slower interpolated movement by not touching the target position
m_targetPosition = orig_position;
downVelocity = m_stepHeight;
Vector3 step_drop2 = upAxisDirection[m_upAxis] * (m_currentStepOffset + downVelocity);
m_targetPosition -= step_drop2;
runonce = true;
continue; //re-run previous tests
}
break;
}
if (callback.HasHit || runonce == true)
{
// we dropped a fraction of the height -> hit floor
float fraction = (m_currentPosition.Y - callback.HitPointWorld.Y) / 2;
//printf("hitpoint: %g - pos %g\n", callback.m_hitPointWorld.getY(), m_currentPosition.getY());
if (bounce_fix == true)
{
if (full_drop == true)
{
Vector3.Lerp(ref m_currentPosition, ref m_targetPosition, callback.ClosestHitFraction, out m_currentPosition);
}
else
{
//due to errors in the closestHitFraction variable when used with large polygons, calculate the hit fraction manually
Vector3.Lerp(ref m_currentPosition, ref m_targetPosition, fraction, out m_currentPosition);
}
}
else
{
Vector3.Lerp(ref m_currentPosition, ref m_targetPosition, callback.ClosestHitFraction, out m_currentPosition);
}
full_drop = false;
m_verticalVelocity = 0.0f;
m_verticalOffset = 0.0f;
m_wasJumping = false;
}
else
{
// we dropped the full height
full_drop = true;
if (bounce_fix == true)
{
downVelocity = (m_verticalVelocity < 0.0f ? -m_verticalVelocity : 0.0f) * dt;
if (downVelocity > m_fallSpeed && (m_wasOnGround || !m_wasJumping))
{
m_targetPosition += step_drop; //undo previous target change
downVelocity = m_fallSpeed;
step_drop = upAxisDirection[m_upAxis] * (m_currentStepOffset + downVelocity);
m_targetPosition -= step_drop;
}
}
//printf("full drop - %g, %g\n", m_currentPosition.getY(), m_targetPosition.getY());
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)
{
float dot;
Vector3.Dot(ref m_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 = (m_currentPosition);
end.Origin = (m_targetPosition);
Vector3 sweepDirNegative = m_currentPosition - m_targetPosition;
KinematicClosestNotMeConvexResultCallback callback = new KinematicClosestNotMeConvexResultCallback(m_ghostObject, sweepDirNegative, 0f);
callback.CollisionFilterGroup = GhostObject.BroadphaseHandle.CollisionFilterGroup;
callback.CollisionFilterMask = GhostObject.BroadphaseHandle.CollisionFilterMask;
float margin = m_convexShape.Margin;
m_convexShape.Margin = margin + m_addedMargin;
if (m_useGhostObjectSweepTest)
{
m_ghostObject.ConvexSweepTestRef(m_convexShape, ref start, ref end, callback, collisionWorld.DispatchInfo.AllowedCcdPenetration);
}
else
{
collisionWorld.ConvexSweepTestRef(m_convexShape, ref start, ref end, callback, collisionWorld.DispatchInfo.AllowedCcdPenetration);
}
m_convexShape.Margin = margin;
fraction -= callback.ClosestHitFraction;
if (callback.HasHit)
{
// we moved only a fraction
float hitDistance = (callback.HitPointWorld - m_currentPosition).Length;
Vector3 hitNormalWorld = callback.HitNormalWorld;
UpdateTargetPositionBasedOnCollision(ref 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." */
float dot;
Vector3.Dot(ref currentDir, ref m_normalizedDirection, out dot);
if (dot <= 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;
}
}
