static Vector3 ParallelComponent(ref Vector3 direction, ref Vector3 normal)
{
float magnitude;
Vector3.Dot(ref direction, ref normal, out magnitude);
return(normal * magnitude);
}
static Vector3 ComputeReflectionDirection(ref Vector3 direction, ref Vector3 normal)
{
float dot;
Vector3.Dot(ref direction, ref normal, out dot);
return(direction - (2.0f * dot) * normal);
}
public void StepUp(float dt)
{
Matrix transform = RigidBody.MotionState.WorldTransform;
if (!_stepping)
{
_stepping = true;
_steppingTo.Y += CapsuleHalfHeight + CapsuleRadius;
}
/* Bullet reacts with velocity also with kinematic bodies, althogh this
* should not change anything, until we do not go back to dynamic body. */
Vector3 origin = transform.Origin;
float hor = Vector3.Dot(origin - _steppingTo, _steppingInvNormal);
Vector3 stepDir = _steppingTo - origin;
stepDir.Y = 0;
stepDir.Normalize();
float speed = Vector3.Dot(stepDir, Vector3.TransformNormal(_targetSpeed, transform.Basis))
* SteppingSpeed;
origin.Y += speed * dt * 2;
float dv = _steppingTo.Y - origin.Y;
if (dv <= -1 * MaxStepHeight || _targetSpeed.LengthSquared < 0.0001f || speed <= 0)
{
CancelStep();
}
else if (dv < CapsuleRadius)
{
float dh = (float)System.Math.Sqrt(dv * (2 * CapsuleRadius - dv));
if (dh < System.Math.Abs(hor))
{
float advance = System.Math.Min(dh * System.Math.Sign(hor) - hor, speed * dt);
var addVec = _steppingInvNormal * advance;
transform.Origin = transform.Origin + addVec;
}
}
RigidBody.MotionState.WorldTransform = transform;
}
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;
}
}
}
