private void TryMoveUp(float ifps)
{
usedAutoStepping = false;
lastStepHeight = 0;
if (!IsFrozen && MaxStepHeight > 0) // when player is moving
{
// 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);
// find max step height
for (int i = 0; i < contactsCount; i++)
{
ShapeContact c = contacts[i];
vec3 normalXY = c.Normal - vec3.UP * MathLib.Dot(vec3.UP, c.Normal);
vec3 velocityXY = velocity - vec3.UP * MathLib.Dot(vec3.UP, velocity);
// skip contacts opposite to movement
if (normalXY.Length2 > MathLib.EPSILON && velocityXY.Length2 > MathLib.EPSILON &&
MathLib.Dot(normalXY.Normalized, velocityXY.Normalized) > 0.5f)
{
continue;
}
float step = MathLib.Dot(new vec3(c.Point - position), vec3.UP);
if (lastStepHeight < step)
{
lastStepHeight = step;
}
}
// apply auto stepping
if (MinStepHeight < lastStepHeight && lastStepHeight < MaxStepHeight)
{
position += new vec3(zAxis * lastStepHeight);
autoSteppingOffset += new vec3(zAxis * lastStepHeight);
usedAutoStepping = true;
}
if (renderUpPassContacts)
{
foreach (var c in contacts)
{
Visualizer.RenderVector(c.Point, c.Point + c.Normal, new vec4(1, 0, 0, 1));
}
}
}
}
private void UpdateTransform()
{
// ortho basis
vec3 tangent, binormal;
Geometry.OrthoBasis(vec3.UP, out tangent, out binormal);
// decompose transformation
position = node.WorldPosition - zAxis * (collisionHeight + 2.0f * CollisionRadius);
direction = MathLib.Normalize(new vec3(-node.WorldTransform.AxisZ));
// decompose direction
// in this case don't use properties
phiAngle = MathLib.Atan2(MathLib.Dot(direction, tangent), MathLib.Dot(direction, binormal)) * MathLib.RAD2DEG;
thetaAngle = MathLib.Acos(MathLib.Clamp(MathLib.Dot(direction, vec3.UP), -1.0f, 1.0f)) * MathLib.RAD2DEG - 90.0f;
// object transformation
playerBody.WorldTransform = GetBodyTransform();
// set camera
camera.Modelview = GetModelview();
}
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));
}
}
}
}
private void UpdatePosition(float ifps)
{
position += new vec3(velocity * ifps);
}
private void Init()
{
if (showAdvancedSettings)
{
showAdvancedSettings = false;
}
if (useExternalBody)
{
useExternalBody = false;
}
PlayerDummy player = node as PlayerDummy;
if (!player)
{
return;
}
// decompose transformation
position = node.WorldPosition;
direction = MathLib.Normalize(new vec3(-node.WorldTransform.AxisZ));
camera = player.Camera;
if (playerBody)
{
dummy = playerBody.Body as BodyDummy;
if (dummy)
{
for (int i = 0; i < dummy.NumShapes; i++)
{
if (!shape)
{
shape = dummy.GetShape(i) as ShapeCapsule;
}
}
if (shape)
{
shape.Restitution = 0.0f;
shape.Continuous = false;
PhysicalIntersectionMask = shape.PhysicsIntersectionMask;
CollisionMask = shape.CollisionMask;
Mass = shape.Mass;
CollisionRadius = shape.Radius;
CollisionHeight = shape.Height;
}
}
}
if (!dummy || !shape)
{
if (playerBody)
{
playerBody.Enabled = false;
}
playerBody = new ObjectDummy();
dummy = new BodyDummy();
shape = new ShapeCapsule(1.0f, 1.0f);
shape.Restitution = 0.0f;
shape.Continuous = false;
dummy.Enabled = true;
playerBody.Body = dummy;
shape.Body = dummy;
PhysicalIntersectionMask = physicalIntersectionMask;
CollisionMask = collisionMask;
Mass = mass;
CollisionRadius = collisionRadius;
CollisionHeight = collisionHeight;
}
contacts = new List <ShapeContact>();
UpdateTransform();
maxSlopeAngle = MathLib.Max(maxSlopeAngle, maxStaticSlopeAngle);
maxSlidingSpeed = MathLib.Max(maxSlidingSpeed, minSlidingSpeed);
if (showDebug)
{
Visualizer.Enabled = true;
Render.ShowTriangles = 1;
}
isInitialized = true;
}
/// <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));
}
}
}
public void Hack_UpdatePosition(vec3 newPosition)
{
position = newPosition;
}
