public void HandleCollision(Manifold m, TFRigidbody a, TFRigidbody b)
{
TFCircleCollider A = (TFCircleCollider)a.coll;
TFCircleCollider B = (TFCircleCollider)b.coll;
//Calculate translational vector, which is normal
FixVec2 normal = ((FixVec2)b.Position) - ((FixVec2)a.Position);
Fix distSpr = normal.GetMagnitudeSquared();
Fix radius = A.radius + B.radius;
//Not in contact
if (distSpr >= radius * radius)
{
m.contactCount = 0;
return;
}
Fix distance = FixMath.Sqrt(distSpr);
m.contactCount = 1;
if (distance == Fix.zero)
{
m.penetration = A.radius;
m.normal = new FixVec2(1, 0);
m.contacts[0] = (FixVec2)a.Position;
}
else
{
m.penetration = radius - distance;
m.normal = normal / distance;
m.contacts[0] = m.normal * A.radius + ((FixVec2)a.Position);
}
}
public void HandleCollision(Manifold m, TFRigidbody a, TFRigidbody b)
{
TFEdgeCollider A = (TFEdgeCollider)a.coll;
TFCircleCollider B = (TFCircleCollider)b.coll;
// No line segments, return out.
if (A.vertices.Count < 2)
{
return;
}
// Transform circle center to Edge model space
FixVec2 circleCenter = A.u.Transposed() * (b.Position - a.Position);
// Iterate through all the line segments to find contact point.
for (int i = 0; i < A.vertices.Count - 1; i++)
{
FixVec2 rayDir = (A.vertices[i + 1] - A.vertices[i]);
FixVec2 centerRay = (A.vertices[i] - circleCenter);
Fix k = rayDir.Dot(rayDir);
Fix l = 2 * centerRay.Dot(rayDir);
Fix n = centerRay.Dot(centerRay) - (B.radius * B.radius);
Fix discriminant = l * l - 4 * k * n;
// No intersection.
if (discriminant <= Fix.zero)
{
continue;
}
discriminant = FixMath.Sqrt(discriminant);
Fix t1 = (-l - discriminant) / (2 * k);
Fix t2 = (-l + discriminant) / (2 * k);
Fix s = FixVec2.Dot(A.normals[i], circleCenter - A.vertices[i]);
if (t1 >= Fix.zero && t1 <= Fix.one)
{
//t1 is the intersection, and it's closer than t2.
m.contactCount = 1;
m.contacts[0] = (A.u * A.vertices[i] + a.Position) + (t1 * rayDir);
m.normal = A.normals[i];
m.penetration = B.radius - s;
return;
}
if (t2 >= Fix.zero && t2 <= Fix.one)
{
// t1 didn't insection, so we either started inside the circle
// or completely past it.
m.contactCount = 1;
m.contacts[0] = (A.u * A.vertices[i] + a.Position) + (t2 * rayDir);
m.normal = A.normals[i];
m.penetration = B.radius - s;
return;
}
}
}
public override bool Raycast(out TFRaycastHit2D hit, FixVec2 pointA, FixVec2 pointB, Fix maxFraction)
{
hit = default;
FixVec2 center = (FixVec2)tdTransform.Position;
FixVec2 s = pointA - center;
Fix b = FixVec2.Dot(s, s) - radius * radius;
// Solve quadratic equation.
FixVec2 r = pointB - pointA;
Fix c = FixVec2.Dot(s, r);
Fix rr = FixVec2.Dot(r, r);
Fix sigma = c * c - rr * b;
// Check for negative discriminant and short segment.
if (sigma < Fix.zero || rr < Fix.Epsilon)
{
return(false);
}
// Find the point of intersection on the line with the circle.
Fix a = -(c + FixMath.Sqrt(sigma));
// Is the intersection point on the segment?
if (Fix.zero <= a && a <= maxFraction * rr)
{
a /= rr;
hit.fraction = a;
hit.normal = s + a * r;
hit.normal.Normalize();
return(true);
}
return(false);
}
private Vector3 GetU3DPosition()
{
return(new Vector3(FixMath.mm2m(position.x),
FixMath.mm2m(position.y),
FixMath.mm2m(position.z)
));
}
protected static SteeringVelocity DoReachOrientation(Entity <Game> owner, Fix targetOrientation)
{
var tolerance = owner.Get <AlignTolerance>();
var steering = SteeringVelocity.Zero;
var orientation = owner.Get <Orientation>().Value;
var rotation = ArithmeticUtils.WrapAngleAroundZero(targetOrientation - orientation);
var rotationSize = FixMath.Abs(rotation);
if (rotationSize <= tolerance.Angle)
{
return(steering);
}
var maxAngularVelocity = owner.Get <MaxAngularVelocity>().Value;
var angular = maxAngularVelocity * rotation / rotationSize;
var decelerationAngle = tolerance.DecelerationAngle;
if (rotationSize <= decelerationAngle)
{
angular *= rotationSize / decelerationAngle;
}
steering.Angular = angular;
return(steering);
}
public virtual void ApplyTraction()
{
if (kineticEnergies.horizontal.X == Fix.Zero)
{
return;
}
bool startedDir = FixMath.Sign(kineticEnergies.horizontal.X) == 1 ? true : false;
if (startedDir)
{
kineticEnergies.horizontal._x -= cInfo.attributes.traction;
if (kineticEnergies.horizontal.X < Fix.Zero)
{
kineticEnergies.horizontal = FixVec2.Zero;
}
}
else
{
kineticEnergies.horizontal._x += cInfo.attributes.traction;
if (kineticEnergies.horizontal.X > Fix.Zero)
{
kineticEnergies.horizontal = FixVec2.Zero;
}
}
}
/// <summary>
/// Converts a quaternion to a euler angle.
/// https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles
/// </summary>
/// <param name="quaternion">The quaternion to convert from.</param>
/// <returns>An euler angle.</returns>
public static FixVec3 ToEuler(FixQuaternion quaternion)
{
FixVec3 result;
Fix t0 = 2 * (quaternion.w * quaternion.z + quaternion.x * quaternion.y);
Fix t1 = Fix.one - (2 * (quaternion.y * quaternion.y + quaternion.z * quaternion.z));
result.z = FixMath.Atan2(t0, t1);
Fix t2 = 2 * (quaternion.w * quaternion.y - quaternion.z * quaternion.x);
if (t2 >= Fix.one)
{
result.y = FixMath.PI / 2;
}
else if (t2 <= -Fix.one)
{
result.y = -(FixMath.PI / 2);
}
else
{
result.y = FixMath.Asin(t2);
}
Fix t3 = 2 * (quaternion.w * quaternion.x + quaternion.y * quaternion.z);
Fix t4 = Fix.one - (2 * (quaternion.x * quaternion.x + quaternion.y * quaternion.y));
result.x = FixMath.Atan2(t3, t4);
return(result);
}
public void initialize()
{
// Calculate average bounciness/restitution
e = FixMath.Min(A.Bounciness, B.Bounciness);
// Calculate static & dynamic friction
sf = FixMath.Sqrt(A.StaticFriction * A.StaticFriction + B.StaticFriction * B.StaticFriction);
df = FixMath.Sqrt(A.DynamicFriction * A.DynamicFriction + B.DynamicFriction * B.DynamicFriction);
for (int i = 0; i < contactCount; i++)
{
//Calculate radii from COM to contact
FixVec2 ra = contacts[i] -= A.info.position;
FixVec2 rb = contacts[i] -= B.info.position;
//?
FixVec2 rv = B.info.velocity + FixVec2.Cross(B.info.angularVelocity, rb)
- A.info.velocity - FixVec2.Cross(A.info.angularVelocity, ra);
// Determine if we should perform a resting collision or not
// The idea is if the only thing moving this object is gravity,
// then the collision should be performed without any restitution
if (rv.GetMagnitudeSquared() < TFPhysics.instance.resting)
{
e = 0;
}
}
}
public override SteeringVelocity DoCalculate(Entity <Game> owner, ref SteeringVelocity accumulatedSteering)
{
var steering = new SteeringVelocity();
if (!owner.Has <FlowField>())
{
return(steering);
}
var flowField = owner.Get <FlowField>().Field;
var flowVector = flowField.LookupFlowVector(owner.Get <Position>().Value);
if (!owner.Has <TargetOrientation>())
{
return(SteeringVelocity.Zero);
}
if (flowVector.MagnitudeSqr <= ZeroVelocity * ZeroVelocity)
{
return(SteeringVelocity.Zero);
}
var targetOrientation = FixMath.Atan2(-flowVector.X, flowVector.Y);
return(DoReachOrientation(owner, targetOrientation));
}
/// <summary>
/// Creates a matrix for rotating points around the Z-axis, from a center point.
/// </summary>
/// <param name="radians">The amount, in radians, by which to rotate around the Z-axis.</param>
/// <param name="centerPoint">The center point.</param>
/// <returns>The rotation matrix.</returns>
public static FixMatrix4x4 RotateZ(Fix64 radians, FixVector3 centerPoint)
{
FixMatrix4x4 result;
Fix64 c = FixMath.Cos(radians);
Fix64 s = FixMath.Sin(radians);
Fix64 x = centerPoint.x * (1 - c) + centerPoint.y * s;
Fix64 y = centerPoint.y * (1 - c) - centerPoint.x * s;
// [ c s 0 0 ]
// [ -s c 0 0 ]
// [ 0 0 1 0 ]
// [ x y 0 1 ]
result.M11 = c;
result.M12 = s;
result.M13 = Fix64.Zero;
result.M14 = Fix64.Zero;
result.M21 = -s;
result.M22 = c;
result.M23 = Fix64.Zero;
result.M24 = Fix64.Zero;
result.M31 = Fix64.Zero;
result.M32 = Fix64.Zero;
result.M33 = Fix64.One;
result.M34 = Fix64.Zero;
result.M41 = Fix64.Zero;
result.M42 = Fix64.Zero;
result.M43 = Fix64.Zero;
result.M44 = Fix64.One;
return(result);
}
/// Returns the fraction along the segment query the cpBB is hit. Returns INFINITY if it doesn't hit.
public static Fix SegmentQuery(ref AABB aabb, ref FixVec2 a, ref FixVec2 b)
{
var idx = 1.0f / (b.X - a.X);
var tx1 = (aabb.L == a.X ? Fix.MinValue : (aabb.L - a.X) * idx);
var tx2 = (aabb.R == a.X ? Fix.MaxValue : (aabb.R - a.X) * idx);
var txmin = FixMath.Min(tx1, tx2);
var txmax = FixMath.Max(tx1, tx2);
var idy = 1.0f / (b.Y - a.Y);
var ty1 = (aabb.B == a.Y ? Fix.MinValue : (aabb.B - a.Y) * idy);
var ty2 = (aabb.T == a.Y ? Fix.MaxValue : (aabb.T - a.Y) * idy);
var tymin = FixMath.Min(ty1, ty2);
var tymax = FixMath.Max(ty1, ty2);
if (tymin <= txmax && txmin <= tymax)
{
var min = FixMath.Max(txmin, tymin);
var max = FixMath.Min(txmax, tymax);
if (0 <= max && min <= 1)
{
return(FixMath.Max(min, 0));
}
}
return(Fix.MaxValue);
}
void moveVertically(ref FixVec2 deltaMovement)
{
var isGoingUp = deltaMovement.y > 0;
var rayDistance = FixMath.Abs(deltaMovement.y) + _skinWidth;
var rayDirection = isGoingUp ? FixVec2.up : -FixVec2.up;
var initialRayOrigin = isGoingUp ? _raycastOrigins.topLeft : _raycastOrigins.bottomLeft;
// apply our horizontal deltaMovement here so that we do our raycast from the actual position we would be in if we had moved
initialRayOrigin.x += deltaMovement.x;
// if we are moving up, we should ignore the layers in oneWayPlatformMask
var mask = platformMask;
if ((isGoingUp && !collisionState.wasGroundedLastFrame) || ignoreOneWayPlatformsThisFrame)
{
mask &= ~oneWayPlatformMask;
}
for (var i = 0; i < totalVerticalRays; i++)
{
FixVec2 ray = new FixVec2(initialRayOrigin.x + i * _horizontalDistanceBetweenRays, initialRayOrigin.y);
DrawRay((Vector3)ray, (Vector3)(rayDirection * rayDistance), Color.red);
_raycastHit = TFPhysics.Raycast(ray, rayDirection, rayDistance, mask);
if (_raycastHit)
{
// set our new deltaMovement and recalculate the rayDistance taking it into account
deltaMovement.y = _raycastHit.point.y - ray.y;
rayDistance = FixMath.Abs(deltaMovement.y);
// remember to remove the skinWidth from our deltaMovement
if (isGoingUp)
{
deltaMovement.y -= _skinWidth;
collisionState.above = true;
}
else
{
deltaMovement.y += _skinWidth;
collisionState.below = true;
}
_raycastHitsThisFrame.Add(_raycastHit);
// this is a hack to deal with the top of slopes. if we walk up a slope and reach the apex we can get in a situation
// where our ray gets a hit that is less then skinWidth causing us to be ungrounded the next frame due to residual velocity.
if (!isGoingUp && deltaMovement.y > Fix.zero)
{
_isGoingUpSlope = true;
}
// we add a small fudge factor for the float operations here. if our rayDistance is smaller
// than the width + fudge bail out because we have a direct impact
if (rayDistance < _skinWidth + kSkinWidthFloatFudgeFactor)
{
break;
}
}
}
}
/// <summary>
/// Creates a matrix for rotating points around the Y-axis, from a center point.
/// </summary>
/// <param name="radians">The amount, in radians, by which to rotate around the Y-axis.</param>
/// <param name="centerPoint">The center point.</param>
/// <returns>The rotation matrix.</returns>
public static FixMatrix4x4 RotateY(Fix64 radians, FixVector3 centerPoint)
{
FixMatrix4x4 result;
Fix64 c = FixMath.Cos(radians);
Fix64 s = FixMath.Sin(radians);
Fix64 x = centerPoint.x * (Fix64.One - c) - centerPoint.z * s;
Fix64 z = centerPoint.x * (Fix64.One - c) + centerPoint.x * s;
// [ c 0 -s 0 ]
// [ 0 1 0 0 ]
// [ s 0 c 0 ]
// [ x 0 z 1 ]
result.M11 = c;
result.M12 = Fix64.Zero;
result.M13 = -s;
result.M14 = Fix64.Zero;
result.M21 = Fix64.Zero;
result.M22 = Fix64.One;
result.M23 = Fix64.Zero;
result.M24 = Fix64.Zero;
result.M31 = s;
result.M32 = Fix64.Zero;
result.M33 = c;
result.M34 = Fix64.Zero;
result.M41 = x;
result.M42 = Fix64.Zero;
result.M43 = z;
result.M44 = Fix64.One;
return(result);
}
/// <summary>
/// Creates a matrix for rotating points around the Y-axis.
/// </summary>
/// <param name="radians">The amount, in radians, by which to rotate around the Y-axis.</param>
/// <returns>The rotation matrix.</returns>
public static FixMatrix4x4 RotateY(Fix64 radians)
{
FixMatrix4x4 result;
Fix64 c = FixMath.Cos(radians);
Fix64 s = FixMath.Sin(radians);
// [ c 0 -s 0 ]
// [ 0 1 0 0 ]
// [ s 0 c 0 ]
// [ 0 0 0 1 ]
result.M11 = c;
result.M12 = Fix64.Zero;
result.M13 = -s;
result.M14 = Fix64.Zero;
result.M21 = Fix64.Zero;
result.M22 = Fix64.One;
result.M23 = Fix64.Zero;
result.M24 = Fix64.Zero;
result.M31 = s;
result.M32 = Fix64.Zero;
result.M33 = c;
result.M34 = Fix64.Zero;
result.M41 = Fix64.Zero;
result.M42 = Fix64.Zero;
result.M43 = Fix64.Zero;
result.M44 = Fix64.One;
return(result);
}
public FixVec2 Rotate(Fix degree)
{
var cos = FixMath.Cos(degree);
var sin = FixMath.Sin(degree);
return(new FixVec2(X * cos - Y * sin, X * sin + Y * cos));
}
/// <summary>
/// Creates a matrix for rotating points around the X-axis, from a center point.
/// </summary>
/// <param name="radians">The amount, in radians, by which to rotate around the X-axis.</param>
/// <param name="centerPoint">The center point.</param>
/// <returns>The rotation matrix.</returns>
public static FixMatrix4x4 RotateX(Fix64 radians, FixVector3 centerPoint)
{
FixMatrix4x4 result;
Fix64 c = FixMath.Cos(radians);
Fix64 s = FixMath.Sin(radians);
Fix64 y = centerPoint.y * (Fix64.One - c) + centerPoint.z * s;
Fix64 z = centerPoint.z * (Fix64.One - c) - centerPoint.y * s;
// [ 1 0 0 0 ]
// [ 0 c s 0 ]
// [ 0 -s c 0 ]
// [ 0 y z 1 ]
result.M11 = Fix64.One;
result.M12 = Fix64.Zero;
result.M13 = Fix64.Zero;
result.M14 = Fix64.Zero;
result.M21 = Fix64.Zero;
result.M22 = c;
result.M23 = s;
result.M24 = Fix64.Zero;
result.M31 = Fix64.Zero;
result.M32 = -s;
result.M33 = c;
result.M34 = Fix64.Zero;
result.M41 = Fix64.Zero;
result.M42 = y;
result.M43 = z;
result.M44 = Fix64.One;
return(result);
}
void LateUpdate()
{
if (position != old_position)
{
old_position = position;
ApplyPosition();
}
if (angle != old_angle)
{
if (lerpRotation)
{
lerpRotationTimes = 0;
startRotation = transform_u3d.rotation;
targetRotation = Quaternion.Euler(0, FixMath.AngleToDegree(angle), 0);
}
else
{
transform_u3d.rotation = Quaternion.Euler(0, FixMath.AngleToDegree(angle), 0);
}
old_angle = angle;
}
if (scale != old_scale)
{
old_scale = scale;
ApplyScale();
}
// lerp
LerpRotation();
}
public GameSimulation(GameSettings setSttings, ConstData conConstantData)
{
m_setGameSettings = setSttings;
m_conConstantGameData = conConstantData;
//calculate queue length
int iQueueLength = (int)FixMath.Ceiling(m_setGameSettings.m_fixTargetQueueLength / m_setGameSettings.m_fixTickDelta);
//fill queue
m_frmDenseFrameQueue = new List <FrameData>(iQueueLength);
for (int i = 0; i < iQueueLength; i++)
{
m_frmDenseFrameQueue.Add(new FrameData(m_conConstantGameData.PlayerCount));
}
m_bInputsForTick = new List <byte>(m_conConstantGameData.PlayerCount);
m_ipbPlayerInputs = new List <InputBuffer>(m_conConstantGameData.PlayerCount);
for (int i = 0; i < m_conConstantGameData.PlayerCount; i++)
{
m_bInputsForTick.Add(0);
m_ipbPlayerInputs.Add(new InputBuffer(iQueueLength));
}
}
public static void CatmullRom(ref FixVector2 value1, ref FixVector2 value2, ref FixVector2 value3, ref FixVector2 value4,
Fix64 amount, out FixVector2 result)
{
result = new FixVector2(
FixMath.CatmullRom(value1.x, value2.x, value3.x, value4.x, amount),
FixMath.CatmullRom(value1.y, value2.y, value3.y, value4.y, amount));
}
public static void Barycentric(ref FixVector2 value1, ref FixVector2 value2, ref FixVector2 value3, Fix64 amount1,
Fix64 amount2, out FixVector2 result)
{
result = new FixVector2(
FixMath.Barycentric(value1.x, value2.x, value3.x, amount1, amount2),
FixMath.Barycentric(value1.y, value2.y, value3.y, amount1, amount2));
}
// The smaller of the two possible angles between the two vectors is returned, therefore the result will never be greater than 180 degrees or smaller than -180 degrees.
// If you imagine the from and to vectors as lines on a piece of paper, both originating from the same point, then the /axis/ vector would point up out of the paper.
// The measured angle between the two vectors would be positive in a clockwise direction and negative in an anti-clockwise direction.
public static Fix64 SignedAngle(FixVector3 from, FixVector3 to, FixVector3 axis)
{
FixVector3 fromNorm = from.Normalized, toNorm = to.Normalized;
Fix64 unsignedAngle = FixMath.Acos(FixMath.Clamp(Dot(fromNorm, toNorm), -Fix64.ONE, Fix64.ONE)) * FixMath.Rad2Deg;
Fix64 sign = FixMath.Sign(Dot(axis, Cross(fromNorm, toNorm)));
return(unsignedAngle * sign);
}
public static FixVector2 Lerp(FixVector2 value1, FixVector2 value2, Fix64 amount)
{
amount = FixMath.Clamp(amount, 0, 1);
return(new FixVector2(
FixMath.Lerp(value1.x, value2.x, amount),
FixMath.Lerp(value1.y, value2.y, amount)));
}
public static AABB Union(AABB a, AABB b)
{
AABB C;
C.min = new FixVec2(FixMath.Min(a.min.x, b.min.x), FixMath.Min(a.min.y, b.min.y));
C.max = new FixVec2(FixMath.Max(a.max.x, b.max.x), FixMath.Max(a.max.y, b.max.y));
return(C);
}
public void SqrtWorks(double value)
{
var expected = Math.Sqrt(value);
var result = (double)FixMath.Sqrt((Fix)value);
Assert.AreEqual(expected, result, (double)Fix.Epsilon * 4);
}
public static Fix MatrixToDegrees(Mat22 m)
{
if (m.m10 == Fix.zero && m.m00 == Fix.zero)
{
return(Fix.zero);
}
return(FixMath.Atan2(m.m10, m.m00));
}
public static FixVec2 NormalizedToPoint(FixRect rectangle, FixVec2 normalizedRectCoordinates)
{
Fix x = FixMath.Min(FixMath.Max(Fix.Zero, normalizedRectCoordinates.X), Fix.One);
Fix y = FixMath.Min(FixMath.Max(Fix.Zero, normalizedRectCoordinates.Y), Fix.One);
return(new FixVec2(rectangle.xMin + (rectangle.xMax - rectangle.xMin) * x,
rectangle.yMin + (rectangle.yMax - rectangle.yMin) * y));
}
public static AABB Union(AABB a, AABB b)
{
AABB C;
C.min = new FixVec2(FixMath.Min(a.min.X, b.min.X), FixMath.Min(a.min.Y, b.min.Y));
C.max = new FixVec2(FixMath.Max(a.max.X, b.max.X), FixMath.Max(a.max.Y, b.max.Y));
return(C);
}
public void CosWorks(double value)
{
var expected = Math.Cos(value * Math.PI / 180.0);
var result = (double)FixMath.Cos((Fix)value);
Assert.AreEqual(expected, result, (double)Fix.Epsilon);
}
public static AABB Merge(AABB a, AABB b)
{
return(new AABB(
FixMath.Min(a.L, b.L),
FixMath.Min(a.B, b.B),
FixMath.Max(a.R, b.R),
FixMath.Max(a.T, b.T)
));
}
public static AABB Expand(AABB aabb, FixVec2 v)
{
return(new AABB(
FixMath.Min(aabb.L, v.X),
FixMath.Min(aabb.B, v.Y),
FixMath.Max(aabb.R, v.X),
FixMath.Max(aabb.T, v.Y)
));
}
