void playerStep(CollisionWorld collisionWorld, VFixedPoint dt)
{
wasOnGround = onGround();
verticalVelocity -= gravity * dt;
if (verticalVelocity > VFixedPoint.Zero && verticalVelocity > jumpSpeed)
{
verticalVelocity = jumpSpeed;
}
if (verticalVelocity < VFixedPoint.Zero && verticalVelocity.Abs() > fallSpeed.Abs())
{
verticalVelocity = -fallSpeed.Abs();
}
verticalOffset = verticalVelocity * dt;
VIntTransform transform = me.getWorldTransform();
//climb up slope
stepUp(collisionWorld);
VFixedPoint dtMoving = dt;
VInt3 move = walkDirection * dtMoving;
stepForwardAndStrafe(collisionWorld, move);
//slide down slope
stepDown(collisionWorld, dt);
transform.position = currentPosition;
me.setWorldTransform(transform);
}
//Interset segment sa, sb against cylinder specified by p, d and r
static bool IntersectSegmentCylinder(VInt3 sa, VInt3 sb, VInt3 p, VInt3 q, VFixedPoint r, ref VInt3 normal, ref VFixedPoint t)
{
if (r < Globals.EPS2)
{
return(false);
}
VInt3 d = q - p, m = sa - p, n = sb - sa;
VFixedPoint md = VInt3.Dot(m, d);
VFixedPoint nd = VInt3.Dot(n, d);
VFixedPoint dd = d.sqrMagnitude;
if (md < VFixedPoint.Zero && md + nd < VFixedPoint.Zero)
{
return(false);
}
if (md > dd && md + nd > dd)
{
return(false);
}
VFixedPoint nn = n.sqrMagnitude;
VFixedPoint mn = VInt3.Dot(m, n);
VFixedPoint a = dd * nn - nd * nd;
VFixedPoint k = m.sqrMagnitude - r * r;
VFixedPoint c = dd * k - md * md;
//if start point is within cylinder, not intersect
if (c < VFixedPoint.Zero)
{
return(false);
}
//if segment is parallel to cylinder, they do not intersect with each other
if (a.Abs() < Globals.EPS)
{
return(false);
}
VFixedPoint b = dd * mn - nd * md;
VFixedPoint discr = b * b - a * c;
if (discr < VFixedPoint.Zero)
{
return(false);
}
VFixedPoint discrSqrt = FMath.Sqrt(discr);
t = (-b - discrSqrt) / a;
if (t < VFixedPoint.Zero || t > VFixedPoint.One)
{
return(false);
}
//we don't need result of segment and endcap
/*if(md + t * nd < VFixedPoint.Zero)
* {
* if (nd <= VFixedPoint.Zero) return false;
* t = -md / nd;
* return k + t * (mn + t * nn) * 2 <= VFixedPoint.Zero;
* }
* else if(md + t * nd > dd)
* {
* if (nd >= VFixedPoint.Zero) return false;
* t = (dd - md) / nd;
* return k + dd - md * 2 + t * ((md - nd) * 2 + t * nn) <= VFixedPoint.Zero;
* }*/
VInt3 hitPoint = sa + n * t;
VFixedPoint param = VFixedPoint.Zero;
Distance.distancePointSegmentSquared(p, q, hitPoint, ref param);
if (param <= VFixedPoint.Zero || param >= VFixedPoint.One)
{
return(false);
}
VInt3 closestPoint = p * (VFixedPoint.One - param) + q * param;
normal = (hitPoint - closestPoint) / r;
return(true);
}
public VInt3 Abs()
{
return(new VInt3(x.Abs(), y.Abs(), z.Abs()));
}
static void getIncidentPolygon(ref VInt3[] pts, ref VInt3 faceNormal, VInt3 axis, VIntTransform transf1To0, VInt3 extents)
{
VFixedPoint ex = extents.x, ey = extents.y, ez = extents.z;
VInt3 u0 = transf1To0.right, u1 = transf1To0.up, u2 = transf1To0.forward;
VFixedPoint d0 = VInt3.Dot(u0, axis), d1 = VInt3.Dot(u1, axis), d2 = VInt3.Dot(u2, axis);
VFixedPoint absd0 = d0.Abs(), absd1 = d1.Abs(), absd2 = d2.Abs();
VInt3 r0 = VInt3.zero, r1 = VInt3.zero, r2 = VInt3.zero;
if (absd0 >= absd1 && absd0 >= absd2)
{
bool con = d0 > VFixedPoint.Zero;
faceNormal = con ? -u0 : u0;
ex = con ? -ex : ex;
r0 = u0 * ex;
r1 = u1 * ey;
r2 = u2 * ez;
VInt3 temp0 = transf1To0.position + r0;
VInt3 temp1 = r1 + r2;
VInt3 temp2 = r1 - r2;
pts[0] = temp0 + temp1;
pts[1] = temp0 + temp2;
pts[2] = temp0 - temp1;
pts[3] = temp0 - temp2;
}
else if (absd1 >= absd2)
{
bool con = d1 > VFixedPoint.Zero;
faceNormal = con ? -u1 : u1;
ey = con ? -ey : ey;
r0 = u0 * ex;
r1 = u1 * ey;
r2 = u2 * ez;
VInt3 temp0 = transf1To0.position + r1;
VInt3 temp1 = r0 + r2;
VInt3 temp2 = r0 - r2;
pts[0] = temp0 + temp1;
pts[1] = temp0 + temp2;
pts[2] = temp0 - temp1;
pts[3] = temp0 - temp2;
}
else
{
bool con = d2 > VFixedPoint.Zero;
faceNormal = con ? -u2 : u2;
ez = con ? -ez : ez;
r0 = u0 * ex;
r1 = u1 * ey;
r2 = u2 * ez;
VInt3 temp0 = transf1To0.position + r2;
VInt3 temp1 = r0 + r1;
VInt3 temp2 = r0 - r1;
pts[0] = temp0 + temp1;
pts[1] = temp0 + temp2;
pts[2] = temp0 - temp1;
pts[3] = temp0 - temp2;
}
}