private void MoveSide(float ifps)
{
vec3 tangent = vec3.ZERO;
vec3 binormal = vec3.ZERO;
for (int i = 0; i < collisionIterations; i++)
{
// move (apply "position")
dummy.Transform = GetBodyTransform();
// find collisions with the capsule
shape.GetCollision(contacts, 0.0f);
// in case of a large number of contacts, we use one iteration to avoid performance degradation
if (contacts.Count > heavyContactsCount)
{
i = collisionIterations - 1;
}
if (contacts.Count == 0)
{
break;
}
int contactsCount = MathLib.Min(contacts.Count, contactsBufferSize);
float inumContacts = 1.0f / MathLib.Itof(contactsCount);
float maxSlopeAngle = -MathLib.INFINITY;
// solving collisions
for (int j = 0; j < contactsCount; j++)
{
ShapeContact c = contacts[j];
FixContact(ref c);
float normalSpeed = 0.0f;
if (IsFrozen)
{
position += zAxis * c.Depth * inumContacts * MathLib.Dot(zAxis, c.Normal);
autoSteppingOffset += zAxis * c.Depth * inumContacts * MathLib.Dot(zAxis, c.Normal);
normalSpeed = MathLib.Dot(zAxis, velocity);
velocity -= zAxis * normalSpeed;
}
else
{
position += c.Normal * c.Depth * inumContacts;
autoSteppingOffset += c.Normal * c.Depth * inumContacts;
IsFrozen = false;
normalSpeed = MathLib.Dot(c.Normal, velocity);
velocity -= c.Normal * normalSpeed;
}
// friction
if (targetSpeed < MathLib.EPSILON)
{
Geometry.OrthoBasis(c.Normal, out tangent, out binormal);
float tangentSpeed = MathLib.Dot(tangent, velocity);
float binormalSpeed = MathLib.Dot(binormal, velocity);
if (MathLib.Abs(tangentSpeed) > MathLib.EPSILON || MathLib.Abs(binormalSpeed) > MathLib.EPSILON)
{
float frictionSpeed = MathLib.Max(-normalSpeed, 0.0f) * friction * MathLib.Rsqrt(tangentSpeed * tangentSpeed + binormalSpeed * binormalSpeed);
frictionSpeed = MathLib.Clamp(frictionSpeed, -1.0f, 1.0f);
velocity -= tangent * tangentSpeed * frictionSpeed;
velocity -= binormal * binormalSpeed * frictionSpeed;
}
}
if (MathLib.Dot(c.Normal, vec3.UP) > 0.5f && MathLib.Dot(new vec3(c.Point - shape.BottomCap), vec3.UP) < 0.0f)
{
IsGround = true;
float angle = MathLib.GetAngle(vec3.UP, c.Normal);
if (angle > maxSlopeAngle && 0 < angle && angle < 90.0f)
{
slopeNormal = c.Normal;
slopePoint = c.Point;
slopeAngle = angle;
maxSlopeAngle = angle;
}
}
if (MathLib.Dot(c.Normal, vec3.UP) < -0.5f && MathLib.Dot(new vec3(c.Point - shape.TopCap), vec3.UP) > 0.0f)
{
IsCeiling = true;
}
}
if (renderSidePassContacts)
{
foreach (var c in contacts)
{
Visualizer.RenderVector(c.Point, c.Point + c.Normal, new vec4(0, 1, 1, 1));
}
}
}
}