/// <summary>
/// Computes contact data for two spheres.
/// </summary>
/// <param name="a">First sphere.</param>
/// <param name="b">Second sphere.</param>
/// <param name="positionA">Position of the first sphere.</param>
/// <param name="positionB">Position of the second sphere.</param>
/// <param name="contact">Contact data between the spheres, if any.</param>
/// <returns>Whether or not the spheres are touching.</returns>
public static bool AreSpheresColliding(SphereShape a, SphereShape b, ref Vector3 positionA, ref Vector3 positionB, out ContactData contact)
{
contact = new ContactData();
float radiusSum = a.collisionMargin + b.collisionMargin;
Vector3 centerDifference;
Vector3.Subtract(ref positionB, ref positionA, out centerDifference);
float centerDistance = centerDifference.LengthSquared();
if (centerDistance < (radiusSum + CollisionDetectionSettings.maximumContactDistance) * (radiusSum + CollisionDetectionSettings.maximumContactDistance))
{
//In collision!
if (radiusSum > Toolbox.Epsilon) //This would be weird, but it is still possible to cause a NaN.
Vector3.Multiply(ref centerDifference, a.collisionMargin / (radiusSum), out contact.Position);
else contact.Position = new Vector3();
Vector3.Add(ref contact.Position, ref positionA, out contact.Position);
centerDistance = (float)Math.Sqrt(centerDistance);
if (centerDistance > Toolbox.BigEpsilon)
{
Vector3.Divide(ref centerDifference, centerDistance, out contact.Normal);
}
else
{
contact.Normal = Toolbox.UpVector;
}
contact.PenetrationDepth = radiusSum - centerDistance;
return true;
}
return false;
}
///<summary>
/// Initializes the pair tester.
///</summary>
///<param name="convex">Convex shape to use.</param>
///<param name="triangle">Triangle shape to use.</param>
public override void Initialize(ConvexShape convex, TriangleShape triangle)
{
this.sphere = (SphereShape)convex;
this.triangle = triangle;
}
/// <summary>
/// Cleans up the pair tester.
/// </summary>
public override void CleanUp()
{
triangle = null;
sphere = null;
Updated = false;
}
/// <summary>
/// Cleans up the pair tester.
/// </summary>
public override void CleanUp()
{
sphere = null;
Updated = false;
}
///<summary>
/// Tests if a box and sphere are colliding.
///</summary>
///<param name="box">Box to test.</param>
///<param name="sphere">Sphere to test.</param>
///<param name="boxTransform">Transform to apply to the box.</param>
///<param name="spherePosition">Transform to apply to the sphere.</param>
///<param name="contact">Contact point between the shapes, if any.</param>
///<returns>Whether or not the shapes were colliding.</returns>
public static bool AreShapesColliding(BoxShape box, SphereShape sphere, ref RigidTransform boxTransform, ref Vector3 spherePosition, out ContactData contact)
{
contact = new ContactData();
Vector3 localPosition;
RigidTransform.TransformByInverse(ref spherePosition, ref boxTransform, out localPosition);
#if !WINDOWS
Vector3 localClosestPoint = new Vector3();
#else
Vector3 localClosestPoint;
#endif
localClosestPoint.X = MathHelper.Clamp(localPosition.X, -box.halfWidth, box.halfWidth);
localClosestPoint.Y = MathHelper.Clamp(localPosition.Y, -box.halfHeight, box.halfHeight);
localClosestPoint.Z = MathHelper.Clamp(localPosition.Z, -box.halfLength, box.halfLength);
RigidTransform.Transform(ref localClosestPoint, ref boxTransform, out contact.Position);
Vector3 offset;
Vector3.Subtract(ref spherePosition, ref contact.Position, out offset);
float offsetLength = offset.LengthSquared();
if (offsetLength > (sphere.collisionMargin + CollisionDetectionSettings.maximumContactDistance) * (sphere.collisionMargin + CollisionDetectionSettings.maximumContactDistance))
{
return false;
}
//Colliding.
if (offsetLength > Toolbox.Epsilon)
{
offsetLength = (float)Math.Sqrt(offsetLength);
//Outside of the box.
Vector3.Divide(ref offset, offsetLength, out contact.Normal);
contact.PenetrationDepth = sphere.collisionMargin - offsetLength;
}
else
{
//Inside of the box.
Vector3 penetrationDepths;
penetrationDepths.X = localClosestPoint.X < 0 ? localClosestPoint.X + box.halfWidth : box.halfWidth - localClosestPoint.X;
penetrationDepths.Y = localClosestPoint.Y < 0 ? localClosestPoint.Y + box.halfHeight : box.halfHeight - localClosestPoint.Y;
penetrationDepths.Z = localClosestPoint.Z < 0 ? localClosestPoint.Z + box.halfLength : box.halfLength - localClosestPoint.Z;
if (penetrationDepths.X < penetrationDepths.Y && penetrationDepths.X < penetrationDepths.Z)
{
contact.Normal = localClosestPoint.X > 0 ? Toolbox.RightVector : Toolbox.LeftVector;
contact.PenetrationDepth = penetrationDepths.X;
}
else if (penetrationDepths.Y < penetrationDepths.Z)
{
contact.Normal = localClosestPoint.Y > 0 ? Toolbox.UpVector : Toolbox.DownVector;
contact.PenetrationDepth = penetrationDepths.Y;
}
else
{
contact.Normal = localClosestPoint.Z > 0 ? Toolbox.BackVector : Toolbox.ForwardVector;
contact.PenetrationDepth = penetrationDepths.X;
}
contact.PenetrationDepth += sphere.collisionMargin;
Vector3.Transform(ref contact.Normal, ref boxTransform.Orientation, out contact.Normal);
}
return true;
}
/// <summary>
/// Create new StaticCollider from existing collision data<para/>
/// Создание нового коллайдера из существующих данных
/// </summary>
/// <param name="colmesh">Collision data<para/>Данные о коллизиях</param>
public StaticCollider(CollisionFile.Group colmesh, Vector3 position, Quaternion angles, Vector3 scale) {
// Create base transformation matrix
// Создание базовой матрицы трансформации
Matrix4 mat =
Matrix4.CreateScale(scale) *
Matrix4.CreateFromQuaternion(angles) *
Matrix4.CreateTranslation(position);
// Create bodies
// Создание тел
List<CollisionEntry> col = new List<CollisionEntry>();
// Spheres
// Сферы
if (colmesh.Spheres!=null) {
foreach (CollisionFile.Sphere s in colmesh.Spheres) {
// Transforming positions to world coordinates
// Трансформация расположения в мировые координаты
Vector3 pos = Vector3.TransformPosition(s.Center, mat);
float radius = Vector3.TransformVector(Vector3.UnitX * s.Radius, mat).Length;
// Create primitive
// Создание примитива
EntityShape shape = new SphereShape(radius);
col.Add(new CollisionEntry() {
Type = PrimitiveType.Sphere,
Position = pos,
Rotation = Quaternion.Identity,
Shape = shape,
Body = new Entity(shape)
});
}
}
// Cubes
// Кубы
if (colmesh.Boxes!=null) {
foreach (CollisionFile.Box b in colmesh.Boxes) {
// Transforming positions to world coordinates
// Трансформация расположения в мировые координаты
Vector3 pos = Vector3.TransformPosition(
new Vector3(
(b.Min.X+b.Max.X)/2f,
(b.Min.Y+b.Max.Y)/2f,
(b.Min.Z+b.Max.Z)/2f
)
, mat);
float factor = Vector3.TransformVector(Vector3.UnitX, mat).Length;
// Create primitive
// Создание примитива
EntityShape shape = new BoxShape(
(float)Math.Abs(b.Max.X-b.Min.X) * factor,
(float)Math.Abs(b.Max.Y-b.Min.Y) * factor,
(float)Math.Abs(b.Max.Z-b.Min.Z) * factor
);
col.Add(new CollisionEntry() {
Type = PrimitiveType.Box,
Position = pos,
Rotation = angles,
Shape = shape,
Body = new Entity(shape)
});
}
}
// Trimeshes
// Тримеши
if (colmesh.Meshes!=null) {
// Creating vertices array
// Создание массива вершин
BEPUutilities.Vector3[] verts = new BEPUutilities.Vector3[colmesh.Vertices.Length];
for (int i = 0; i < colmesh.Vertices.Length; i++)
{
verts[i] = new BEPUutilities.Vector3(
colmesh.Vertices[i].X,
colmesh.Vertices[i].Y,
colmesh.Vertices[i].Z
);
}
foreach (CollisionFile.Trimesh m in colmesh.Meshes) {
// Creating affine transformation
// Создание трансформации
BEPUutilities.AffineTransform transform = new BEPUutilities.AffineTransform(
new BEPUutilities.Vector3(scale.X, scale.Y, scale.Z),
new BEPUutilities.Quaternion(angles.X, angles.Y, angles.Z, angles.W),
new BEPUutilities.Vector3(position.X, position.Y, position.Z)
);
// Create primitive
// Создание примитива
col.Add(new CollisionEntry() {
Type = PrimitiveType.Mesh,
Mesh = new StaticMesh(verts, m.Indices, transform)
});
}
}
subColliders = col.ToArray();
}
///<summary>
/// Initializes the pair tester.
///</summary>
///<param name="convex">Convex shape to use.</param>
public override void Initialize(ConvexShape convex)
{
this.sphere = (SphereShape)convex;
}
public static void Test()
{
var f0 = BuildHull();
f0.CollisionMargin = 0;
//Generate spheres all around the central froxel in such a way that we know that they're not colliding.
var froxelSphereSurface = new BoundingBox(new Vector3(-1.51f, -1.51f, -1.51f), new Vector3(1.51f, 1.51f, 1.51f));
int testIterations = 1000;
int innerIterations = 1000;
Random random = new Random(5);
long sphereFroxelSeparatedTicks = 0;
SphereShape sphere = new SphereShape(1);
for (int i = 0; i < testIterations; ++i)
{
var ray = GetRandomRay(ref froxelSphereSurface, random);
float t;
ray.Intersects(ref froxelSphereSurface, out t);
var sphereTransform = new RigidTransform { Position = ray.Position + ray.Direction * t, Orientation = Quaternion.Identity };
var start = Stopwatch.GetTimestamp();
for (int j = 0; j < innerIterations; ++j)
{
if (MPRToolbox.AreLocalShapesOverlapping(f0, sphere, ref sphereTransform))
{
Trace.Fail("By construction there can be no intersection!");
}
}
var end = Stopwatch.GetTimestamp();
sphereFroxelSeparatedTicks += (end - start);
}
Console.WriteLine($"Sphere-froxel separated: {(1e6 * sphereFroxelSeparatedTicks) / (testIterations * innerIterations * Stopwatch.Frequency)}");
//Do the same kind of test, but now with intersection.
froxelSphereSurface = new BoundingBox(new Vector3(-0.5f, -0.5f, -0.5f), new Vector3(0.5f, 0.5f, 0.5f));
long sphereFroxelIntersectingTicks = 0;
for (int i = 0; i < testIterations; ++i)
{
var ray = GetRandomRay(ref froxelSphereSurface, random);
float t;
ray.Intersects(ref froxelSphereSurface, out t);
var sphereTransform = new RigidTransform { Position = ray.Position + ray.Direction * (t - 0.99f), Orientation = Quaternion.Identity };
var start = Stopwatch.GetTimestamp();
for (int j = 0; j < innerIterations; ++j)
{
if (!MPRToolbox.AreLocalShapesOverlapping(f0, sphere, ref sphereTransform))
{
Trace.Fail("By construction there can be no separation!");
}
}
var end = Stopwatch.GetTimestamp();
sphereFroxelIntersectingTicks += (end - start);
}
Console.WriteLine($"Sphere-froxel intersecting: {(1e6 * sphereFroxelIntersectingTicks) / (testIterations * innerIterations * Stopwatch.Frequency)}");
//Create a surface for the rays to hit such that every query froxel will be just outside of the central froxel.
var froxelFroxelSurface = new BoundingBox(new Vector3(-1.01f, -1.01f, -1.01f), new Vector3(1.01f, 1.01f, 1.01f));
var queryHull = BuildHull();
queryHull.CollisionMargin = 0;
long froxelFroxelSeparatedTicks = 0;
for (int i = 0; i < testIterations; ++i)
{
var ray = GetRandomRay(ref froxelFroxelSurface, random);
float t;
ray.Intersects(ref froxelFroxelSurface, out t);
var queryTransform = new RigidTransform(ray.Position + ray.Direction * t);
var start = Stopwatch.GetTimestamp();
for (int j = 0; j < innerIterations; ++j)
{
if (MPRToolbox.AreLocalShapesOverlapping(f0, queryHull, ref queryTransform))
{
Trace.Fail("By construction there can be no intersection!");
}
}
var end = Stopwatch.GetTimestamp();
froxelFroxelSeparatedTicks += (end - start);
}
Console.WriteLine($"Froxel-froxel separated: {(1e6 * froxelFroxelSeparatedTicks) / (testIterations * innerIterations * Stopwatch.Frequency)}");
//Same thing as above, but now with slight intersection.
froxelFroxelSurface = new BoundingBox(new Vector3(-.99f, -.99f, -.99f), new Vector3(0.99f, 0.99f, 0.99f));
long froxelFroxelIntersectingTicks = 0;
for (int i = 0; i < testIterations; ++i)
{
var ray = GetRandomRay(ref froxelFroxelSurface, random);
float t;
ray.Intersects(ref froxelFroxelSurface, out t);
var queryTransform = new RigidTransform(ray.Position + ray.Direction * t);
var start = Stopwatch.GetTimestamp();
for (int j = 0; j < innerIterations; ++j)
{
if (!MPRToolbox.AreLocalShapesOverlapping(f0, queryHull, ref queryTransform))
{
Trace.Fail("By construction there can be no separation!");
}
}
var end = Stopwatch.GetTimestamp();
froxelFroxelIntersectingTicks += (end - start);
}
Console.WriteLine($"Froxel-froxel intersecting: {(1e6 * froxelFroxelIntersectingTicks) / (testIterations * innerIterations * Stopwatch.Frequency)}");
}
// Steer away from obstacles in the way. This method returns a zero vector if no correction is required.
// It should be high priority and the steering from other behaviors should blend into the remaining space.
// So if this returns a length 1.0f vector, avoiding the obstacle is most urgent and there is no room for other
// steering.
private Vector2 AvoidObstacles(Actor owner)
{
BipedControllerComponent bcc = owner.GetComponent<BipedControllerComponent>(ActorComponent.ComponentType.Control);
// Conditions where we do not want to use this steering force.
if (GetAngleFromVertical(bcc.Controller.Body.LinearVelocity) < MathHelper.PiOver4 || // We're probably falling...
!bcc.Controller.SupportFinder.HasSupport ||
!bcc.Controller.SupportFinder.HasTraction)
return Vector2.Zero;
// Sphere cast ahead along facing.
List<RayCastResult> obstacles = new List<RayCastResult>();
SphereShape probe = new SphereShape(bcc.Controller.BodyRadius * 1.1f);
RigidTransform probeStartPosition = new RigidTransform(bcc.Controller.Body.Position);
// Add a small constant to the probe length because we want a minimum amount of forward probing, even if we are not moving.
float probeLength = Math.Max(BepuVec3.Dot(bcc.Controller.Body.LinearVelocity, bcc.Controller.ViewDirection), 0.0f) + 1.0f;
BepuVec3 probeSweep = bcc.Controller.ViewDirection * probeLength;
ObstacleFilter filter = new ObstacleFilter(bcc.Controller.Body.CollisionInformation);
GameResources.ActorManager.SimSpace.ConvexCast(probe, ref probeStartPosition, ref probeSweep, filter.Test, obstacles);
RayCastDistanceComparer rcdc = new RayCastDistanceComparer();
obstacles.Sort(rcdc);
BEPUutilities.Vector3 cross = BEPUutilities.Vector3.Zero;
int obstacleIndex = 0;
do
{
if (obstacles.Count == obstacleIndex)
return Vector2.Zero;
cross = BEPUutilities.Vector3.Cross(bcc.Controller.ViewDirection, -obstacles[obstacleIndex++].HitData.Normal);
}
while (cross.X > 0.7f); // if cross.X > 0.7f, the obstacle is some kind of gentle ramp; ignore it.
// dot will typically be negative and magnitude indicates how directly ahead the obstacle is.
float dot = BEPUutilities.Vector3.Dot(bcc.Controller.ViewDirection, -obstacles[0].HitData.Normal);
if (dot >= 0.0f) // The obstacle won't hinder us if we touch it.
return Vector2.Zero;
// When cross.Y is positive, the object is generally to the right, so veer left (and vice versa).
float directionSign = cross.Y >= 0.0f ? -1.0f : 1.0f;
BEPUutilities.Vector2 result = BEPUutilities.Vector2.UnitX * directionSign * -dot;
// Also scale response by how close the obstacle is.
float distance = (obstacles[0].HitData.Location - bcc.Controller.Body.Position).Length();
result *= MathHelper.Clamp((1.0f - distance / probeLength), 0.0f, 1.0f); // / Math.Abs(dot);
// So far the result is in terms of 'velocity space'. Rotate it to align with the controller facing.
float velocityTheta = (float)(Math.Atan2(-probeSweep.X, -probeSweep.Z));
BEPUutilities.Matrix2x2 velocityWorld = SpaceUtils.Create2x2RotationMatrix(velocityTheta);
float facingTheta = (float)(Math.Atan2(-bcc.Controller.HorizontalViewDirection.X, -bcc.Controller.HorizontalViewDirection.Z));
BEPUutilities.Matrix2x2 facingWorldInv = SpaceUtils.Create2x2RotationMatrix(facingTheta);
facingWorldInv.Transpose(); // We want the transpose/inverse of the facing transform because we want to transform the movement into 'facing space'.
return BepuConverter.Convert(SpaceUtils.TransformVec2(SpaceUtils.TransformVec2(result, velocityWorld), facingWorldInv));
}
/// <summary>
/// Returns the list of ConvexCollidable's and Entities inside or touching the specified sphere.
/// Result does not include static geometry and non-entity physical objects.
/// </summary>
/// <param name="world"></param>
/// <param name="origin"></param>
/// <param name="radius"></param>
/// <returns></returns>
public List<Entity> WeaponOverlap( Vector3 origin, float radius, Entity entToSkip )
{
BU.BoundingSphere sphere = new BU.BoundingSphere(MathConverter.Convert(origin), radius);
SphereShape sphereShape = new SphereShape(radius);
BU.Vector3 zeroSweep = BU.Vector3.Zero;
BU.RigidTransform rigidXForm = new BU.RigidTransform( MathConverter.Convert(origin) );
var candidates = PhysicsResources.GetBroadPhaseEntryList();
physSpace.BroadPhase.QueryAccelerator.BroadPhase.QueryAccelerator.GetEntries(sphere, candidates);
var result = new List<Entity>();
foreach ( var candidate in candidates ) {
BU.RayHit rayHit;
bool r = candidate.ConvexCast( sphereShape, ref rigidXForm, ref zeroSweep, out rayHit );
if (r) {
var collidable = candidate as ConvexCollidable;
var entity = collidable==null ? null : collidable.Entity.Tag as Entity;
if (collidable==null) continue;
if (entity==null) continue;
result.Add( entity );
}
}
result.RemoveAll( e => e == entToSkip );
return result;
}
public void PreHandleSpawn()
{
model = TheClient.Models.GetModel(mod);
model.LoadSkin(TheClient.Textures);
int ignoreme;
if (mode == ModelCollisionMode.PRECISE)
{
Shape = TheClient.Models.Handler.MeshToBepu(model.Original, out ignoreme);
}
else if (mode == ModelCollisionMode.CONVEXHULL)
{
Shape = TheClient.Models.Handler.MeshToBepuConvex(model.Original, out ignoreme);
}
else if (mode == ModelCollisionMode.AABB)
{
List<BEPUutilities.Vector3> vecs = TheClient.Models.Handler.GetCollisionVertices(model.Original);
Location zero = new Location(vecs[0]);
AABB abox = new AABB() { Min = zero, Max = zero };
for (int v = 1; v < vecs.Count; v++)
{
abox.Include(new Location(vecs[v]));
}
Location size = abox.Max - abox.Min;
Location center = abox.Max - size / 2;
Shape = new BoxShape((float)size.X * (float)scale.X, (float)size.Y * (float)scale.Y, (float)size.Z * (float)scale.Z);
Offset = -center;
}
else
{
List<BEPUutilities.Vector3> vecs = TheClient.Models.Handler.GetCollisionVertices(model.Original);
// Location zero = new Location(vecs[0].X, vecs[0].Y, vecs[0].Z);
double distSq = 0;
for (int v = 1; v < vecs.Count; v++)
{
if (vecs[v].LengthSquared() > distSq)
{
distSq = vecs[v].LengthSquared();
}
}
double size = Math.Sqrt(distSq);
Offset = Location.Zero;
Shape = new SphereShape((float)size * (float)scale.X);
}
}
public override void SpawnBody()
{
Model smod = TheServer.Models.GetModel(model);
if (smod == null) // TODO: smod should return a cube when all else fails?
{
// TODO: Make it safe to -> TheRegion.DespawnEntity(this);
return;
}
Model3D smodel = smod.Original;
if (smodel == null) // TODO: smodel should return a cube when all else fails?
{
// TODO: Make it safe to -> TheRegion.DespawnEntity(this);
return;
}
if (mode == ModelCollisionMode.PRECISE)
{
Shape = TheServer.Models.handler.MeshToBepu(smodel, out modelVerts); // TODO: Scale!
}
if (mode == ModelCollisionMode.CONVEXHULL)
{
Shape = TheServer.Models.handler.MeshToBepuConvex(smodel, out modelVerts); // TODO: Scale!
}
else if (mode == ModelCollisionMode.AABB)
{
List<BEPUutilities.Vector3> vecs = TheServer.Models.handler.GetCollisionVertices(smodel);
Location zero = new Location(vecs[0]);
AABB abox = new AABB() { Min = zero, Max = zero };
for (int v = 1; v < vecs.Count; v++)
{
abox.Include(new Location(vecs[v]));
}
Location size = abox.Max - abox.Min;
Location center = abox.Max - size / 2;
offset = -center;
Shape = new BoxShape((double)size.X * (double)scale.X, (double)size.Y * (double)scale.Y, (double)size.Z * (double)scale.Z);
}
else
{
List<BEPUutilities.Vector3> vecs = TheServer.Models.handler.GetCollisionVertices(smodel);
double distSq = 0;
for (int v = 1; v < vecs.Count; v++)
{
if (vecs[v].LengthSquared() > distSq)
{
distSq = vecs[v].LengthSquared();
}
}
double size = Math.Sqrt(distSq);
offset = Location.Zero;
Shape = new SphereShape((double)size * (double)scale.X);
}
base.SpawnBody();
if (mode == ModelCollisionMode.PRECISE)
{
offset = InternalOffset;
}
}
