public void CheckGround(ManifoldPoint cp)
{
if (HaveGround)
{
return;
}
Matrix inverse = _controller.RigidBody.WorldTransform;
inverse.Invert();
Vector3 localPoint = Vector3.TransformCoordinate(cp.PositionWorldOnB, inverse);
localPoint = new Vector3(localPoint.X, localPoint.Y + _controller.CapsuleHalfHeight, localPoint.Z);
float r = localPoint.Length;
float cosTheta = localPoint.Y / r;
if (System.Math.Abs(r - _controller.CapsuleRadius) <= _controller.GroundSearchMargin &&
cosTheta < _controller.MaxCosGround)
{
HaveGround = true;
GroundPoint = cp.PositionWorldOnB;
GroundNormal = cp.NormalWorldOnB;
}
}
