/// <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; } }