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)); } } } }