845 lines
25 KiB
C#
845 lines
25 KiB
C#
using System;
|
|
using System.Collections.Generic;
|
|
|
|
[Serializable]
|
|
public sealed class YmirStepRequest
|
|
{
|
|
public float deltaTime;
|
|
public YmirWorld world;
|
|
}
|
|
|
|
[Serializable]
|
|
public sealed class YmirStepResult
|
|
{
|
|
public YmirWorld world;
|
|
public YmirContactEvent[] contacts;
|
|
}
|
|
|
|
[Serializable]
|
|
public sealed class YmirSphereOverlapRequest
|
|
{
|
|
public YmirVec3 center;
|
|
public float radius;
|
|
public YmirSphereQueryBody[] bodies;
|
|
}
|
|
|
|
[Serializable]
|
|
public sealed class YmirSphereOverlapResult
|
|
{
|
|
public YmirSphereOverlapHit[] hits;
|
|
}
|
|
|
|
[Serializable]
|
|
public sealed class YmirSphereOverlapHit
|
|
{
|
|
public string bodyId;
|
|
public YmirVec3 point;
|
|
public YmirVec3 normal;
|
|
public float penetration;
|
|
public float distance;
|
|
}
|
|
|
|
[Serializable]
|
|
public sealed class YmirCircleCastRequest
|
|
{
|
|
public YmirVec2 origin;
|
|
public YmirVec2 direction;
|
|
public float distance;
|
|
public float radius;
|
|
public YmirWorld world;
|
|
}
|
|
|
|
[Serializable]
|
|
public sealed class YmirCircleOverlapRequest
|
|
{
|
|
public YmirVec2 center;
|
|
public float radius;
|
|
public YmirWorld world;
|
|
}
|
|
|
|
[Serializable]
|
|
public sealed class YmirCircleOverlapResult
|
|
{
|
|
public YmirCircleOverlapHit[] hits;
|
|
}
|
|
|
|
[Serializable]
|
|
public sealed class YmirCircleOverlapHit
|
|
{
|
|
public string bodyId;
|
|
public YmirVec2 point;
|
|
public YmirVec2 normal;
|
|
public float penetration;
|
|
public float distance;
|
|
}
|
|
|
|
[Serializable]
|
|
public sealed class YmirCircleCastResult
|
|
{
|
|
public YmirCircleCastHit[] hits;
|
|
}
|
|
|
|
[Serializable]
|
|
public sealed class YmirCircleCastHit
|
|
{
|
|
public string bodyId;
|
|
public YmirVec2 point;
|
|
public YmirVec2 normal;
|
|
public float distance;
|
|
}
|
|
|
|
[Serializable]
|
|
public sealed class YmirSphereCastRequest
|
|
{
|
|
public YmirVec3 origin;
|
|
public YmirVec3 direction;
|
|
public float distance;
|
|
public float radius;
|
|
public YmirSphereQueryBody[] bodies;
|
|
}
|
|
|
|
[Serializable]
|
|
public sealed class YmirSphereCastResult
|
|
{
|
|
public YmirSphereCastHit[] hits;
|
|
}
|
|
|
|
[Serializable]
|
|
public sealed class YmirSphereCastHit
|
|
{
|
|
public string bodyId;
|
|
public YmirVec3 point;
|
|
public YmirVec3 normal;
|
|
public float distance;
|
|
}
|
|
|
|
[Serializable]
|
|
public sealed class YmirWorld
|
|
{
|
|
public float time;
|
|
public YmirPhysicsBody[] bodies;
|
|
public YmirRadialField[] fields;
|
|
}
|
|
|
|
[Serializable]
|
|
public sealed class YmirSphereQueryWorld
|
|
{
|
|
public YmirSphereQueryBody[] bodies;
|
|
}
|
|
|
|
[Serializable]
|
|
public sealed class YmirSphereQueryBody
|
|
{
|
|
public string id;
|
|
public YmirVec3 position;
|
|
public float radius;
|
|
}
|
|
|
|
[Serializable]
|
|
public sealed class YmirPhysicsBody
|
|
{
|
|
public string id;
|
|
public YmirVec2 position;
|
|
public YmirVec2 velocity;
|
|
public YmirVec2 direction;
|
|
public float angularVelocity;
|
|
public float torque;
|
|
public float momentOfInertia;
|
|
public float radius;
|
|
public float mass;
|
|
public bool isStatic;
|
|
public float restitution;
|
|
}
|
|
|
|
[Serializable]
|
|
public sealed class YmirRadialField
|
|
{
|
|
public string id;
|
|
public YmirVec2 position;
|
|
public float strength;
|
|
public float radius;
|
|
}
|
|
|
|
[Serializable]
|
|
public sealed class YmirContactEvent
|
|
{
|
|
public string bodyA;
|
|
public string bodyB;
|
|
public YmirVec2 point;
|
|
public YmirVec2 normal;
|
|
public float penetration;
|
|
public float relativeSpeed;
|
|
}
|
|
|
|
[Serializable]
|
|
public struct YmirVec2
|
|
{
|
|
public float x;
|
|
public float y;
|
|
}
|
|
|
|
[Serializable]
|
|
public struct YmirVec3
|
|
{
|
|
public float x;
|
|
public float y;
|
|
public float z;
|
|
}
|
|
|
|
public static class YmirPhysicsQueries
|
|
{
|
|
private const float Epsilon = 0.000001f;
|
|
|
|
public static YmirStepResult Step(YmirStepRequest request)
|
|
{
|
|
if (request == null || request.world == null)
|
|
{
|
|
return new YmirStepResult
|
|
{
|
|
world = new YmirWorld
|
|
{
|
|
time = 0,
|
|
bodies = Array.Empty<YmirPhysicsBody>(),
|
|
fields = Array.Empty<YmirRadialField>()
|
|
},
|
|
contacts = Array.Empty<YmirContactEvent>()
|
|
};
|
|
}
|
|
|
|
return Step(request.world, request.deltaTime);
|
|
}
|
|
|
|
public static YmirStepResult Step(YmirWorld world, float deltaTime)
|
|
{
|
|
var bodies = CopyBodies(world?.bodies);
|
|
var fields = CopyFields(world?.fields);
|
|
var steppedWorld = new YmirWorld
|
|
{
|
|
time = world == null ? 0 : world.time + Math.Max(0, deltaTime),
|
|
bodies = bodies,
|
|
fields = fields
|
|
};
|
|
|
|
if (world == null || bodies.Length == 0 || deltaTime <= 0)
|
|
{
|
|
return new YmirStepResult
|
|
{
|
|
world = steppedWorld,
|
|
contacts = Array.Empty<YmirContactEvent>()
|
|
};
|
|
}
|
|
|
|
ApplyFields(bodies, fields, deltaTime);
|
|
IntegrateBodies(bodies, deltaTime);
|
|
var contacts = ResolveContacts(bodies);
|
|
return new YmirStepResult
|
|
{
|
|
world = steppedWorld,
|
|
contacts = contacts
|
|
};
|
|
}
|
|
|
|
public static YmirSphereOverlapResult OverlapSphere(YmirSphereOverlapRequest request)
|
|
{
|
|
return new YmirSphereOverlapResult
|
|
{
|
|
hits = request == null
|
|
? Array.Empty<YmirSphereOverlapHit>()
|
|
: OverlapSphere(request.bodies, request.center, request.radius)
|
|
};
|
|
}
|
|
|
|
public static YmirSphereOverlapHit[] OverlapSphere(
|
|
IReadOnlyList<YmirSphereQueryBody> bodies,
|
|
YmirVec3 center,
|
|
float radius)
|
|
{
|
|
var hits = new List<YmirSphereOverlapHit>();
|
|
OverlapSphere(bodies, center, radius, hits);
|
|
return hits.Count == 0 ? Array.Empty<YmirSphereOverlapHit>() : hits.ToArray();
|
|
}
|
|
|
|
public static int OverlapSphere(
|
|
IReadOnlyList<YmirSphereQueryBody> bodies,
|
|
YmirVec3 center,
|
|
float radius,
|
|
List<YmirSphereOverlapHit> hits)
|
|
{
|
|
if (hits == null)
|
|
throw new ArgumentNullException(nameof(hits));
|
|
|
|
hits.Clear();
|
|
if (bodies == null || bodies.Count == 0 || radius < 0)
|
|
return 0;
|
|
|
|
for (var i = 0; i < bodies.Count; i++)
|
|
{
|
|
var body = bodies[i];
|
|
if (body == null || body.radius < 0)
|
|
continue;
|
|
|
|
var delta = Subtract(body.position, center);
|
|
var centerDistanceSquared = LengthSquared(delta);
|
|
var combinedRadius = radius + body.radius;
|
|
if (centerDistanceSquared > combinedRadius * combinedRadius)
|
|
continue;
|
|
|
|
var centerDistance = (float)Math.Sqrt(centerDistanceSquared);
|
|
var normal = centerDistance > Epsilon
|
|
? Scale(delta, 1.0f / centerDistance)
|
|
: new YmirVec3 { x = 1, y = 0, z = 0 };
|
|
|
|
hits.Add(new YmirSphereOverlapHit
|
|
{
|
|
bodyId = body.id,
|
|
point = Subtract(body.position, Scale(normal, body.radius)),
|
|
normal = normal,
|
|
penetration = combinedRadius - centerDistance,
|
|
distance = Math.Max(0, centerDistance - body.radius)
|
|
});
|
|
}
|
|
|
|
hits.Sort((left, right) => CompareHitOrder(left.distance, left.bodyId, right.distance, right.bodyId));
|
|
return hits.Count;
|
|
}
|
|
|
|
public static YmirSphereCastResult CastSphere(YmirSphereCastRequest request)
|
|
{
|
|
return new YmirSphereCastResult
|
|
{
|
|
hits = request == null
|
|
? Array.Empty<YmirSphereCastHit>()
|
|
: CastSphere(request.bodies, request.origin, request.direction, request.distance, request.radius)
|
|
};
|
|
}
|
|
|
|
public static YmirCircleCastResult CastCircle(YmirCircleCastRequest request)
|
|
{
|
|
return new YmirCircleCastResult
|
|
{
|
|
hits = request == null || request.world == null
|
|
? Array.Empty<YmirCircleCastHit>()
|
|
: CastCircle(request.world.bodies, request.origin, request.direction, request.distance, request.radius)
|
|
};
|
|
}
|
|
|
|
public static YmirCircleOverlapResult OverlapCircle(YmirCircleOverlapRequest request)
|
|
{
|
|
return new YmirCircleOverlapResult
|
|
{
|
|
hits = request == null || request.world == null
|
|
? Array.Empty<YmirCircleOverlapHit>()
|
|
: OverlapCircle(request.world.bodies, request.center, request.radius)
|
|
};
|
|
}
|
|
|
|
public static YmirCircleOverlapHit[] OverlapCircle(
|
|
IReadOnlyList<YmirPhysicsBody> bodies,
|
|
YmirVec2 center,
|
|
float radius)
|
|
{
|
|
var hits = new List<YmirCircleOverlapHit>();
|
|
OverlapCircle(bodies, center, radius, hits);
|
|
return hits.Count == 0 ? Array.Empty<YmirCircleOverlapHit>() : hits.ToArray();
|
|
}
|
|
|
|
public static int OverlapCircle(
|
|
IReadOnlyList<YmirPhysicsBody> bodies,
|
|
YmirVec2 center,
|
|
float radius,
|
|
List<YmirCircleOverlapHit> hits)
|
|
{
|
|
if (hits == null)
|
|
throw new ArgumentNullException(nameof(hits));
|
|
|
|
hits.Clear();
|
|
if (bodies == null || bodies.Count == 0 || radius < 0)
|
|
return 0;
|
|
|
|
for (var i = 0; i < bodies.Count; i++)
|
|
{
|
|
var body = bodies[i];
|
|
if (body == null || body.radius < 0)
|
|
continue;
|
|
|
|
var delta = Subtract(body.position, center);
|
|
var centerDistanceSquared = LengthSquared(delta);
|
|
var combinedRadius = radius + body.radius;
|
|
if (centerDistanceSquared > combinedRadius * combinedRadius)
|
|
continue;
|
|
|
|
var centerDistance = (float)Math.Sqrt(centerDistanceSquared);
|
|
var normal = centerDistance > Epsilon
|
|
? Scale(delta, 1.0f / centerDistance)
|
|
: new YmirVec2 { x = 1, y = 0 };
|
|
|
|
hits.Add(new YmirCircleOverlapHit
|
|
{
|
|
bodyId = body.id,
|
|
point = Subtract(body.position, Scale(normal, body.radius)),
|
|
normal = normal,
|
|
penetration = combinedRadius - centerDistance,
|
|
distance = Math.Max(0, centerDistance - body.radius)
|
|
});
|
|
}
|
|
|
|
hits.Sort((left, right) => CompareHitOrder(left.distance, left.bodyId, right.distance, right.bodyId));
|
|
return hits.Count;
|
|
}
|
|
|
|
public static YmirCircleCastHit[] CastCircle(
|
|
IReadOnlyList<YmirPhysicsBody> bodies,
|
|
YmirVec2 origin,
|
|
YmirVec2 direction,
|
|
float distance,
|
|
float radius)
|
|
{
|
|
var hits = new List<YmirCircleCastHit>();
|
|
CastCircle(bodies, origin, direction, distance, radius, hits);
|
|
return hits.Count == 0 ? Array.Empty<YmirCircleCastHit>() : hits.ToArray();
|
|
}
|
|
|
|
public static int CastCircle(
|
|
IReadOnlyList<YmirPhysicsBody> bodies,
|
|
YmirVec2 origin,
|
|
YmirVec2 direction,
|
|
float distance,
|
|
float radius,
|
|
List<YmirCircleCastHit> hits)
|
|
{
|
|
if (hits == null)
|
|
throw new ArgumentNullException(nameof(hits));
|
|
|
|
hits.Clear();
|
|
var directionLengthSquared = LengthSquared(direction);
|
|
if (bodies == null || bodies.Count == 0 || distance < 0 || radius < 0 || directionLengthSquared <= Epsilon)
|
|
return 0;
|
|
|
|
var directionLength = (float)Math.Sqrt(directionLengthSquared);
|
|
var rayDirection = Scale(direction, 1.0f / directionLength);
|
|
for (var i = 0; i < bodies.Count; i++)
|
|
{
|
|
var body = bodies[i];
|
|
if (body == null || body.radius < 0)
|
|
continue;
|
|
|
|
var toOrigin = Subtract(origin, body.position);
|
|
var combinedRadius = radius + body.radius;
|
|
var c = LengthSquared(toOrigin) - combinedRadius * combinedRadius;
|
|
float hitDistance;
|
|
if (c <= 0)
|
|
{
|
|
hitDistance = 0;
|
|
}
|
|
else
|
|
{
|
|
var b = Dot(toOrigin, rayDirection);
|
|
if (b > 0)
|
|
continue;
|
|
|
|
var discriminant = b * b - c;
|
|
if (discriminant < 0)
|
|
continue;
|
|
|
|
hitDistance = -b - (float)Math.Sqrt(discriminant);
|
|
if (hitDistance < 0 || hitDistance > distance)
|
|
continue;
|
|
}
|
|
|
|
var castCenter = Add(origin, Scale(rayDirection, hitDistance));
|
|
var delta = Subtract(body.position, castCenter);
|
|
var centerDistanceSquared = LengthSquared(delta);
|
|
var normal = centerDistanceSquared > Epsilon
|
|
? Scale(delta, 1.0f / (float)Math.Sqrt(centerDistanceSquared))
|
|
: Scale(rayDirection, -1);
|
|
|
|
hits.Add(new YmirCircleCastHit
|
|
{
|
|
bodyId = body.id,
|
|
point = Subtract(body.position, Scale(normal, body.radius)),
|
|
normal = normal,
|
|
distance = hitDistance
|
|
});
|
|
}
|
|
|
|
hits.Sort((left, right) => CompareHitOrder(left.distance, left.bodyId, right.distance, right.bodyId));
|
|
return hits.Count;
|
|
}
|
|
|
|
public static YmirSphereCastHit[] CastSphere(
|
|
IReadOnlyList<YmirSphereQueryBody> bodies,
|
|
YmirVec3 origin,
|
|
YmirVec3 direction,
|
|
float distance,
|
|
float radius)
|
|
{
|
|
var hits = new List<YmirSphereCastHit>();
|
|
CastSphere(bodies, origin, direction, distance, radius, hits);
|
|
return hits.Count == 0 ? Array.Empty<YmirSphereCastHit>() : hits.ToArray();
|
|
}
|
|
|
|
public static int CastSphere(
|
|
IReadOnlyList<YmirSphereQueryBody> bodies,
|
|
YmirVec3 origin,
|
|
YmirVec3 direction,
|
|
float distance,
|
|
float radius,
|
|
List<YmirSphereCastHit> hits)
|
|
{
|
|
if (hits == null)
|
|
throw new ArgumentNullException(nameof(hits));
|
|
|
|
hits.Clear();
|
|
var directionLengthSquared = LengthSquared(direction);
|
|
if (bodies == null || bodies.Count == 0 || distance < 0 || radius < 0 || directionLengthSquared <= Epsilon)
|
|
return 0;
|
|
|
|
var directionLength = (float)Math.Sqrt(directionLengthSquared);
|
|
var rayDirection = Scale(direction, 1.0f / directionLength);
|
|
for (var i = 0; i < bodies.Count; i++)
|
|
{
|
|
var body = bodies[i];
|
|
if (body == null || body.radius < 0)
|
|
continue;
|
|
|
|
var toOrigin = Subtract(origin, body.position);
|
|
var combinedRadius = radius + body.radius;
|
|
var c = LengthSquared(toOrigin) - combinedRadius * combinedRadius;
|
|
float hitDistance;
|
|
if (c <= 0)
|
|
{
|
|
hitDistance = 0;
|
|
}
|
|
else
|
|
{
|
|
var b = Dot(toOrigin, rayDirection);
|
|
if (b > 0)
|
|
continue;
|
|
|
|
var discriminant = b * b - c;
|
|
if (discriminant < 0)
|
|
continue;
|
|
|
|
hitDistance = -b - (float)Math.Sqrt(discriminant);
|
|
if (hitDistance < 0 || hitDistance > distance)
|
|
continue;
|
|
}
|
|
|
|
var castCenter = Add(origin, Scale(rayDirection, hitDistance));
|
|
var delta = Subtract(body.position, castCenter);
|
|
var centerDistanceSquared = LengthSquared(delta);
|
|
var normal = centerDistanceSquared > Epsilon
|
|
? Scale(delta, 1.0f / (float)Math.Sqrt(centerDistanceSquared))
|
|
: Scale(rayDirection, -1);
|
|
|
|
hits.Add(new YmirSphereCastHit
|
|
{
|
|
bodyId = body.id,
|
|
point = Subtract(body.position, Scale(normal, body.radius)),
|
|
normal = normal,
|
|
distance = hitDistance
|
|
});
|
|
}
|
|
|
|
hits.Sort((left, right) => CompareHitOrder(left.distance, left.bodyId, right.distance, right.bodyId));
|
|
return hits.Count;
|
|
}
|
|
|
|
private static int CompareHitOrder(float leftDistance, string leftBodyId, float rightDistance, string rightBodyId)
|
|
{
|
|
var distanceOrder = leftDistance.CompareTo(rightDistance);
|
|
return distanceOrder != 0
|
|
? distanceOrder
|
|
: string.CompareOrdinal(leftBodyId, rightBodyId);
|
|
}
|
|
|
|
private static void ApplyFields(IReadOnlyList<YmirPhysicsBody> bodies, IReadOnlyList<YmirRadialField> fields, float deltaTime)
|
|
{
|
|
if (fields == null || fields.Count == 0)
|
|
return;
|
|
|
|
for (var bodyIndex = 0; bodyIndex < bodies.Count; bodyIndex++)
|
|
{
|
|
var body = bodies[bodyIndex];
|
|
if (body == null || body.isStatic)
|
|
continue;
|
|
|
|
for (var fieldIndex = 0; fieldIndex < fields.Count; fieldIndex++)
|
|
{
|
|
var field = fields[fieldIndex];
|
|
if (field == null || field.radius <= 0 || Math.Abs(field.strength) <= Epsilon)
|
|
continue;
|
|
|
|
var offset = Subtract(body.position, field.position);
|
|
var distanceSquared = LengthSquared(offset);
|
|
var radiusSquared = field.radius * field.radius;
|
|
if (distanceSquared <= Epsilon || distanceSquared > radiusSquared)
|
|
continue;
|
|
|
|
var distance = (float)Math.Sqrt(distanceSquared);
|
|
var falloff = 1.0f - Math.Min(1.0f, distance / field.radius);
|
|
var acceleration = Scale(offset, field.strength * falloff / distance);
|
|
body.velocity = Add(body.velocity, Scale(acceleration, deltaTime));
|
|
}
|
|
}
|
|
}
|
|
|
|
private static void IntegrateBodies(IReadOnlyList<YmirPhysicsBody> bodies, float deltaTime)
|
|
{
|
|
for (var i = 0; i < bodies.Count; i++)
|
|
{
|
|
var body = bodies[i];
|
|
if (body == null || body.isStatic)
|
|
continue;
|
|
|
|
if (body.momentOfInertia > Epsilon)
|
|
body.angularVelocity += body.torque / body.momentOfInertia * deltaTime;
|
|
|
|
body.position = Add(body.position, Scale(body.velocity, deltaTime));
|
|
if (Math.Abs(body.angularVelocity) > Epsilon)
|
|
body.direction = Rotate(body.direction, body.angularVelocity * deltaTime);
|
|
}
|
|
}
|
|
|
|
private static YmirContactEvent[] ResolveContacts(IReadOnlyList<YmirPhysicsBody> bodies)
|
|
{
|
|
var contacts = new List<YmirContactEvent>();
|
|
for (var leftIndex = 0; leftIndex < bodies.Count; leftIndex++)
|
|
{
|
|
var left = bodies[leftIndex];
|
|
if (left == null || left.radius < 0)
|
|
continue;
|
|
|
|
for (var rightIndex = leftIndex + 1; rightIndex < bodies.Count; rightIndex++)
|
|
{
|
|
var right = bodies[rightIndex];
|
|
if (right == null || right.radius < 0 || left.isStatic && right.isStatic)
|
|
continue;
|
|
|
|
var delta = Subtract(right.position, left.position);
|
|
var distanceSquared = LengthSquared(delta);
|
|
var combinedRadius = left.radius + right.radius;
|
|
if (distanceSquared > combinedRadius * combinedRadius)
|
|
continue;
|
|
|
|
var distance = (float)Math.Sqrt(distanceSquared);
|
|
var normal = distance > Epsilon
|
|
? Scale(delta, 1.0f / distance)
|
|
: new YmirVec2 { x = 1, y = 0 };
|
|
var penetration = combinedRadius - distance;
|
|
var relativeVelocity = Subtract(left.velocity, right.velocity);
|
|
|
|
contacts.Add(new YmirContactEvent
|
|
{
|
|
bodyA = left.id,
|
|
bodyB = right.id,
|
|
point = Add(left.position, Scale(normal, Math.Max(0, left.radius - penetration * 0.5f))),
|
|
normal = normal,
|
|
penetration = penetration,
|
|
relativeSpeed = Math.Max(0, Dot(relativeVelocity, normal))
|
|
});
|
|
|
|
SeparateBodies(left, right, normal, penetration);
|
|
ResolveVelocity(left, right, normal);
|
|
}
|
|
}
|
|
|
|
return contacts.ToArray();
|
|
}
|
|
|
|
private static void SeparateBodies(YmirPhysicsBody left, YmirPhysicsBody right, YmirVec2 normal, float penetration)
|
|
{
|
|
if (penetration <= 0)
|
|
return;
|
|
|
|
if (left.isStatic)
|
|
{
|
|
right.position = Add(right.position, Scale(normal, penetration));
|
|
return;
|
|
}
|
|
|
|
if (right.isStatic)
|
|
{
|
|
left.position = Subtract(left.position, Scale(normal, penetration));
|
|
return;
|
|
}
|
|
|
|
var leftInverseMass = InverseMass(left);
|
|
var rightInverseMass = InverseMass(right);
|
|
var inverseMassTotal = leftInverseMass + rightInverseMass;
|
|
if (inverseMassTotal <= Epsilon)
|
|
return;
|
|
|
|
var correction = Scale(normal, penetration / inverseMassTotal);
|
|
left.position = Subtract(left.position, Scale(correction, leftInverseMass));
|
|
right.position = Add(right.position, Scale(correction, rightInverseMass));
|
|
}
|
|
|
|
private static void ResolveVelocity(YmirPhysicsBody left, YmirPhysicsBody right, YmirVec2 normal)
|
|
{
|
|
var leftInverseMass = InverseMass(left);
|
|
var rightInverseMass = InverseMass(right);
|
|
var inverseMassTotal = leftInverseMass + rightInverseMass;
|
|
if (inverseMassTotal <= Epsilon)
|
|
return;
|
|
|
|
var relativeVelocity = Subtract(right.velocity, left.velocity);
|
|
var velocityAlongNormal = Dot(relativeVelocity, normal);
|
|
if (velocityAlongNormal > 0)
|
|
return;
|
|
|
|
var restitution = Math.Max(0, Math.Min(1, Math.Max(left.restitution, right.restitution)));
|
|
var impulseMagnitude = -(1 + restitution) * velocityAlongNormal / inverseMassTotal;
|
|
var impulse = Scale(normal, impulseMagnitude);
|
|
if (!left.isStatic)
|
|
left.velocity = Subtract(left.velocity, Scale(impulse, leftInverseMass));
|
|
if (!right.isStatic)
|
|
right.velocity = Add(right.velocity, Scale(impulse, rightInverseMass));
|
|
}
|
|
|
|
private static float InverseMass(YmirPhysicsBody body)
|
|
{
|
|
return body == null || body.isStatic || body.mass <= Epsilon ? 0 : 1.0f / body.mass;
|
|
}
|
|
|
|
private static YmirPhysicsBody[] CopyBodies(IReadOnlyList<YmirPhysicsBody> bodies)
|
|
{
|
|
if (bodies == null || bodies.Count == 0)
|
|
return Array.Empty<YmirPhysicsBody>();
|
|
|
|
var copy = new YmirPhysicsBody[bodies.Count];
|
|
for (var i = 0; i < copy.Length; i++)
|
|
{
|
|
var body = bodies[i];
|
|
copy[i] = body == null
|
|
? null
|
|
: new YmirPhysicsBody
|
|
{
|
|
id = body.id,
|
|
position = body.position,
|
|
velocity = body.velocity,
|
|
direction = body.direction,
|
|
angularVelocity = body.angularVelocity,
|
|
torque = body.torque,
|
|
momentOfInertia = body.momentOfInertia,
|
|
radius = body.radius,
|
|
mass = body.mass,
|
|
isStatic = body.isStatic,
|
|
restitution = body.restitution
|
|
};
|
|
}
|
|
|
|
return copy;
|
|
}
|
|
|
|
private static YmirRadialField[] CopyFields(IReadOnlyList<YmirRadialField> fields)
|
|
{
|
|
if (fields == null || fields.Count == 0)
|
|
return Array.Empty<YmirRadialField>();
|
|
|
|
var copy = new YmirRadialField[fields.Count];
|
|
for (var i = 0; i < copy.Length; i++)
|
|
{
|
|
var field = fields[i];
|
|
copy[i] = field == null
|
|
? null
|
|
: new YmirRadialField
|
|
{
|
|
id = field.id,
|
|
position = field.position,
|
|
strength = field.strength,
|
|
radius = field.radius
|
|
};
|
|
}
|
|
|
|
return copy;
|
|
}
|
|
|
|
private static YmirVec3 Subtract(YmirVec3 left, YmirVec3 right)
|
|
{
|
|
return new YmirVec3
|
|
{
|
|
x = left.x - right.x,
|
|
y = left.y - right.y,
|
|
z = left.z - right.z
|
|
};
|
|
}
|
|
|
|
private static YmirVec2 Subtract(YmirVec2 left, YmirVec2 right)
|
|
{
|
|
return new YmirVec2
|
|
{
|
|
x = left.x - right.x,
|
|
y = left.y - right.y
|
|
};
|
|
}
|
|
|
|
private static YmirVec3 Add(YmirVec3 left, YmirVec3 right)
|
|
{
|
|
return new YmirVec3
|
|
{
|
|
x = left.x + right.x,
|
|
y = left.y + right.y,
|
|
z = left.z + right.z
|
|
};
|
|
}
|
|
|
|
private static YmirVec2 Add(YmirVec2 left, YmirVec2 right)
|
|
{
|
|
return new YmirVec2
|
|
{
|
|
x = left.x + right.x,
|
|
y = left.y + right.y
|
|
};
|
|
}
|
|
|
|
private static YmirVec3 Scale(YmirVec3 value, float scale)
|
|
{
|
|
return new YmirVec3
|
|
{
|
|
x = value.x * scale,
|
|
y = value.y * scale,
|
|
z = value.z * scale
|
|
};
|
|
}
|
|
|
|
private static YmirVec2 Scale(YmirVec2 value, float scale)
|
|
{
|
|
return new YmirVec2
|
|
{
|
|
x = value.x * scale,
|
|
y = value.y * scale
|
|
};
|
|
}
|
|
|
|
private static YmirVec2 Rotate(YmirVec2 value, float radians)
|
|
{
|
|
var sine = (float)Math.Sin(radians);
|
|
var cosine = (float)Math.Cos(radians);
|
|
return new YmirVec2
|
|
{
|
|
x = value.x * cosine - value.y * sine,
|
|
y = value.x * sine + value.y * cosine
|
|
};
|
|
}
|
|
|
|
private static float LengthSquared(YmirVec3 value)
|
|
{
|
|
return value.x * value.x + value.y * value.y + value.z * value.z;
|
|
}
|
|
|
|
private static float LengthSquared(YmirVec2 value)
|
|
{
|
|
return value.x * value.x + value.y * value.y;
|
|
}
|
|
|
|
private static float Dot(YmirVec3 left, YmirVec3 right)
|
|
{
|
|
return left.x * right.x + left.y * right.y + left.z * right.z;
|
|
}
|
|
|
|
private static float Dot(YmirVec2 left, YmirVec2 right)
|
|
{
|
|
return left.x * right.x + left.y * right.y;
|
|
}
|
|
}
|