public TestBox(BoxGeometry geometry, float3 translation, quaternion rotation)
{
Entity = EntityManager.CreateEntity(
typeof(PhysicsColliderBlob),
typeof(LocalToWorld)
);
Geometry = geometry;
PhysicsTransform = new PhysicsTransform(translation, rotation);
Translation = translation;
Rotation = rotation;
var colliderBlob = PhysicsBoxCollider.Create(Geometry);
Aabb = colliderBlob.Value.CalculateAabb(PhysicsTransform);
EntityManager.AddComponentData(
Entity,
new PhysicsColliderBlob {
Collider = colliderBlob
}
);
EntityManager.AddComponentData(
Entity,
new LocalToWorld {
Value = new float4x4(rotation, translation)
}
);
}
private static unsafe void DrawConvex(
Collider *collider,
ref PhysicsTransform worldTransform,
ref PhysicsDebugStreamSystem.Context outputStream,
Color colliderColor)
{
var convexCollider = (ConvexCollider *)collider;
var vertexCount = convexCollider->VertexCount;
var vertices = convexCollider->Vertices;
var convexRadius = convexCollider->m_ConvexHull.ConvexRadius;
switch (collider->ColliderType)
{
case ColliderType.Box:
case ColliderType.Polygon:
{
Assert.IsTrue(vertexCount >= 3, "ConvexCollider must have >= 3 vertices.");
outputStream.Polygon(vertices, vertexCount, worldTransform, colliderColor);
return;
}
case ColliderType.Circle:
{
Assert.AreEqual(1, vertexCount, "CircleCollider must have 1 vertex.");
var position = PhysicsMath.mul(worldTransform, vertices[0]);
outputStream.Circle(position, convexRadius, colliderColor);
return;
}
case ColliderType.Capsule:
{
Assert.AreEqual(2, vertexCount, "CapsuleCollider must have 2 vertices.");
var vertex0 = PhysicsMath.mul(worldTransform, vertices[0]);
var vertex1 = PhysicsMath.mul(worldTransform, vertices[1]);
var offset = PhysicsMath.perp(math.normalizesafe(vertex1 - vertex0)) * new float2(convexRadius);
// Side Edges.
{
outputStream.Line(vertex0 - offset, vertex1 - offset, colliderColor);
outputStream.Line(vertex0 + offset, vertex1 + offset, colliderColor);
}
// End Caps.
{
var startAngle = math.atan2(offset.y, offset.x);
var endAngle = startAngle + math.PI;
outputStream.Arc(vertex0, convexRadius, startAngle, endAngle, colliderColor);
outputStream.Arc(vertex1, convexRadius, startAngle + math.PI, endAngle + math.PI, colliderColor);
}
return;
}
default:
return;
}
}
public void Execute(ArchetypeChunk chunk, int chunkIndex, int firstEntityIndex)
{
var entities = chunk.GetNativeArray(EntityType);
var localToWorlds = chunk.GetNativeArray(LocalToWorldType);
var translations = chunk.GetNativeArray(TranslationType);
var rotations = chunk.GetNativeArray(RotationType);
var colliders = chunk.GetNativeArray(ColliderType);
var hasParentType = chunk.Has(ParentType);
var hasLocalToWorldType = chunk.Has(LocalToWorldType);
var hasTranslationType = chunk.Has(TranslationType);
var hasRotationType = chunk.Has(RotationType);
var hasColliderType = chunk.Has(ColliderType);
var worldTransform = PhysicsTransform.Identity;
var instanceCount = chunk.Count;
for (int i = 0, physicsBodyIndex = firstEntityIndex; i < instanceCount; ++i, ++physicsBodyIndex)
{
if (hasParentType)
{
if (hasLocalToWorldType)
{
var localToWorld = localToWorlds[i];
var matrix = localToWorld.Value;
var orientation = quaternion.LookRotationSafe(matrix.c2.xyz, matrix.c1.xyz);
worldTransform = new PhysicsTransform(localToWorld.Position, orientation);
}
}
else
{
if (hasTranslationType)
{
worldTransform.Translation = translations[i].Value.xy;
}
else if (hasLocalToWorldType)
{
worldTransform.Translation = localToWorlds[i].Position.xy;
}
if (hasRotationType)
{
worldTransform.SetQuaternionRotation(rotations[i].Value);
}
else if (hasLocalToWorldType)
{
var localToWorld = localToWorlds[i];
var matrix = localToWorld.Value;
worldTransform.SetQuaternionRotation(quaternion.LookRotationSafe(matrix.c2.xyz, matrix.c1.xyz));
}
}
var entity = entities[i];
PhysicsBodies[physicsBodyIndex] = new PhysicsBody
{
Collider = hasColliderType ? colliders[i].Collider : default,
public unsafe void PhysicsBodyOverlapColliderTest()
{
var geometry = new BoxGeometry
{
Size = new float2(2f),
};
using (var collider = PhysicsBoxCollider.Create(geometry))
{
var physicsBody = new PhysicsBody(collider);
var queryInput = new OverlapColliderInput()
{
Filter = CollisionFilter.Default
};
var closestHit = new OverlapColliderHit();
var allHits = new NativeList <OverlapColliderHit>(Allocator.Temp);
var circleGeometry = new CircleGeometry {
Radius = 0.5f
};
using (var circleBlob = PhysicsCircleCollider.Create(circleGeometry))
{
queryInput.Collider = circleBlob;
// OK case.
var transformOK = new PhysicsTransform(new float2(1f));
queryInput.Transform = transformOK;
Assert.IsTrue(physicsBody.OverlapCollider(queryInput));
Assert.IsTrue(physicsBody.OverlapCollider(queryInput, out closestHit));
Assert.IsTrue(physicsBody.OverlapCollider(queryInput, ref allHits));
// Fail Case.
var transformFail = new PhysicsTransform(new float2(-10f));
queryInput.Transform = transformFail;
Assert.IsFalse(physicsBody.OverlapCollider(queryInput));
Assert.IsFalse(physicsBody.OverlapCollider(queryInput, out closestHit));
Assert.IsFalse(physicsBody.OverlapCollider(queryInput, ref allHits));
}
allHits.Dispose();
}
}
public static unsafe void DrawCollider(
Collider *collider,
ref PhysicsTransform worldTransform,
ref PhysicsDebugStreamSystem.Context outputStream,
Color colliderColor)
{
switch (collider->CollisionType)
{
case CollisionType.Convex:
{
DrawConvex(collider, ref worldTransform, ref outputStream, colliderColor);
return;
}
case CollisionType.Composite:
{
DrawComposite(collider, ref worldTransform, ref outputStream, colliderColor);
return;
}
}
}
private static unsafe void DrawComposite(
Collider *collider,
ref PhysicsTransform worldTransform,
ref PhysicsDebugStreamSystem.Context outputStream,
Color colliderColor)
{
switch (collider->ColliderType)
{
case ColliderType.Compound:
{
var compoundCollider = (PhysicsCompoundCollider *)collider;
var children = compoundCollider->Children;
for (var i = 0; i < children.Length; ++i)
{
ref PhysicsCompoundCollider.Child child = ref children[i];
var colliderWorldTransform = PhysicsMath.mul(worldTransform, child.CompoundFromChild);
DrawCollider(children[i].Collider, ref colliderWorldTransform, ref outputStream, colliderColor);
}
return;
}
}
static unsafe bool CastCollider(
Ray ray, ref float2x2 rotation,
ref DistanceProxy proxySource, ref DistanceProxy proxyTarget,
out ColliderCastHit hit)
{
hit = default;
var transformSource = new PhysicsTransform
{
Translation = ray.Origin,
Rotation = rotation
};
// Check we're not initially overlapped.
if ((proxySource.VertexCount < 3 || proxyTarget.VertexCount < 3) &&
OverlapQueries.OverlapConvexConvex(ref transformSource, ref proxySource, ref proxyTarget))
{
return(false);
}
// B = Source
// A = Target
var radiusSource = proxySource.ConvexRadius;
var radiusTarget = proxyTarget.ConvexRadius;
var totalRadius = radiusSource + radiusTarget;
var invRotation = math.inverse(rotation);
var sweepDirection = ray.Displacement;
var normal = float2.zero;
var lambda = 0.0f;
// Initialize the simplex.
var simplex = new Simplex();
simplex.Count = 0;
var vertices = &simplex.Vertex1;
// Get a support point in the inverse direction.
var indexTarget = proxyTarget.GetSupport(-sweepDirection);
var supportTarget = proxyTarget.Vertices[indexTarget];
var indexSource = proxySource.GetSupport(PhysicsMath.mul(invRotation, sweepDirection));
var supportSource = PhysicsMath.mul(transformSource, proxySource.Vertices[indexSource]);
var v = supportTarget - supportSource;
// Sigma is the target distance between polygons
var sigma = math.max(PhysicsSettings.Constants.MinimumConvexRadius, totalRadius - PhysicsSettings.Constants.MinimumConvexRadius);
const float tolerance = PhysicsSettings.Constants.LinearSlop * 0.5f;
var iteration = 0;
while (
iteration++ < PhysicsSettings.Constants.MaxGJKInterations &&
math.abs(math.length(v) - sigma) > tolerance
)
{
if (simplex.Count >= 3)
{
SafetyChecks.ThrowInvalidOperationException("ColliderCast Simplex must have less than 3 vertex.");
}
// Support in direction -supportV (Target - Source)
indexTarget = proxyTarget.GetSupport(-v);
supportTarget = proxyTarget.Vertices[indexTarget];
indexSource = proxySource.GetSupport(PhysicsMath.mul(invRotation, v));
supportSource = PhysicsMath.mul(transformSource, proxySource.Vertices[indexSource]);
var p = supportTarget - supportSource;
v = math.normalizesafe(v);
// Intersect ray with plane.
var vp = math.dot(v, p);
var vr = math.dot(v, sweepDirection);
if (vp - sigma > lambda * vr)
{
if (vr <= 0.0f)
{
return(false);
}
lambda = (vp - sigma) / vr;
if (lambda > 1.0f)
{
return(false);
}
normal = -v;
simplex.Count = 0;
}
// Reverse simplex since it works with B - A.
// Shift by lambda * r because we want the closest point to the current clip point.
// Note that the support point p is not shifted because we want the plane equation
// to be formed in un-shifted space.
var vertex = vertices + simplex.Count;
vertex->IndexA = indexSource;
vertex->SupportA = supportSource + lambda * sweepDirection;
vertex->IndexB = indexTarget;
vertex->SupportB = supportTarget;
vertex->W = vertex->SupportB - vertex->SupportA;
vertex->A = 1.0f;
simplex.Count += 1;
switch (simplex.Count)
{
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)
{
// Overlap.
return(false);
}
// Get search direction.
v = simplex.GetClosestPoint();
}
// Ensure we don't process an empty simplex.
if (simplex.Count == 0)
{
return(false);
}
// Prepare result.
var pointSource = float2.zero;
var pointTarget = float2.zero;
simplex.GetWitnessPoints(ref pointSource, ref pointTarget);
normal = math.normalizesafe(-v);
hit = new ColliderCastHit
{
Position = pointTarget + (normal * radiusTarget),
SurfaceNormal = normal,
Fraction = lambda
};
return(true);
}
public override void Ready()
{
manager = GetManager<PhysicsManager>();
manager.Add(this);
transformComponent = GetComponent<TransformComponent>();
transform = new PhysicsTransform(transformComponent.Transform);
transformComponent.Transform = transform;
CreateBody();
transform.Body = body;
if (def.Speed != 0)
{
float velocityX = (float)Math.Cos(PhysicsManager.DegreesToRadians(transform.Rotation)) * def.Speed;
float velocityY = (float)Math.Sin(PhysicsManager.DegreesToRadians(transform.Rotation)) * def.Speed;
body.LinearVelocity = new Vector2(velocityX, velocityY);
}
}
public unsafe void Polygon(ConvexHull.ConvexArray.Accessor vertices, int vertexCount, PhysicsTransform physicsTransform, Color color)
{
SafetyChecks.IsTrue(vertexCount <= PhysicsPolygonCollider.Constants.MaxVertexCount);
Writer.Write(Type.Polygon);
var polygon = new Polygon
{
Transform = physicsTransform,
VertexCount = vertexCount,
Color = color
};
UnsafeUtility.MemCpy(polygon.Vertices, vertices.GetUnsafePtr(), sizeof(float2) * vertexCount);
Writer.Write(polygon);
}
// Use this for initialization
void Start()
{
_velocity = DVector3.zero;
_ptransform = gameObject.AddComponent <PhysicsTransform>();
}
