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 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);
}
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 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 void SqrtAverageDeviationTest()
{
var maxDiff = double.MinValue;
for (var i = 0; i <= 1000; i++)
{
var diff = FixMath.Sqrt(i).AsDouble - Math.Sqrt(i);
maxDiff = Math.Max(maxDiff, Math.Abs(diff));
}
FixAssert.AssertEquals(maxDiff, 0, Epsilon);
}
public static DynValue sqrt(ScriptExecutionContext executionContext, CallbackArguments args)
{
return(exec1(args, "sqrt", (x) => FixMath.Sqrt(x)));
}
/// <summary>
/// Returns the length of this quaternion.
/// </summary>
/// <returns>The length.</returns>
public Fix Magnitude()
{
return(FixMath.Sqrt(x * x + y * y + z * z + w * w));
}