public Aabb CalculateAabb(PhysicsTransform transform)
{
return(Collider.IsCreated ?
Collider.Value.CalculateAabb(PhysicsMath.mul(transform, WorldTransform)) :
new Aabb {
Min = WorldTransform.Translation, Max = WorldTransform.Translation
});
}
internal static unsafe bool PointDistance <T>(PointDistanceInput input, Collider *collider, ref T collector) where T : struct, ICollector <DistanceHit>
{
if (!CollisionFilter.IsCollisionEnabled(input.Filter, collider->Filter))
{
return(false);
}
// Ensure the query context is initialized.
input.QueryContext.EnsureIsInitialized();
var proxySource = new DistanceProxy(1, &input.Position, 0f);
DistanceProxy proxyTarget;
switch (collider->ColliderType)
{
case ColliderType.Box:
case ColliderType.Polygon:
case ColliderType.Capsule:
case ColliderType.Circle:
{
var convexCollider = (ConvexCollider *)collider;
proxyTarget = new DistanceProxy(ref convexCollider->m_ConvexHull);
break;
}
case ColliderType.Compound:
return(PointDistanceCompound(input, (PhysicsCompoundCollider *)collider, ref collector));
default:
SafetyChecks.ThrowNotImplementedException();
return(default);
}
var hit = ColliderDistance(PhysicsTransform.Identity, ref proxySource, ref proxyTarget);
if (hit.Distance < collector.MaxFraction)
{
hit.PhysicsBodyIndex = input.QueryContext.PhysicsBodyIndex;
hit.ColliderKey = input.QueryContext.ColliderKey;
hit.Entity = input.QueryContext.Entity;
hit.PointA = PhysicsMath.mul(input.QueryContext.LocalToWorldTransform, hit.PointA);
hit.PointB = PhysicsMath.mul(input.QueryContext.LocalToWorldTransform, hit.PointB);
return(collector.AddHit(hit));
}
return(false);
}
internal static void Execute(int index, NativeArray <PhysicsBody.MotionData> motionDatas, NativeArray <PhysicsBody> physicsbodies)
{
var motionData = motionDatas[index];
var physicsBody = physicsbodies[index];
var rotation = float2x2.Rotate(motionData.WorldAngle);
var translation = motionData.WorldPosition - PhysicsMath.mul(rotation, motionData.LocalCenterOfMass);
physicsBody.WorldTransform = new PhysicsTransform
{
Rotation = rotation,
Translation = translation
};
physicsbodies[index] = physicsBody;
}
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(PhysicsMath.mul(transform, Broadphase.Domain));
}
public void SetQuaternionRotation(quaternion rotation)
{
Rotation = float2x2.Rotate(PhysicsMath.ZRotationFromQuaternion(rotation));
}
public PhysicsTransform(RigidTransform rigidTransform)
{
Translation = rigidTransform.pos.xy;
Rotation = float2x2.Rotate(PhysicsMath.ZRotationFromQuaternion(rigidTransform.rot));
}
public PhysicsTransform(float3 translation, quaternion rotation)
{
Translation = translation.xy;
Rotation = float2x2.Rotate(PhysicsMath.ZRotationFromQuaternion(rotation));
}
internal static unsafe bool OverlapPoint <T>(OverlapPointInput input, Collider *collider, ref T collector) where T : struct, ICollector <OverlapPointHit>
{
// Nothing to do if:
// - MaxFraction is zero.
// - Filtered out.
if (math.abs(collector.MaxFraction) < 0f ||
!CollisionFilter.IsCollisionEnabled(input.Filter, collider->Filter))
{
return(false);
}
// Ensure the query context is initialized.
input.QueryContext.EnsureIsInitialized();
bool hadHit;
switch (collider->ColliderType)
{
case ColliderType.Box:
{
var box = (PhysicsBoxCollider *)collider;
hadHit = PointConvex(input.Position, ref box->m_ConvexHull);
break;
}
case ColliderType.Polygon:
{
var polygon = (PhysicsPolygonCollider *)collider;
hadHit = PointConvex(input.Position, ref polygon->m_ConvexHull);
break;
}
case ColliderType.Capsule:
{
var capsule = (PhysicsCapsuleCollider *)collider;
hadHit = PointCapsule(input.Position, capsule->Vertex0, capsule->Vertex1, capsule->Radius);
break;
}
case ColliderType.Circle:
{
var circle = (PhysicsCircleCollider *)collider;
hadHit = PointCircle(input.Position, circle->Center, circle->Radius);
break;
}
case ColliderType.Compound:
{
return(PointCompound(input, (PhysicsCompoundCollider *)collider, ref collector));
}
default:
SafetyChecks.ThrowNotImplementedException();
return(default);
}
if (hadHit)
{
var hit = new OverlapPointHit
{
Fraction = 0f,
Position = PhysicsMath.mul(input.QueryContext.LocalToWorldTransform, input.Position),
PhysicsBodyIndex = input.QueryContext.PhysicsBodyIndex,
ColliderKey = input.QueryContext.ColliderKey,
Entity = input.QueryContext.Entity
};
return(collector.AddHit(hit));
}
return(false);
}
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 unsafe void SetAndGiftWrap(NativeArray <float2> points)
{
SafetyChecks.IsTrue(Length == points.Length);
// Find rightmost point.
var maxX = points[0].x;
var maxIndex = 0;
for (var i = 0; i < Length; ++i)
{
var vertex = points[i];
var x = vertex.x;
if (x > maxX || (x == maxX && vertex.y < points[maxIndex].y))
{
maxIndex = i;
maxX = x;
}
}
// Find convex hull.
var hullIndices = new NativeArray <int>(Length, Allocator.Temp, NativeArrayOptions.UninitializedMemory);
var m = 0;
var ih = maxIndex;
while (true)
{
SafetyChecks.IsTrue(m < Length);
hullIndices[m] = ih;
var ie = 0;
for (var j = 1; j < Length; ++j)
{
if (ie == ih)
{
ie = j;
continue;
}
var r = points[ie] - points[hullIndices[m]];
var v = points[j] - points[hullIndices[m]];
var crossEdge = PhysicsMath.cross(r, v);
// Check hull point or being collinear.
if (crossEdge < 0f || (math.abs(crossEdge) < float.Epsilon && math.lengthsq(v) > math.lengthsq(r)))
{
ie = j;
}
}
++m;
ih = ie;
if (ie == maxIndex)
{
break;
}
}
// Trim lengths for vertices and normals.
Length = m_Vertices.Length = m_Normals.Length = m;
// Copy hull vertices.
var vertices = Vertices.GetUnsafePtr();
for (var i = 0; i < Length; ++i)
{
vertices[i] = points[hullIndices[i]];
}
hullIndices.Dispose();
// Calculate normals.
var normals = Normals.GetUnsafePtr();
for (var i = 0; i < Length; ++i)
{
var i1 = i;
var i2 = i + 1 < Length ? i + 1 : 0;
var edge = vertices[i2] - vertices[i1];
SafetyChecks.IsTrue(math.lengthsq(edge) > float.Epsilon);
normals[i] = math.normalize(PhysicsMath.cross(edge, 1.0f));
}
}
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));
}
/// <summary>
/// Compute axis and pivot of a given range.
/// </summary>
/// <param name="range"></param>
/// <param name="axis"></param>
/// <param name="pivot"></param>
static void ComputeAxisAndPivot(ref Range range, out int axis, out float pivot)
{
// Compute axis and pivot.
axis = PhysicsMath.IndexOfMaxComponent(range.Domain.Extents);
pivot = ((range.Domain.Min + range.Domain.Max) * 0.5f)[axis];
}
