/// <summary>
/// This function fixed contacts only with external objects for capsule
/// </summary>
private void FixContact(ref ShapeContact contact)
{
Vec3 firstPoint = Vec3.ZERO;
// set first point for intersection
// it's bottom or top sphere of capsule
// or cylinder part of capsule
if (contact.Point.z < shape.BottomCap.z)
{
firstPoint = shape.BottomCap;
}
else if (shape.BottomCap.z <= contact.Point.z && contact.Point.z <= shape.TopCap.z)
{
firstPoint = new Vec3(shape.TopCap.x, shape.TopCap.y, contact.Point.z);
}
else
{
firstPoint = shape.TopCap;
}
// try get contact object
WorldIntersectionNormal normalIntersection = new WorldIntersectionNormal();
var hitObj = World.GetIntersection(firstPoint, contact.Point, contactsIntersectionMask, normalIntersection);
if (hitObj)
{
// get real distance to contact
float distance = (float)(normalIntersection.Point - firstPoint).Length;
if (distance < shape.Radius)
{
// set correct parameters for contact
contact.Point = normalIntersection.Point;
contact.Depth = shape.Radius - distance;
contact.Normal = new vec3((firstPoint - contact.Point).Normalize());
}
else
{
// contact outside capsule
contact.Point = Vec3.ZERO;
contact.Depth = 0.0f;
contact.Normal = vec3.ZERO;
}
}
// check contact with horizontal plane, because intersection can't detect it
else if (MathLib.Dot(vec3.UP, contact.Normal) < MathLib.EPSILON)
{
if (contact.Object)
{
vec3 scale = contact.Object.WorldScale;
// check object scale
if (MathLib.Compare(scale.x, 1.0f) != 1 ||
MathLib.Compare(scale.y, 1.0f) != 1 ||
MathLib.Compare(scale.z, 1.0f) != 1)
{
// this value avoids jittering
contact.Depth = MathLib.Max(Physics.PenetrationTolerance, 0.01f);
}
}
}
else
{
// capsule does not contact with this object
contact.Point = Vec3.ZERO;
contact.Depth = 0.0f;
contact.Normal = vec3.ZERO;
}
}
private void TryMoveDown(float ifps)
{
vec3 moveImpulseXY = moveImpulse - vec3.UP * MathLib.Dot(vec3.UP, moveImpulse);
Vec3 pos1 = position + (moveImpulseXY.Normalized + vec3.UP) * shape.Radius;
Vec3 pos2 = pos1 - vec3.UP * shape.Radius;
WorldIntersectionNormal intersection = new WorldIntersectionNormal();
Unigine.Object hitObj = World.GetIntersection(pos1, pos2, contactsIntersectionMask, intersection);
if (hitObj)
{
float angle = MathLib.GetAngle(vec3.UP, intersection.Normal);
if (autoSteppingCancelAngle < angle && angle < 90.0f)
{
position -= autoSteppingOffset;
usedAutoStepping = false;
return;
}
}
vec3 velocityXY = velocity - vec3.UP * MathLib.Dot(vec3.UP, velocity);
pos1 = position + (velocityXY.Normalized + vec3.UP) * shape.Radius;
pos2 = pos1 - vec3.UP * shape.Radius;
hitObj = World.GetIntersection(pos1, pos2, contactsIntersectionMask, intersection);
if (hitObj)
{
float angle = MathLib.GetAngle(vec3.UP, intersection.Normal);
if (autoSteppingCancelAngle < angle && angle < 90.0f)
{
position -= autoSteppingOffset;
usedAutoStepping = false;
return;
}
}
// this correction allows to avoid jittering on large stairs
if (lastStepHeight > shape.Radius)
{
if (GravityMultiplier > 1.0f)
{
lastStepHeight = shape.Radius / GravityMultiplier - Physics.PenetrationTolerance;
}
else
{
lastStepHeight = shape.Radius - Physics.PenetrationTolerance;
}
}
// try to drop down the player
position -= new vec3(zAxis * lastStepHeight);
autoSteppingOffset -= new vec3(zAxis * lastStepHeight);
// move (apply "position")
dummy.Transform = GetBodyTransform();
// find collisions with the capsule
shape.GetCollision(contacts, 0.0f);
if (contacts.Count == 0)
{
return;
}
int contactsCount = MathLib.Min(contacts.Count, contactsBufferSize);
float inumContacts = 1.0f / MathLib.Itof(contactsCount);
// push up (if collisions exists)
for (int i = 0; i < contactsCount; i++)
{
ShapeContact c = contacts[i];
position += new vec3(zAxis * (MathLib.Max(c.Depth, 0.0f) * inumContacts * MathLib.Dot(zAxis, c.Normal)));
if (MathLib.Dot(c.Normal, vec3.UP) > 0.5f && MathLib.Dot(new vec3(c.Point - shape.BottomCap), vec3.UP) < 0.0f)
{
IsGround = true;
}
}
if (renderDownPassContacts)
{
foreach (var c in contacts)
{
Visualizer.RenderVector(c.Point, c.Point + c.Normal, new vec4(0, 1, 0, 1));
}
}
}
