/// <summary>
/// Adds the collider to the engine processing pipeline.
/// </summary>
/// <param name="collider">The collider.</param>
/// <param name="group">The group.</param>
/// <param name="mask">The mask.</param>
public void AddCollider(Collider collider, CollisionFilterGroupFlags group, CollisionFilterGroupFlags mask)
{
collisionWorld.AddCollisionObject(collider.InternalCollider, (BulletSharp.CollisionFilterGroups)group, (BulletSharp.CollisionFilterGroups)mask);
aliveColliders.Add(collider.InternalCollider, collider);
collider.Simulation = this;
}
/// <summary>
/// Counts how many objects are overlapping with a given ColliderShape. If trackObjects are true, you can
/// use OverlappedWith to check if another PhysicsComponent was part of the overlapping objects
/// </summary>
/// <param name="shape">ColliderShape to check for overlaps with</param>
/// <param name="position">Position to move the ColliderShape, in addition to its LocalOffset</param>
/// <param name="myGroup">What collision group is the ColliderShape in?</param>
/// <param name="overlapsWith">What collision groups does the ColliderShape overlap with?</param>
/// <param name="contactTest">If true, contact test overlapping objects. See ContactResults for output. Defaults to false</param>
/// <param name="stopAfterFirstContact">If contact testing, should we stop contact testing after our first contact was found?</param>
/// <returns>Number of overlapping objects</returns>
public static int PerformOverlapTest(ColliderShape shape, Xenko.Core.Mathematics.Vector3?position = null,
CollisionFilterGroups myGroup = CollisionFilterGroups.DefaultFilter,
CollisionFilterGroupFlags overlapsWith = CollisionFilterGroupFlags.AllFilter,
bool contactTest = false, bool stopAfterFirstContact = false)
{
// doesn't support multithreading
if (mySimulation.simulationLocker != null)
{
throw new InvalidOperationException("Overlap testing not supported with multithreaded physics");
}
if (ghostObject == null)
{
ghostObject = new BulletSharp.PairCachingGhostObject
{
ContactProcessingThreshold = 1e18f,
UserObject = shape
};
ghostObject.CollisionFlags |= BulletSharp.CollisionFlags.NoContactResponse;
internalResults = new OverlapCallback();
NativeOverlappingObjects = new HashSet <object>();
}
ghostObject.CollisionShape = shape.InternalShape;
ghostObject.WorldTransform = Matrix.Transformation(shape.Scaling, shape.LocalRotation, position.HasValue ? position.Value + shape.LocalOffset : shape.LocalOffset);
mySimulation.collisionWorld.AddCollisionObject(ghostObject, (BulletSharp.CollisionFilterGroups)myGroup, (BulletSharp.CollisionFilterGroups)overlapsWith);
int overlapCount = ghostObject.NumOverlappingObjects;
NativeOverlappingObjects.Clear();
for (int i = 0; i < overlapCount; i++)
{
NativeOverlappingObjects.Add(ghostObject.OverlappingPairs[i]);
}
if (contactTest)
{
internalResults.Clear();
internalResults.CollisionFilterGroup = (int)myGroup;
internalResults.CollisionFilterMask = (int)overlapsWith;
foreach (object nativeobj in NativeOverlappingObjects)
{
mySimulation.collisionWorld.ContactPairTest(ghostObject, (BulletSharp.CollisionObject)nativeobj, internalResults);
if (stopAfterFirstContact && internalResults.Contacts.Count > 0)
{
break;
}
}
}
mySimulation.collisionWorld.RemoveCollisionObject(ghostObject);
return(overlapCount);
}
internal void AddRigidBody(RigidbodyComponent rigidBody, CollisionFilterGroupFlags group, CollisionFilterGroupFlags mask)
{
if (discreteDynamicsWorld == null)
{
throw new Exception("Cannot perform this action when the physics engine is set to CollisionsOnly");
}
discreteDynamicsWorld.AddRigidBody(rigidBody.InternalRigidBody, (short)group, (short)mask);
}
/// <summary>
/// Raycast between the camera and its target. The script assumes the camera is a child entity of its target.
/// </summary>
private void UpdateCameraRaycast()
{
var maxLength = DefaultDistance;
var cameraVector = new Vector3(0, 0, DefaultDistance);
Entity.GetParent().Transform.Rotation.Rotate(ref cameraVector);
if (ConeRadius <= 0)
{
// If the cone radius
var raycastStart = Entity.GetParent().Transform.WorldMatrix.TranslationVector;
var hitResult = this.GetSimulation().Raycast(raycastStart, raycastStart + cameraVector);
if (hitResult.Succeeded)
{
maxLength = Math.Min(DefaultDistance, (raycastStart - hitResult.Point).Length());
}
}
else
{
// If the cone radius is > 0 we will sweep an actual cone and see where it collides
var fromMatrix = Matrix.Translation(0, 0, -DefaultDistance * 0.5f)
* Entity.GetParent().Transform.WorldMatrix;
var toMatrix = Matrix.Translation(0, 0, DefaultDistance * 0.5f)
* Entity.GetParent().Transform.WorldMatrix;
resultsOutput.Clear();
const CollisionFilterGroups cfg = CollisionFilterGroups.DefaultFilter;
const CollisionFilterGroupFlags cfgf = CollisionFilterGroupFlags.DefaultFilter; // Intentionally ignoring the CollisionFilterGroupFlags.StaticFilter; to avoid collision with poles
this.GetSimulation().ShapeSweepPenetrating(coneShape, fromMatrix, toMatrix, resultsOutput, cfg, cfgf);
foreach (var result in resultsOutput)
{
if (result.Succeeded)
{
var signedVector = result.Point - Entity.GetParent().Transform.WorldMatrix.TranslationVector;
var signedDistance = Vector3.Dot(cameraVector, signedVector);
var currentLength = DefaultDistance * result.HitFraction;
if (signedDistance > 0 && currentLength < maxLength)
{
maxLength = currentLength;
}
}
}
}
if (maxLength < MinimumDistance)
{
maxLength = MinimumDistance;
}
Entity.Transform.Position.Z = maxLength;
}
/// <summary>
/// Adds the rigid body to the engine processing pipeline.
/// </summary>
/// <param name="rigidBody">The rigid body.</param>
/// <param name="group">The group.</param>
/// <param name="mask">The mask.</param>
/// <exception cref="System.Exception">Cannot perform this action when the physics engine is set to CollisionsOnly</exception>
public void AddRigidBody(RigidBody rigidBody, CollisionFilterGroupFlags group, CollisionFilterGroupFlags mask)
{
if (discreteDynamicsWorld == null)
{
throw new Exception("Cannot perform this action when the physics engine is set to CollisionsOnly");
}
discreteDynamicsWorld.AddRigidBody(rigidBody.InternalRigidBody, (short)group, (short)mask);
aliveColliders.Add(rigidBody.InternalRigidBody, rigidBody);
rigidBody.Simulation = this;
}
/// <summary>
/// Hashes and entity's transform and it's collider shape settings
/// </summary>
/// <param name="collider">The collider to hash</param>
/// <param name="includedCollisionGroups">The filter group for active collides,
/// which is used to hash if this colliders participates in the navigation mesh build</param>
/// <returns></returns>
public static int HashEntityCollider(StaticColliderComponent collider, CollisionFilterGroupFlags includedCollisionGroups)
{
int hash = 0;
hash = (hash * 397) ^ collider.Entity.Transform.WorldMatrix.GetHashCode();
hash = (hash * 397) ^ collider.Enabled.GetHashCode();
hash = (hash * 397) ^ collider.IsTrigger.GetHashCode();
hash = (hash * 397) ^ CheckColliderFilter(collider, includedCollisionGroups).GetHashCode();
foreach (var shape in collider.ColliderShapes)
{
hash = (hash * 397) ^ shape.GetType().GetHashCode();
hash = (hash * 397) ^ shape.GetHashCode();
}
return(hash);
}
/// <summary>
/// Adds the character to the engine processing pipeline.
/// </summary>
/// <param name="character">The character.</param>
/// <param name="group">The group.</param>
/// <param name="mask">The mask.</param>
/// <exception cref="System.Exception">Cannot perform this action when the physics engine is set to CollisionsOnly</exception>
public void AddCharacter(Character character, CollisionFilterGroupFlags group, CollisionFilterGroupFlags mask)
{
if (discreteDynamicsWorld == null)
{
throw new Exception("Cannot perform this action when the physics engine is set to CollisionsOnly");
}
var collider = character.InternalCollider;
var action = character.KinematicCharacter;
discreteDynamicsWorld.AddCollisionObject(collider, (BulletSharp.CollisionFilterGroups)group, (BulletSharp.CollisionFilterGroups)mask);
discreteDynamicsWorld.AddCharacter(action);
character.Simulation = this;
}
/// <summary>
/// Since you can't have non-convex shapes (e.g. mesh's) in a compound object, this helper will generate a bunch of individual static components to attach to an entity, with each shape.
/// </summary>
/// <param name="e">Entity to add static components to</param>
/// <param name="shapes">shapes that will generate a static component for each</param>
/// <param name="offsets">optional offset for each</param>
/// <param name="rotations">optional rotation for each</param>
public static void GenerateStaticComponents(Entity e, List <IShape> shapes, List <Vector3> offsets = null, List <Quaternion> rotations = null,
CollisionFilterGroups group = CollisionFilterGroups.DefaultFilter, CollisionFilterGroupFlags collidesWith = CollisionFilterGroupFlags.AllFilter,
float FrictionCoefficient = 0.5f, float MaximumRecoverableVelocity = 2f, SpringSettings?springSettings = null)
{
for (int i = 0; i < shapes.Count; i++)
{
BepuStaticColliderComponent sc = new BepuStaticColliderComponent();
sc.ColliderShape = shapes[i];
if (offsets != null && offsets.Count > i)
{
sc.Position = offsets[i];
}
if (rotations != null && rotations.Count > i)
{
sc.Rotation = rotations[i];
}
sc.CanCollideWith = collidesWith;
sc.CollisionGroup = group;
sc.FrictionCoefficient = FrictionCoefficient;
sc.MaximumRecoveryVelocity = MaximumRecoverableVelocity;
if (springSettings.HasValue)
{
sc.SpringSettings = springSettings.Value;
}
e.Add(sc);
}
}
/// <summary>
/// Raycasts penetrating any shape the ray encounters.
/// Filtering by CollisionGroup
/// </summary>
/// <param name="simulation">Physics simulation.</param>
/// <param name="raySegment">Ray.</param>
/// <param name="resultsOutput">The list to fill with results.</param>
/// <param name="collisionFilterGroups">The collision group of this shape sweep</param>
/// <param name="collisionFilterGroupFlags">The collision group that this shape sweep can collide with</param>
/// <exception cref="ArgumentNullException">If the simulation argument is null.</exception>
public static void RaycastPenetrating(this Simulation simulation, RaySegment raySegment, IList <HitResult> resultsOutput, CollisionFilterGroups collisionFilterGroups, CollisionFilterGroupFlags collisionFilterGroupFlags)
{
if (simulation == null)
{
throw new ArgumentNullException(nameof(simulation));
}
simulation.RaycastPenetrating(raySegment.Start, raySegment.End, resultsOutput, collisionFilterGroups, collisionFilterGroupFlags);
}
/// <summary>
/// Raycasts and stops at the first hit.
/// </summary>
/// <param name="simulation">Physics simulation.</param>
/// <param name="raySegment">Ray.</param>
/// <param name="collisionFilterGroups">The collision group of this shape sweep</param>
/// <param name="collisionFilterGroupFlags">The collision group that this shape sweep can collide with</param>
/// <returns>The hit results.</returns>
/// <exception cref="ArgumentNullException">If the simulation argument is null.</exception>
public static HitResult Raycast(this Simulation simulation, RaySegment raySegment, CollisionFilterGroups collisionFilterGroups, CollisionFilterGroupFlags collisionFilterGroupFlags)
{
if (simulation == null)
{
throw new ArgumentNullException(nameof(simulation));
}
return(simulation.Raycast(raySegment.Start, raySegment.End, collisionFilterGroups, collisionFilterGroupFlags));
}
/// <summary>
/// Performs a sweep test using a collider shape and never stops until "to"
/// </summary>
/// <param name="shape">The shape.</param>
/// <param name="from">From.</param>
/// <param name="to">To.</param>
/// <param name="collisionFilterGroups">The collision group of this shape sweep</param>
/// <param name="collisionFilterGroupFlags">The collision group that this shape sweep can collide with</param>
/// <returns>The list with hit results.</returns>
/// <exception cref="System.Exception">This kind of shape cannot be used for a ShapeSweep.</exception>
public FastList<HitResult> ShapeSweepPenetrating(ColliderShape shape, Matrix from, Matrix to, CollisionFilterGroups collisionFilterGroups, CollisionFilterGroupFlags collisionFilterGroupFlags)
{
var results = new FastList<HitResult>();
ShapeSweepPenetrating(shape, from, to, results, collisionFilterGroups, collisionFilterGroupFlags);
return results;
}
/// <summary>
/// Adds the collider to the engine processing pipeline.
/// </summary>
/// <param name="collider">The collider.</param>
/// <param name="group">The group.</param>
/// <param name="mask">The mask.</param>
public void AddCollider(Collider collider, CollisionFilterGroupFlags group, CollisionFilterGroupFlags mask)
{
collisionWorld.AddCollisionObject(collider.InternalCollider, (BulletSharp.CollisionFilterGroups)group, (BulletSharp.CollisionFilterGroups)mask);
collider.Simulation = this;
}
/// <summary>
/// Raycasts penetrating any shape the ray encounters.
/// </summary>
/// <param name="from">From.</param>
/// <param name="to">To.</param>
/// <param name="collisionFilterGroups">The collision group of this shape sweep</param>
/// <param name="collisionFilterGroupFlags">The collision group that this shape sweep can collide with</param>
/// <returns>The list with hit results.</returns>
public FastList<HitResult> RaycastPenetrating(Vector3 from, Vector3 to, CollisionFilterGroups collisionFilterGroups, CollisionFilterGroupFlags collisionFilterGroupFlags)
{
var results = new FastList<HitResult>();
RaycastPenetrating(from, to, results, collisionFilterGroups, collisionFilterGroupFlags);
return results;
}
internal void AddRigidBody(RigidbodyComponent rigidBody, CollisionFilterGroupFlags group, CollisionFilterGroupFlags mask)
{
if (discreteDynamicsWorld == null) throw new Exception("Cannot perform this action when the physics engine is set to CollisionsOnly");
discreteDynamicsWorld.AddRigidBody(rigidBody.InternalRigidBody, (short)group, (short)mask);
}
/// <summary>
/// Performs a sweep test using a collider shape and stops at the first hit
/// </summary>
/// <param name="shape">The shape.</param>
/// <param name="from">From.</param>
/// <param name="to">To.</param>
/// <param name="collisionFilterGroups">The collision group of this shape sweep</param>
/// <param name="collisionFilterGroupFlags">The collision group that this shape sweep can collide with</param>
/// <returns></returns>
/// <exception cref="System.Exception">This kind of shape cannot be used for a ShapeSweep.</exception>
public HitResult ShapeSweep(ColliderShape shape, Matrix from, Matrix to, CollisionFilterGroups collisionFilterGroups, CollisionFilterGroupFlags collisionFilterGroupFlags)
{
var sh = shape.InternalShape as BulletSharp.ConvexShape;
if (sh == null) throw new Exception("This kind of shape cannot be used for a ShapeSweep.");
var result = new HitResult(); //result.Succeeded is false by default
using (var rcb = new BulletSharp.ClosestConvexResultCallback(from.TranslationVector, to.TranslationVector)
{
CollisionFilterGroup = (BulletSharp.CollisionFilterGroups)collisionFilterGroups,
CollisionFilterMask = (BulletSharp.CollisionFilterGroups)collisionFilterGroupFlags
})
{
collisionWorld.ConvexSweepTest(sh, from, to, rcb);
if (rcb.HitCollisionObject == null) return result;
result.Succeeded = true;
result.Collider = (PhysicsComponent)rcb.HitCollisionObject.UserObject;
result.Normal = rcb.HitNormalWorld;
result.Point = rcb.HitPointWorld;
result.HitFraction = rcb.ClosestHitFraction;
}
return result;
}
/// <summary>
/// Raycasts penetrating any shape the ray encounters.
/// </summary>
/// <param name="simulation">Physics simulation.</param>
/// <param name="raySegment">Ray.</param>
/// <param name="collisionFilterGroups">The collision group of this shape sweep</param>
/// <param name="collisionFilterGroupFlags">The collision group that this shape sweep can collide with</param>
/// <returns>The list with hit results.</returns>
/// <exception cref="ArgumentNullException">If the simulation argument is null.</exception>
public static FastList <HitResult> RaycastPenetrating(this Simulation simulation, RaySegment raySegment, CollisionFilterGroups collisionFilterGroups, CollisionFilterGroupFlags collisionFilterGroupFlags)
{
if (simulation == null)
{
throw new ArgumentNullException(nameof(simulation));
}
return(simulation.RaycastPenetrating(raySegment, collisionFilterGroups, collisionFilterGroupFlags));
}
/// <summary>
/// Adds the rigid body to the engine processing pipeline.
/// </summary>
/// <param name="rigidBody">The rigid body.</param>
/// <param name="group">The group.</param>
/// <param name="mask">The mask.</param>
/// <exception cref="System.Exception">Cannot perform this action when the physics engine is set to CollisionsOnly</exception>
public void AddRigidBody(RigidBody rigidBody, CollisionFilterGroupFlags group, CollisionFilterGroupFlags mask)
{
if (discreteDynamicsWorld == null) throw new Exception("Cannot perform this action when the physics engine is set to CollisionsOnly");
discreteDynamicsWorld.AddRigidBody(rigidBody.InternalRigidBody, (short)group, (short)mask);
aliveColliders.Add(rigidBody.InternalRigidBody, rigidBody);
rigidBody.Simulation = this;
}
/// <summary>
/// Rebuilds outdated triangle data for colliders and recalculates hashes storing everything in StaticColliderData
/// </summary>
private void BuildInput(StaticColliderData[] collidersLocal, CollisionFilterGroupFlags includedCollisionGroups)
{
NavigationMeshCache lastCache = oldNavigationMesh?.Cache;
bool clearCache = false;
Dispatcher.ForEach(collidersLocal, colliderData =>
{
var entity = colliderData.Component.Entity;
TransformComponent entityTransform = entity.Transform;
Matrix entityWorldMatrix = entityTransform.WorldMatrix;
NavigationMeshInputBuilder entityNavigationMeshInputBuilder = colliderData.InputBuilder = new NavigationMeshInputBuilder();
// Compute hash of collider and compare it with the previous build if there is one
colliderData.ParameterHash = NavigationMeshBuildUtils.HashEntityCollider(colliderData.Component, includedCollisionGroups);
colliderData.Previous = null;
if (lastCache?.Objects.TryGetValue(colliderData.Component.Id, out colliderData.Previous) ?? false)
{
if ((!colliderData.Component.AlwaysUpdateNaviMeshCache) &&
(colliderData.Previous.ParameterHash == colliderData.ParameterHash))
{
// In this case, we don't need to recalculate the geometry for this shape, since it wasn't changed
// here we take the triangle mesh from the previous build as the current
colliderData.InputBuilder = colliderData.Previous.InputBuilder;
colliderData.Planes.Clear();
colliderData.Planes.AddRange(colliderData.Previous.Planes);
colliderData.Processed = false;
return;
}
}
// Clear cache on removal of infinite planes
if (colliderData.Planes.Count > 0)
{
clearCache = true;
}
// Clear planes
colliderData.Planes.Clear();
// Return empty data for disabled colliders, filtered out colliders or trigger colliders
if (!colliderData.Component.Enabled || colliderData.Component.IsTrigger ||
!NavigationMeshBuildUtils.CheckColliderFilter(colliderData.Component, includedCollisionGroups))
{
colliderData.Processed = true;
return;
}
// Make sure shape is up to date
if (!NavigationMeshBuildUtils.HasLatestColliderShape(colliderData.Component))
{
colliderData.Component.ComposeShape();
}
// Interate through all the colliders shapes while queueing all shapes in compound shapes to process those as well
Queue <ColliderShape> shapesToProcess = new Queue <ColliderShape>();
if (colliderData.Component.ColliderShape != null)
{
shapesToProcess.Enqueue(colliderData.Component.ColliderShape);
while (shapesToProcess.Count > 0)
{
var shape = shapesToProcess.Dequeue();
var shapeType = shape.GetType();
if (shapeType == typeof(BoxColliderShape))
{
var box = (BoxColliderShape)shape;
var boxDesc = GetColliderShapeDesc <BoxColliderShapeDesc>(box.Description);
Matrix transform = box.PositiveCenterMatrix * entityWorldMatrix;
var meshData = GeometricPrimitive.Cube.New(boxDesc.Size, toLeftHanded: true);
entityNavigationMeshInputBuilder.AppendMeshData(meshData, transform);
}
else if (shapeType == typeof(SphereColliderShape))
{
var sphere = (SphereColliderShape)shape;
var sphereDesc = GetColliderShapeDesc <SphereColliderShapeDesc>(sphere.Description);
Matrix transform = sphere.PositiveCenterMatrix * entityWorldMatrix;
var meshData = GeometricPrimitive.Sphere.New(sphereDesc.Radius, toLeftHanded: true);
entityNavigationMeshInputBuilder.AppendMeshData(meshData, transform);
}
else if (shapeType == typeof(CylinderColliderShape))
{
var cylinder = (CylinderColliderShape)shape;
var cylinderDesc = GetColliderShapeDesc <CylinderColliderShapeDesc>(cylinder.Description);
Matrix transform = cylinder.PositiveCenterMatrix * entityWorldMatrix;
var meshData = GeometricPrimitive.Cylinder.New(cylinderDesc.Height, cylinderDesc.Radius, toLeftHanded: true);
entityNavigationMeshInputBuilder.AppendMeshData(meshData, transform);
}
else if (shapeType == typeof(CapsuleColliderShape))
{
var capsule = (CapsuleColliderShape)shape;
var capsuleDesc = GetColliderShapeDesc <CapsuleColliderShapeDesc>(capsule.Description);
Matrix transform = capsule.PositiveCenterMatrix * entityWorldMatrix;
var meshData = GeometricPrimitive.Capsule.New(capsuleDesc.Length, capsuleDesc.Radius, toLeftHanded: true);
entityNavigationMeshInputBuilder.AppendMeshData(meshData, transform);
}
else if (shapeType == typeof(ConeColliderShape))
{
var cone = (ConeColliderShape)shape;
var coneDesc = GetColliderShapeDesc <ConeColliderShapeDesc>(cone.Description);
Matrix transform = cone.PositiveCenterMatrix * entityWorldMatrix;
var meshData = GeometricPrimitive.Cone.New(coneDesc.Radius, coneDesc.Height, toLeftHanded: true);
entityNavigationMeshInputBuilder.AppendMeshData(meshData, transform);
}
else if (shapeType == typeof(StaticPlaneColliderShape))
{
var planeShape = (StaticPlaneColliderShape)shape;
var planeDesc = GetColliderShapeDesc <StaticPlaneColliderShapeDesc>(planeShape.Description);
Matrix transform = entityWorldMatrix;
Plane plane = new Plane(planeDesc.Normal, planeDesc.Offset);
// Pre-Transform plane parameters
plane.Normal = Vector3.TransformNormal(planeDesc.Normal, transform);
plane.Normal.Normalize();
plane.D += Vector3.Dot(transform.TranslationVector, plane.Normal);
colliderData.Planes.Add(plane);
}
else if (shapeType == typeof(ConvexHullColliderShape))
{
var hull = (ConvexHullColliderShape)shape;
Matrix transform = hull.PositiveCenterMatrix * entityWorldMatrix;
// Convert hull indices to int
int[] indices = new int[hull.Indices.Count];
if (hull.Indices.Count % 3 != 0)
{
throw new InvalidOperationException($"{shapeType} does not consist of triangles");
}
for (int i = 0; i < hull.Indices.Count; i += 3)
{
indices[i] = (int)hull.Indices[i];
indices[i + 2] = (int)hull.Indices[i + 1]; // NOTE: Reversed winding to create left handed input
indices[i + 1] = (int)hull.Indices[i + 2];
}
entityNavigationMeshInputBuilder.AppendArrays(hull.Points.ToArray(), indices, transform);
}
else if (shapeType == typeof(StaticMeshColliderShape))
{
var mesh = (StaticMeshColliderShape)shape;
Matrix transform = mesh.PositiveCenterMatrix * entityWorldMatrix;
mesh.GetMeshDataCopy(out var verts, out var indices);
// Convert hull indices to int
if (indices.Length % 3 != 0)
{
throw new InvalidOperationException($"{shapeType} does not consist of triangles");
}
for (int i = 0; i < indices.Length; i += 3)
{
// NOTE: Reversed winding to create left handed input
(indices[i + 1], indices[i + 2]) = (indices[i + 2], indices[i + 1]);
}
/// <summary>
/// Checks if a static collider passes the given filter group
/// </summary>
/// <param name="collider">The collider to check</param>
/// <param name="includedCollisionGroups">The collision filter</param>
/// <returns><c>true</c> if the collider passes the filter, <c>false</c> otherwise</returns>
public static bool CheckColliderFilter(StaticColliderComponent collider, CollisionFilterGroupFlags includedCollisionGroups)
{
return(true); // TODO: Update navmesh to use the new collision types
}
/// <summary>
/// Performs the build of a navigation mesh
/// </summary>
/// <param name="buildSettings">The build settings to pass to recast</param>
/// <param name="agentSettings">A collection of agent settings to use, this will generate a layer in the navigation mesh for every agent settings in this collection (in the same order)</param>
/// <param name="includedCollisionGroups">The collision groups that will affect which colliders are considered solid</param>
/// <param name="boundingBoxes">A collection of bounding boxes to use as the region for which to generate navigation mesh tiles</param>
/// <param name="cancellationToken">A cancellation token to interrupt the build process</param>
/// <returns>The build result</returns>
public NavigationMeshBuildResult Build(NavigationMeshBuildSettings buildSettings, ICollection <NavigationMeshGroup> groups, CollisionFilterGroupFlags includedCollisionGroups,
ICollection <BoundingBox> boundingBoxes, CancellationToken cancellationToken)
{
var lastCache = oldNavigationMesh?.Cache;
var result = new NavigationMeshBuildResult();
if (groups.Count == 0)
{
Logger?.Warning("No group settings found");
result.Success = true;
result.NavigationMesh = new NavigationMesh();
return(result);
}
if (boundingBoxes.Count == 0)
{
Logger?.Warning("No bounding boxes found");
}
var settingsHash = groups?.ComputeHash() ?? 0;
settingsHash = (settingsHash * 397) ^ buildSettings.GetHashCode();
if (lastCache != null && lastCache.SettingsHash != settingsHash)
{
// Start from scratch if settings changed
oldNavigationMesh = null;
Logger?.Info("Build settings changed, doing a full rebuild");
}
// Copy colliders so the collection doesn't get modified
StaticColliderData[] collidersLocal;
lock (colliders)
{
collidersLocal = colliders.ToArray();
}
BuildInput(collidersLocal, includedCollisionGroups);
// Check if cache was cleared while building the input
lastCache = oldNavigationMesh?.Cache;
// The new navigation mesh that will be created
result.NavigationMesh = new NavigationMesh();
result.NavigationMesh.CellSize = buildSettings.CellSize;
result.NavigationMesh.TileSize = buildSettings.TileSize;
// Tile cache for this new navigation mesh
NavigationMeshCache newCache = result.NavigationMesh.Cache = new NavigationMeshCache();
newCache.SettingsHash = settingsHash;
// Generate global bounding box for planes
BoundingBox globalBoundingBox = BoundingBox.Empty;
foreach (var boundingBox in boundingBoxes)
{
globalBoundingBox = BoundingBox.Merge(boundingBox, globalBoundingBox);
}
// Combine input and collect tiles to build
NavigationMeshInputBuilder sceneNavigationMeshInputBuilder = new NavigationMeshInputBuilder();
foreach (var colliderData in collidersLocal)
{
if (colliderData.InputBuilder == null)
{
continue;
}
// Otherwise, skip building these tiles
sceneNavigationMeshInputBuilder.AppendOther(colliderData.InputBuilder);
newCache.Add(colliderData.Component, colliderData.InputBuilder, colliderData.Planes, colliderData.ParameterHash);
// Generate geometry for planes
foreach (var plane in colliderData.Planes)
{
sceneNavigationMeshInputBuilder.AppendOther(BuildPlaneGeometry(plane, globalBoundingBox));
}
}
// TODO: Generate tile local mesh input data
var inputVertices = sceneNavigationMeshInputBuilder.Points.ToArray();
var inputIndices = sceneNavigationMeshInputBuilder.Indices.ToArray();
// Enumerate over every layer, and build tiles for each of those layers using the provided agent settings
using (var groupEnumerator = groups.NotNull().GetEnumerator())
{
for (int layerIndex = 0; layerIndex < groups.Count; layerIndex++)
{
groupEnumerator.MoveNext();
var currentGroup = groupEnumerator.Current;
var currentAgentSettings = currentGroup.AgentSettings;
if (result.NavigationMesh.LayersInternal.ContainsKey(currentGroup.Id))
{
Logger.Error($"The same group can't be selected twice: {currentGroup}");
return(result);
}
HashSet <Point> tilesToBuild = new HashSet <Point>();
foreach (var colliderData in collidersLocal)
{
if (colliderData.InputBuilder == null)
{
continue;
}
if (colliderData.Processed)
{
MarkTiles(colliderData.InputBuilder, ref buildSettings, ref currentAgentSettings, tilesToBuild);
if (colliderData.Previous != null)
{
MarkTiles(colliderData.Previous.InputBuilder, ref buildSettings, ref currentAgentSettings, tilesToBuild);
}
}
}
// Check for removed colliders
if (lastCache != null)
{
foreach (var obj in lastCache.Objects)
{
if (!newCache.Objects.ContainsKey(obj.Key))
{
MarkTiles(obj.Value.InputBuilder, ref buildSettings, ref currentAgentSettings, tilesToBuild);
}
}
}
// Calculate updated/added bounding boxes
foreach (var boundingBox in boundingBoxes)
{
if (!lastCache?.BoundingBoxes.Contains(boundingBox) ?? true) // In the case of no case, mark all tiles in all bounding boxes to be rebuilt
{
var tiles = NavigationMeshBuildUtils.GetOverlappingTiles(buildSettings, boundingBox);
foreach (var tile in tiles)
{
tilesToBuild.Add(tile);
}
}
}
// Check for removed bounding boxes
if (lastCache != null)
{
foreach (var boundingBox in lastCache.BoundingBoxes)
{
if (!boundingBoxes.Contains(boundingBox))
{
var tiles = NavigationMeshBuildUtils.GetOverlappingTiles(buildSettings, boundingBox);
foreach (var tile in tiles)
{
tilesToBuild.Add(tile);
}
}
}
}
long buildTimeStamp = DateTime.UtcNow.Ticks;
ConcurrentCollector <Tuple <Point, NavigationMeshTile> > builtTiles = new ConcurrentCollector <Tuple <Point, NavigationMeshTile> >(tilesToBuild.Count);
Dispatcher.ForEach(tilesToBuild.ToArray(), tileCoordinate =>
{
// Allow cancellation while building tiles
if (cancellationToken.IsCancellationRequested)
{
return;
}
// Builds the tile, or returns null when there is nothing generated for this tile (empty tile)
NavigationMeshTile meshTile = BuildTile(tileCoordinate, buildSettings, currentAgentSettings, boundingBoxes,
inputVertices, inputIndices, buildTimeStamp);
// Add the result to the list of built tiles
builtTiles.Add(new Tuple <Point, NavigationMeshTile>(tileCoordinate, meshTile));
});
if (cancellationToken.IsCancellationRequested)
{
Logger?.Warning("Operation cancelled");
return(result);
}
// Add layer to the navigation mesh
var layer = new NavigationMeshLayer();
result.NavigationMesh.LayersInternal.Add(currentGroup.Id, layer);
// Copy tiles from from the previous build into the current
NavigationMeshLayer sourceLayer = null;
if (oldNavigationMesh != null && oldNavigationMesh.LayersInternal.TryGetValue(currentGroup.Id, out sourceLayer))
{
foreach (var sourceTile in sourceLayer.Tiles)
{
layer.TilesInternal.Add(sourceTile.Key, sourceTile.Value);
}
}
foreach (var p in builtTiles)
{
if (p.Item2 == null)
{
// Remove a tile
if (layer.TilesInternal.ContainsKey(p.Item1))
{
layer.TilesInternal.Remove(p.Item1);
}
}
else
{
// Set or update tile
layer.TilesInternal[p.Item1] = p.Item2;
}
}
// Add information about which tiles were updated to the result
if (tilesToBuild.Count > 0)
{
var layerUpdateInfo = new NavigationMeshLayerUpdateInfo();
layerUpdateInfo.GroupId = currentGroup.Id;
layerUpdateInfo.UpdatedTiles = tilesToBuild.ToList();
result.UpdatedLayers.Add(layerUpdateInfo);
}
}
}
// Check for removed layers
if (oldNavigationMesh != null)
{
var newGroups = groups.ToLookup(x => x.Id);
foreach (var oldLayer in oldNavigationMesh.Layers)
{
if (!newGroups.Contains(oldLayer.Key))
{
var updateInfo = new NavigationMeshLayerUpdateInfo();
updateInfo.UpdatedTiles.Capacity = oldLayer.Value.Tiles.Count;
foreach (var tile in oldLayer.Value.Tiles)
{
updateInfo.UpdatedTiles.Add(tile.Key);
}
result.UpdatedLayers.Add(updateInfo);
}
}
}
// Store bounding boxes in new tile cache
newCache.BoundingBoxes = new List <BoundingBox>(boundingBoxes);
// Update navigation mesh
oldNavigationMesh = result.NavigationMesh;
result.Success = true;
return(result);
}
/// <summary>
/// Raycasts and stops at the first hit.
/// </summary>
/// <param name="from">From.</param>
/// <param name="to">To.</param>
/// <param name="collisionFilterGroups">The collision group of this shape sweep</param>
/// <param name="collisionFilterGroupFlags">The collision group that this shape sweep can collide with</param>
/// <returns>The list with hit results.</returns>
public HitResult Raycast(Vector3 from, Vector3 to, CollisionFilterGroups collisionFilterGroups, CollisionFilterGroupFlags collisionFilterGroupFlags)
{
var result = new HitResult(); //result.Succeeded is false by default
using (var rcb = new BulletSharp.ClosestRayResultCallback(from, to)
{
CollisionFilterGroup = (short)collisionFilterGroups,
CollisionFilterMask = (short)collisionFilterGroupFlags
})
{
collisionWorld.RayTest(ref from, ref to, rcb);
if (rcb.CollisionObject == null) return result;
result.Succeeded = true;
result.Collider = (PhysicsComponent)rcb.CollisionObject.UserObject;
result.Normal = rcb.HitNormalWorld;
result.Point = rcb.HitPointWorld;
result.HitFraction = rcb.ClosestHitFraction;
}
return result;
}
/// <summary>
/// Rebuilds outdated triangle data for colliders and recalculates hashes storing everything in StaticColliderData
/// </summary>
private void BuildInput(StaticColliderData[] collidersLocal, CollisionFilterGroupFlags includedCollisionGroups)
{
NavigationMeshCache lastCache = oldNavigationMesh?.Cache;
bool clearCache = false;
Dispatcher.ForEach(collidersLocal, colliderData =>
{
var entity = colliderData.Component.Entity;
TransformComponent entityTransform = entity.Transform;
Matrix entityWorldMatrix = entityTransform.WorldMatrix;
NavigationMeshInputBuilder entityNavigationMeshInputBuilder = colliderData.InputBuilder = new NavigationMeshInputBuilder();
// Compute hash of collider and compare it with the previous build if there is one
colliderData.ParameterHash = NavigationMeshBuildUtils.HashEntityCollider(colliderData.Component, includedCollisionGroups);
colliderData.Previous = null;
if (lastCache?.Objects.TryGetValue(colliderData.Component.Id, out colliderData.Previous) ?? false)
{
if ((!NavigationMeshBuildUtils.IsDynamicShape(colliderData.Component.ColliderShape) || !colliderData.CacheSettings.EnableAlwaysUpdateDynamicShape) &&
(colliderData.Previous.ParameterHash == colliderData.ParameterHash))
{
// In this case, we don't need to recalculate the geometry for this shape, since it wasn't changed
// here we take the triangle mesh from the previous build as the current
colliderData.InputBuilder = colliderData.Previous.InputBuilder;
colliderData.Planes.Clear();
colliderData.Planes.AddRange(colliderData.Previous.Planes);
colliderData.Processed = false;
return;
}
}
// Clear cache on removal of infinite planes
if (colliderData.Planes.Count > 0)
{
clearCache = true;
}
// Clear planes
colliderData.Planes.Clear();
// Return empty data for disabled colliders, filtered out colliders or trigger colliders
if (!colliderData.Component.Enabled || colliderData.Component.IsTrigger ||
!NavigationMeshBuildUtils.CheckColliderFilter(colliderData.Component, includedCollisionGroups))
{
colliderData.Processed = true;
return;
}
// Make sure shape is up to date
if (!NavigationMeshBuildUtils.HasLatestColliderShape(colliderData.Component))
{
colliderData.Component.ComposeShape();
}
// Interate through all the colliders shapes while queueing all shapes in compound shapes to process those as well
Queue <ColliderShape> shapesToProcess = new Queue <ColliderShape>();
if (colliderData.Component.ColliderShape != null)
{
shapesToProcess.Enqueue(colliderData.Component.ColliderShape);
while (shapesToProcess.Count > 0)
{
var shape = shapesToProcess.Dequeue();
var shapeType = shape.GetType();
if (shapeType == typeof(BoxColliderShape))
{
var box = (BoxColliderShape)shape;
var boxDesc = GetColliderShapeDesc <BoxColliderShapeDesc>(box.Description);
Matrix transform = box.PositiveCenterMatrix * entityWorldMatrix;
var meshData = GeometricPrimitive.Cube.New(boxDesc.Size, toLeftHanded: true);
entityNavigationMeshInputBuilder.AppendMeshData(meshData, transform);
}
else if (shapeType == typeof(SphereColliderShape))
{
var sphere = (SphereColliderShape)shape;
var sphereDesc = GetColliderShapeDesc <SphereColliderShapeDesc>(sphere.Description);
Matrix transform = sphere.PositiveCenterMatrix * entityWorldMatrix;
var meshData = GeometricPrimitive.Sphere.New(sphereDesc.Radius, toLeftHanded: true);
entityNavigationMeshInputBuilder.AppendMeshData(meshData, transform);
}
else if (shapeType == typeof(CylinderColliderShape))
{
var cylinder = (CylinderColliderShape)shape;
var cylinderDesc = GetColliderShapeDesc <CylinderColliderShapeDesc>(cylinder.Description);
Matrix transform = cylinder.PositiveCenterMatrix * entityWorldMatrix;
var meshData = GeometricPrimitive.Cylinder.New(cylinderDesc.Height, cylinderDesc.Radius, toLeftHanded: true);
entityNavigationMeshInputBuilder.AppendMeshData(meshData, transform);
}
else if (shapeType == typeof(CapsuleColliderShape))
{
var capsule = (CapsuleColliderShape)shape;
var capsuleDesc = GetColliderShapeDesc <CapsuleColliderShapeDesc>(capsule.Description);
Matrix transform = capsule.PositiveCenterMatrix * entityWorldMatrix;
var meshData = GeometricPrimitive.Capsule.New(capsuleDesc.Length, capsuleDesc.Radius, toLeftHanded: true);
entityNavigationMeshInputBuilder.AppendMeshData(meshData, transform);
}
else if (shapeType == typeof(ConeColliderShape))
{
var cone = (ConeColliderShape)shape;
var coneDesc = GetColliderShapeDesc <ConeColliderShapeDesc>(cone.Description);
Matrix transform = cone.PositiveCenterMatrix * entityWorldMatrix;
var meshData = GeometricPrimitive.Cone.New(coneDesc.Radius, coneDesc.Height, toLeftHanded: true);
entityNavigationMeshInputBuilder.AppendMeshData(meshData, transform);
}
else if (shapeType == typeof(StaticPlaneColliderShape))
{
var planeShape = (StaticPlaneColliderShape)shape;
var planeDesc = GetColliderShapeDesc <StaticPlaneColliderShapeDesc>(planeShape.Description);
Matrix transform = entityWorldMatrix;
Plane plane = new Plane(planeDesc.Normal, planeDesc.Offset);
// Pre-Transform plane parameters
plane.Normal = Vector3.TransformNormal(planeDesc.Normal, transform);
plane.Normal.Normalize();
plane.D += Vector3.Dot(transform.TranslationVector, plane.Normal);
colliderData.Planes.Add(plane);
}
else if (shapeType == typeof(ConvexHullColliderShape))
{
var hull = (ConvexHullColliderShape)shape;
Matrix transform = hull.PositiveCenterMatrix * entityWorldMatrix;
// Convert hull indices to int
int[] indices = new int[hull.Indices.Count];
if (hull.Indices.Count % 3 != 0)
{
throw new InvalidOperationException($"{shapeType} does not consist of triangles");
}
for (int i = 0; i < hull.Indices.Count; i += 3)
{
indices[i] = (int)hull.Indices[i];
indices[i + 2] = (int)hull.Indices[i + 1]; // NOTE: Reversed winding to create left handed input
indices[i + 1] = (int)hull.Indices[i + 2];
}
entityNavigationMeshInputBuilder.AppendArrays(hull.Points.ToArray(), indices, transform);
}
else if (shapeType == typeof(StaticMeshColliderShape))
{
var mesh = (StaticMeshColliderShape)shape;
Matrix transform = mesh.PositiveCenterMatrix * entityWorldMatrix;
// Convert hull indices to int
int[] indices = new int[mesh.Indices.Count];
if (mesh.Indices.Count % 3 != 0)
{
throw new InvalidOperationException($"{shapeType} does not consist of triangles");
}
for (int i = 0; i < mesh.Indices.Count; i += 3)
{
indices[i] = (int)mesh.Indices[i];
indices[i + 2] = (int)mesh.Indices[i + 1]; // NOTE: Reversed winding to create left handed input
indices[i + 1] = (int)mesh.Indices[i + 2];
}
entityNavigationMeshInputBuilder.AppendArrays(mesh.Vertices.ToArray(), indices, transform);
}
else if (shapeType == typeof(HeightfieldColliderShape))
{
var heightfield = (HeightfieldColliderShape)shape;
var halfRange = (heightfield.MaxHeight - heightfield.MinHeight) * 0.5f;
var offset = -(heightfield.MinHeight + halfRange);
Matrix transform = Matrix.Translation(new Vector3(0, offset, 0)) * heightfield.PositiveCenterMatrix * entityWorldMatrix;
var width = heightfield.HeightStickWidth - 1;
var length = heightfield.HeightStickLength - 1;
var mesh = GeometricPrimitive.Plane.New(width, length, width, length, normalDirection: NormalDirection.UpY, toLeftHanded: true);
var arrayLength = heightfield.HeightStickWidth * heightfield.HeightStickLength;
using (heightfield.LockToReadHeights())
{
switch (heightfield.HeightType)
{
case HeightfieldTypes.Short:
if (heightfield.ShortArray == null)
{
continue;
}
for (int i = 0; i < arrayLength; ++i)
{
mesh.Vertices[i].Position.Y = heightfield.ShortArray[i] * heightfield.HeightScale;
}
break;
case HeightfieldTypes.Byte:
if (heightfield.ByteArray == null)
{
continue;
}
for (int i = 0; i < arrayLength; ++i)
{
mesh.Vertices[i].Position.Y = heightfield.ByteArray[i] * heightfield.HeightScale;
}
break;
case HeightfieldTypes.Float:
if (heightfield.FloatArray == null)
{
continue;
}
for (int i = 0; i < arrayLength; ++i)
{
mesh.Vertices[i].Position.Y = heightfield.FloatArray[i];
}
break;
}
}
entityNavigationMeshInputBuilder.AppendMeshData(mesh, transform);
}
else if (shapeType == typeof(CompoundColliderShape))
{
// Unroll compound collider shapes
var compound = (CompoundColliderShape)shape;
for (int i = 0; i < compound.Count; i++)
{
shapesToProcess.Enqueue(compound[i]);
}
}
}
}
// Clear cache on addition of infinite planes
if (colliderData.Planes.Count > 0)
{
clearCache = true;
}
// Mark collider as processed
colliderData.Processed = true;
});
if (clearCache && oldNavigationMesh != null)
{
oldNavigationMesh = null;
}
}
/// <summary>
/// Raycasts penetrating any shape the ray encounters.
/// Filtering by CollisionGroup
/// </summary>
/// <param name="from">From.</param>
/// <param name="to">To.</param>
/// <param name="resultsOutput">The list to fill with results.</param>
/// <param name="collisionFilterGroups">The collision group of this shape sweep</param>
/// <param name="collisionFilterGroupFlags">The collision group that this shape sweep can collide with</param>
public void RaycastPenetrating(Vector3 from, Vector3 to, IList<HitResult> resultsOutput, CollisionFilterGroups collisionFilterGroups, CollisionFilterGroupFlags collisionFilterGroupFlags)
{
using (var rcb = new XenkoAllHitsRayResultCallback(ref from, ref to, resultsOutput)
{
CollisionFilterGroup = (short)collisionFilterGroups,
CollisionFilterMask = (short)collisionFilterGroupFlags
})
{
collisionWorld.RayTest(ref from, ref to, rcb);
}
}
/// <summary>
/// Checks if a static collider passes the given filter group
/// </summary>
/// <param name="collider">The collider to check</param>
/// <param name="includedCollisionGroups">The collision filter</param>
/// <returns><c>true</c> if the collider passes the filter, <c>false</c> otherwise</returns>
public static bool CheckColliderFilter(StaticColliderComponent collider, CollisionFilterGroupFlags includedCollisionGroups)
{
return(((CollisionFilterGroupFlags)collider.CollisionGroup & includedCollisionGroups) != 0);
}
/// <summary>
/// Raycasts penetrating any shape the ray encounters.
/// </summary>
/// <param name="simulation">Physics simulation.</param>
/// <param name="raySegment">Ray.</param>
/// <param name="collisionFilterGroups">The collision group of this shape sweep</param>
/// <param name="collisionFilterGroupFlags">The collision group that this shape sweep can collide with</param>
/// <returns>The list with hit results.</returns>
/// <exception cref="ArgumentNullException">If the simulation argument is null.</exception>
public static FastList <HitResult> RaycastPenetrating(this Simulation simulation, RaySegment raySegment, CollisionFilterGroups collisionFilterGroups, CollisionFilterGroupFlags collisionFilterGroupFlags)
{
if (simulation == null)
{
throw new ArgumentNullException(nameof(simulation));
}
var result = new FastList <HitResult>();
simulation.RaycastPenetrating(raySegment.Start, raySegment.End, result, collisionFilterGroups, collisionFilterGroupFlags);
return(result);
}
internal void AddCollider(PhysicsComponent component, CollisionFilterGroupFlags group, CollisionFilterGroupFlags mask)
{
collisionWorld.AddCollisionObject(component.NativeCollisionObject, (BulletSharp.CollisionFilterGroups)group, (BulletSharp.CollisionFilterGroups)mask);
}
/// <summary>
/// Performs a sweep test using a collider shape and never stops until "to"
/// </summary>
/// <param name="shape">The shape.</param>
/// <param name="from">From.</param>
/// <param name="to">To.</param>
/// <param name="resultsOutput">The list to fill with results.</param>
/// <param name="collisionFilterGroups">The collision group of this shape sweep</param>
/// <param name="collisionFilterGroupFlags">The collision group that this shape sweep can collide with</param>
/// <exception cref="System.Exception">This kind of shape cannot be used for a ShapeSweep.</exception>
public void ShapeSweepPenetrating(ColliderShape shape, Matrix from, Matrix to, IList<HitResult> resultsOutput, CollisionFilterGroups collisionFilterGroups, CollisionFilterGroupFlags collisionFilterGroupFlags)
{
var sh = shape.InternalShape as BulletSharp.ConvexShape;
if (sh == null) throw new Exception("This kind of shape cannot be used for a ShapeSweep.");
using (var rcb = new XenkoAllHitsConvexResultCallback(resultsOutput)
{
CollisionFilterGroup = (BulletSharp.CollisionFilterGroups)collisionFilterGroups,
CollisionFilterMask = (BulletSharp.CollisionFilterGroups)collisionFilterGroupFlags
})
{
collisionWorld.ConvexSweepTest(sh, from, to, rcb);
}
}
internal void AddCharacter(CharacterComponent character, CollisionFilterGroupFlags group, CollisionFilterGroupFlags mask)
{
if (discreteDynamicsWorld == null) throw new Exception("Cannot perform this action when the physics engine is set to CollisionsOnly");
var collider = character.NativeCollisionObject;
var action = character.KinematicCharacter;
discreteDynamicsWorld.AddCollisionObject(collider, (BulletSharp.CollisionFilterGroups)group, (BulletSharp.CollisionFilterGroups)mask);
discreteDynamicsWorld.AddCharacter(action);
character.Simulation = this;
}
internal void AddCollider(PhysicsComponent component, CollisionFilterGroupFlags group, CollisionFilterGroupFlags mask)
{
collisionWorld.AddCollisionObject(component.NativeCollisionObject, (BulletSharp.CollisionFilterGroups)group, (BulletSharp.CollisionFilterGroups)mask);
}