public static PhysicsTransform mul(PhysicsTransform transform1, PhysicsTransform transform2)
{
return(new PhysicsTransform
{
Translation = math.mul(transform1.Rotation, transform2.Translation) + transform1.Translation,
Rotation = math.mul(transform1.Rotation, transform2.Rotation)
});
}
public Aabb CalculateAabb(PhysicsTransform transform)
{
return(Collider.IsCreated ?
Collider.Value.CalculateAabb(PhysicsMath.mul(transform, WorldTransform)) :
new Aabb {
Min = WorldTransform.Translation, Max = WorldTransform.Translation
});
}
public static PhysicsTransform inverse(PhysicsTransform transform)
{
var inverseRotation = math.transpose(transform.Rotation);
return(new PhysicsTransform
{
Translation = math.mul(inverseRotation, -transform.Translation),
Rotation = inverseRotation
});
}
public QueryContext(int physicsBodyIndex, Entity entity, PhysicsTransform localToWorldTransform)
{
PhysicsBodyIndex = physicsBodyIndex;
Entity = entity;
LocalToWorldTransform = localToWorldTransform;
ColliderKey = ColliderKey.Empty;
NumColliderKeyBits = 0;
m_IsInitialized = 1;
}
public static Aabb mul(PhysicsTransform transform, Aabb aabb)
{
var halfExtents = aabb.HalfExtents;
var transformedHalfExtents = math.abs(transform.Rotation.c0 * halfExtents.x) + math.abs(transform.Rotation.c1 * halfExtents.y);
var transformedCenter = mul(transform, aabb.Center);
return(new Aabb
{
Min = transformedCenter - transformedHalfExtents,
Max = transformedCenter + transformedHalfExtents
});
}
public unsafe Aabb CalculateAabb(PhysicsTransform transform)
{
var min = new float2(float.MaxValue);
var max = new float2(float.MinValue);
var vertices = Vertices.GetUnsafePtr();
for (var i = 0; i < Length; ++i, ++vertices)
{
min = math.min(min, PhysicsMath.mul(transform, *vertices));
max = math.max(max, PhysicsMath.mul(transform, *vertices));
}
return(new Aabb
{
Min = min,
Max = max
});
}
public Aabb CalculateAabb(PhysicsTransform transform)
{
return(CollisionWorld.CalculateAabb(transform));
}
public Aabb CalculateAabb(PhysicsTransform transform)
{
return(PhysicsMath.mul(transform, Broadphase.Domain));
}
public static float2 mul(PhysicsTransform transform, float2 value)
{
return(math.mul(transform.Rotation, value) + transform.Translation);
}
public PhysicsBody(BlobAssetReference <Collider> colliderBlob)
{
Collider = colliderBlob;
WorldTransform = PhysicsTransform.Identity;
Entity = Entity.Null;
}
internal static unsafe DistanceHit ColliderDistance(PhysicsTransform transformA, ref DistanceProxy proxyA, ref DistanceProxy proxyB)
{
var simplex = new Simplex();
simplex.Reset(transformA, proxyA, proxyB);
var inverseRotationA = math.transpose(transformA.Rotation);
var vertices = &simplex.Vertex1;
Simplex.VertexIndexTriple saveA;
Simplex.VertexIndexTriple saveB;
var iteration = 0;
while (iteration < PhysicsSettings.Constants.MaxGJKInterations)
{
// Copy simplex so we can identify duplicates.
var saveCount = simplex.Count;
for (var i = 0; i < saveCount; ++i)
{
saveA.Index[i] = vertices[i].IndexA;
saveB.Index[i] = vertices[i].IndexB;
}
switch (saveCount)
{
case 1:
break;
case 2:
simplex.Solve2();
break;
case 3:
simplex.Solve3();
break;
default:
SafetyChecks.ThrowInvalidOperationException("Simplex has invalid count.");
return(default);
}
// If we have 3 points, then the origin is in the corresponding triangle.
if (simplex.Count == 3)
{
break;
}
// Get search direction.
var direction = simplex.GetSearchDirection();
// Ensure the search direction is numerically fit.
if (math.lengthsq(direction) < float.Epsilon * float.Epsilon)
{
// The origin is probably contained by a line segment
// or triangle. Thus the shapes are overlapped.
// We can't return zero here even though there may be overlap.
// In case the simplex is a point, segment, or triangle it is difficult
// to determine if the origin is contained in the CSO or very close to it.
break;
}
// Compute a tentative new simplex vertex using support points.
var vertex = vertices + simplex.Count;
vertex->IndexA = proxyA.GetSupport(PhysicsMath.mul(inverseRotationA, -direction));
vertex->SupportA = PhysicsMath.mul(transformA, proxyA.Vertices[vertex->IndexA]);
vertex->IndexB = proxyB.GetSupport(direction);
vertex->SupportB = proxyB.Vertices[vertex->IndexB];
vertex->W = vertex->SupportB - vertex->SupportA;
// Iteration count is equated to the number of support point calls.
++iteration;
// Check for duplicate support points. This is the main termination criteria.
var duplicate = false;
for (var i = 0; i < saveCount; ++i)
{
if (vertex->IndexA == saveA.Index[i] && vertex->IndexB == saveB.Index[i])
{
duplicate = true;
break;
}
}
// If we found a duplicate support point we must exit to avoid cycling.
if (duplicate)
{
break;
}
// New vertex is okay and needed.
simplex.Count++;
}
// Prepare result.
var pointA = float2.zero;
var pointB = float2.zero;
simplex.GetWitnessPoints(ref pointA, ref pointB);
var distance = math.distance(pointA, pointB);
var radiusA = proxyA.ConvexRadius;
var radiusB = proxyB.ConvexRadius;
if (distance > (radiusA + radiusB) && distance > float.Epsilon)
{
// Shapes not overlapped.
// Move the witness points to the outer surface.
distance -= radiusA + radiusB;
var normal = math.normalize(pointB - pointA);
pointA += radiusA * normal;
pointB -= radiusB * normal;
}
else
{
// Shapes are overlapped.
// Move the witness points to the middle.
pointA = pointB = 0.5f * (pointA + pointB);
distance = 0f;
}
return(new DistanceHit
{
PointA = pointA,
PointB = pointB,
Fraction = distance
});
}
public Aabb CalculateAabb(PhysicsTransform transform)
{
// TODO: Store a convex hull wrapping all the children, and use that to calculate tighter AABBs?
return(PhysicsMath.mul(transform, BoundingVolumeHierarchy.Domain));
}
