diff --git a/README.md b/README.md old mode 100644 new mode 100755 index 56108d351..dbbf92b9e --- a/README.md +++ b/README.md @@ -1 +1 @@ -# tracking unity packages \ No newline at end of file +Unity's C# stateless physics library. This package is still in experimental phase. diff --git a/package/CHANGELOG.md b/package/CHANGELOG.md new file mode 100755 index 000000000..a9b00d1f9 --- /dev/null +++ b/package/CHANGELOG.md @@ -0,0 +1,2 @@ +## [0.0.1] - 2019-03-12 +* Initial package version diff --git a/package/CHANGELOG.md.meta b/package/CHANGELOG.md.meta new file mode 100755 index 000000000..813dbbe7c --- /dev/null +++ b/package/CHANGELOG.md.meta @@ -0,0 +1,7 @@ +fileFormatVersion: 2 +guid: 259e073a32ee4df42be6858276d2f51d +TextScriptImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Documentation~/collision_queries.md b/package/Documentation~/collision_queries.md new file mode 100755 index 000000000..e4125bc47 --- /dev/null +++ b/package/Documentation~/collision_queries.md @@ -0,0 +1,252 @@ +# Spatial queries and filtering + +Collision queries or spatial queries are one of the most important features of any physics engine and often drive a significant amount of game logic. Unity physics has a powerful collision query system which supports queries like ray casting, linear casting and closest point estimation. These queries support options like efficient collision filtering and user data retrieval. + +## Local Vs Global + +Queries can be performed against individual colliders or against an entire collision world. When performed against an entire world, a query acceleration structure, in our case a bounding volume tree, is used for efficiency. + +You can choose to create you own collision worlds which are entirely independent of the physics world. However, if you are performing queries against the same physics world you are simulating, which is common, then you can take advantage of the fact that the broad phase had already been built. + +## Query Types + + + +| Query Type | Input |Description | +| -- | -- | -- | +| Ray cast | Origin, Direction, Filter | Finds all (or closest) intersections for an oriented line segment | +| Collider cast | Collider, Origin, Direction | Find all (or closest) intersection for the given shape swept along the given line segment in the given context | +| Collider Distance | Collider, Origin, Max distance | Finds the closest point between the given shape and any others within a specified maximum radius | +| Point Distance | Point, Filter, Max distance | Finds the closest point to any shapes within a given maximum radius of the specified point | +| Overlap query | AABB, Filter | Find all the bodies with bounding boxes overlapping a given area | + +Convenience functions are provided to return any hit, the closest hit or all hits. These convenience functions use [collectors](#Collectors) to interpret their results. + + Note: We also provide direct access to all underlying query algorithms, e.g. ray sphere intersection, so you can + use these algorithms directly if desired without allocating memory for the collision world / colliders. + +### Query outputs + +Many queries produce similar outputs or hits as they are referred to in the code. These hits have a subset of the following fields + +- **Fraction** : The proportion along the ray or cast in the given direction to the point of intersection. +- **Position** : The point of intersection on the surface, in world space. +- **SurfaceNormal** : The normal to the surface at the point of intersection, in world space. +- **RigidBodyIndex** : The index of the rigid body in the collision world if a world query was performed +- **ColliderKey** : Internal information about which part of a composite shape (e.g. mesh) was hit i.e. a reference to which triangle or quad in the mesh collider. + +### Ray Cast + +Ray cast queries use a point and direction as their input and produce a set of hit results. + +Lower level ray cast routines against surfaces have a slightly different output. They do not compute the surface intersection point explicitly for efficiency reasons. Instead, for a ray of origin,`O`, and direction (including distance), `D`, they return a hit fraction, `f`, which can be used later to compute the hit position if needed using `O + (D * f)`. See [RayCast.cs](..\Unity.Physics\Collision\Queries\Raycast.cs) for more details + +Ray intersection starting inside spheres, capsules, box and convex shapes do not report an intersection as the ray leaves the volume. + +### Collider Cast + +Collider casts sweep a collider along a ray stopping at the first point of contact with another collider. These queries can be significantly more expensive than performing a ray cast. In the image below you can see the results (magenta) of casting a collider (orange) against other colliders in the world (yellow) + +![collider_cast](images/collider_cast_queries.gif) + +Collider cast inputs are +- **Collider** : A reference to the collider to cast +- **Position** : The initial position of the collider +- **Orientation** : The initial orientation of the collider +- **Direction** : The direction to cast and distance. + + +### Distance query + +Distance queries or closest point queries are often used to determine proximity to surfaces. The image below shows the results (magenta points) of a distance query between the query collider (orange) and the rest of the collision world (yellow). You can see that not all queries return a result for all shapes. This is because the query has specified a maximum range which helps control the computational cost of the query. + +![Closest_Points](images/closest_points_all_hits.gif) + + +Distance query inputs include + +- **Collider** : (Optional) If present then this distance query is between pairs of surfaces. If absent then we are performing a closest point query to a fixed point in world space. +- **Position** : The position of the collider to use as the source of the query or the point in world space to query from. +- **Orientation** : The initial orientation of the collider. +- **Filter** : For point queries this allows you to specify a collision filter +- **MaxDistance** : points further than this range are not considered. Try to keep this value as small as possible for your needs for best performance. + +## Region queries + +Region queries are performed by calling `OverlapAabb` directly on the `CollisionWorld`. Given an `Aabb` and a `CollisionFilter` this query returns a list of indices into the bodies in the `CollisionWorld`. + +## Collectors + +Collectors provide a code driven way for you to intercept results as they are generated from any of the queries. When intercepting a result you can control the efficiency of a query by, for example, exiting early. We provide 3 implementations of the collector interface which return the closest hit found, all hits found or exit early is any hit is found. + +## Filtering + +Filtering is the preferred data driven method for controlling which results queries will perform. Our default collision filter provides for a simple but flexible way to control which bodies are involved in spatial queries or collision detection. + +The default collision filter is designed around a concept of collision layers and has 3 important members. + +| Member | Type | Purpose | +| --- | --- | --- | +| MaskBits | uint | A bitmask which describes which layers a collider belongs too. | +| CategoryBits | uint | A bitmask which describes which layers this collider should interact with | +| GroupIndex | int | An override for the bit mask checks. If the value in both objects is equal and positive, the objects always collide. If the value in both objects is equal and negative, the objects never collide. | + +When determining if two colliders should collide or a query should be performed we check the mask bits of one against the category bots of the other. + +```csharp +public static bool IsCollisionEnabled(CollisionFilter filterA, CollisionFilter filterB) +{ + if (filterA.GroupIndex > 0 && filterA.GroupIndex == filterB.GroupIndex) + { + return true; + } + if (filterA.GroupIndex < 0 && filterA.GroupIndex == filterB.GroupIndex) + { + return false; + } + return + (filterA.MaskBits & filterB.CategoryBits) != 0 && + (filterB.MaskBits & filterA.CategoryBits) != 0; +} +``` + +Currently the editor view of the Collision Filter just exposes the Category and Mask as 'Belongs To' and 'Collides With'. Think of each layer listed in those drop downs as being or-d together so that CategoryBits = (1u << belongLayer1) | (1u << belongLayer2) etc. Similarly for the' Collides With' for the mask. The groupIndex is not exposed currently in the editor, but do use at runtime if you have the need (eg, '-1' for a few objects you dont want to collide with each other but dont want to change the general settings for layers ) + +# Code examples + +## Ray casts + + ```csharp + public Entity Raycast(float3 RayFrom, float3 RayTo) + { + var physicsWorldSystem = Unity.Entities.World.Active.GetExistingManager(); + var collisionWorld = physicsWorldSystem.PhysicsWorld.CollisionWorld; + RaycastInput input = new RaycastInput() + { + Ray = new Ray() + { + Origin = RayFrom, + Direction = RayTo - RayFrom + }, + Filter = new CollisionFilter() + { + CategoryBits = ~0u, // all 1s, so all layers, collide with everything + MaskBits = ~0u, + GroupIndex = 0 + } + }; + + RaycastHit hit = new RaycastHit(); + bool haveHit = collisionWorld.CastRay(input, out hit); + if (haveHit) + { + // see hit.Position + // see hit.SurfaceNormal + Entity e = physicsWorldSystem.PhysicsWorld.Bodies[hit.RigidBodyIndex].Entity; + return e; + } + return Entity.Null; + } +``` + +That will return the closet hit Entity along the desired ray. You can inspect the results of RaycastHit for more information such as hit position and normal etc. + +## Collider casts +ColliderCasts are very similar tio the ray casts, just that we need to make (or borrow from an existing PhysicsCollider on a body) a Collider. If you read the [Getting Started](getting_started.md) guide the Collider creation code will look familar. + +```csharp + public unsafe Entity SphereCast(float3 RayFrom, float3 RayTo, float radius) + { + var physicsWorldSystem = Unity.Entities.World.Active.GetExistingManager(); + var collisionWorld = physicsWorldSystem.PhysicsWorld.CollisionWorld; + + var filter = new CollisionFilter() + { + CategoryBits = ~0u, // all 1s, so all layers, collide with everything + MaskBits = ~0u, + GroupIndex = 0 + }; + + BlobAssetReference sphereCollider = Unity.Physics.SphereCollider.Create(float3.zero, radius, filter); + + ColliderCastInput input = new ColliderCastInput() + { + Position = RayFrom, + Orientation = quaternion.identity, + Direction = RayTo - RayFrom, + Collider = (Collider*)sphereCollider.GetUnsafePtr() + }; + + ColliderCastHit hit = new ColliderCastHit(); + bool haveHit = collisionWorld.CastCollider(input, out hit); + if (haveHit) + { + // see hit.Position + // see hit.SurfaceNormal + Entity e = physicsWorldSystem.PhysicsWorld.Bodies[hit.RigidBodyIndex].Entity; + return e; + } + return Entity.Null; + } +``` + +## Cast Performance + +The above code all calls into Unity.Physics through normal C#. That is fine, will work, but is not at all optimal. To get really good performance from the casts and other queries you should do them from within a Burst compiled job. That way the code within the Unity Physics for the cast can also avail of Burst. If you are already in a Burst job, just call as normal, otherwise you will need to create a simple Job to do it for you. Since it is a threaded job, try to batch a few together too and wait some time later for results instead of straight away. + +```csharp + [BurstCompile] + public struct RaycastJob : IJobParallelFor + { + [ReadOnly] public Physics.CollisionWorld world; + [ReadOnly] public NativeArray inputs; + public NativeArray results; + + public unsafe void Execute(int index) + { + Physics.RaycastHit hit; + world.CastRay(inputs[index], out hit); + results[index] = hit; + } + } + + public static JobHandle ScheduleBatchRayCast(Physics.CollisionWorld world, + NativeArray inputs, NativeArray results) + { + JobHandle rcj = new RaycastJob + { + inputs = inputs, + results = results, + world = world + + }.Schedule(inputs.Length, 5); + return rcj; + } +``` + +If the scene is complex it may be worth doing so, even just for one ray. For example to call the above for one ray and wait: + +```csharp + public static void SingleRayCast(Physics.CollisionWorld world, Physics.RaycastInput input, + ref Physics.RaycastHit result) + { + var rayCommands = new NativeArray(1, Allocator.TempJob); + var rayResults = new NativeArray(1, Allocator.TempJob); + rayCommands[0] = input; + var handle = ScheduleBatchRayCast(world, rayCommands, rayResults); + handle.Complete(); + result = rayResults[0]; + rayCommands.Dispose(); + rayResults.Dispose(); + } +``` + +The code will be similar for Collider casts and overlap queries too. + + +## Using Collider keys + +Collider keys correspond to the internal primitives interleaved with the bounding volume data. They do not correspond 1:1 with the triangles of a Unity.Mesh when a mesh collider is created from one. At a later stage we intend to provide a mapping or embed user data to support this use case. But you can at least get the triangle data for a given ray hit above, using Collider->GetChild(key, out childleaf), but most of the information you need should already be in the hit return struct (pos, normal, etc). + + +[Back to Index](index.md) diff --git a/package/Documentation~/core_components.md b/package/Documentation~/core_components.md new file mode 100755 index 000000000..d633cca99 --- /dev/null +++ b/package/Documentation~/core_components.md @@ -0,0 +1,72 @@ +In the [getting started](getting_started.md) section, we discussed how to setup and configure bodies and shapes. Under the hood, when we start simulating the scene, several conversion systems (`PhysicsBodyConversionSystem`, `PhysicsShapeConversionSystem` and `PhysicsJointConversionSystem`) read the `PhysicsShape`, `PhysicsBody` and joint scripts (there are several types of joint script), convert them to components and add them to your entities. Various physics systems read from these components to generate input to the simulation. This document discusses each of these components, so you can write systems which process these to apply game effects, or even create simulated bodies from code. + +## Collider `PhysicsCollider` + +This is the most important component for the simulation of physics. By adding this component to an entity, you declare that this body will participate in the physics simulation and collision queries (though, we also need a Translation and Rotation component). This component decides what the collision geometry "looks" like to the physics simulation. This component is analogous to a mesh in a rendering system. For performance reasons, we try to avoid using a mesh during physics - we use specialized primitive types when possible, which greatly simplifies the process of determining collisions between two colliders. For example, if a collision geometry can be represented by a sphere, we can write collision tests which only need to consider the sphere center and radius; with a mesh wrapping the same sphere, we would need to consider every triangle in the +mesh. + +The most important property of a `PhysicsCollider` is a `BlobAssetReference` to the collider data used by the physics simulation (which is in a format optimized for collision queries) - each of our collider types implement an `ICollider` interface, and for each collider type, we have a static `Create()` function, which takes parameters specific to the shape -- for example , a `SphereCollider` is built from a position and a radius, while a `ConvexCollider` is built from a point cloud. + +_Note_ that it is possible to create a collider which has a null `BlobAssetReference``. We'll still simulate the body, but the results may not be what you expect. This can be useful in some particular scenarios, where you only have joints. + +### Collider materials + +The collider also stores a material, which describes how it reacts when in collision with other objects. + +The [restitution](https://en.wikipedia.org/wiki/Coefficient_of_restitution) of a material determines how "bouncy" the material is - how much of a body's velocity is preserved when it collides with another. A value of zero indicates that the object should not bounce at all, while a value of one indicates that it should all the speed of the object should be preserved. _Note_ that due to numerical precision and approximations inside the physics simulation, a body with a restitution of one will eventually come to rest. + +The [coefficient of friction](https://en.wikipedia.org/wiki/Friction) of a body relates how "sticky" an object is when another object is sliding along it's surface. This is the ratio between the force pushing down on the surface and the force pushing against the relative velocities between the bodies. A value of zero means that friction would not slow down the body, while higher values indicate that more energy should be lost. + +Both friction and restitution have a `CombinePolicy` which determines how the engine should merge two different values. For example, you may want to always use the largest or smallest value in a collision. + +In addition to these, a material also has a set of flags, which enable special behaviors during the physics simulation. The two most important of these are: + +* `IsTrigger`: if this flag is enabled, the collider is treated as a "detector" rather than as a physical body. This means it cannot receive forces from a collision, instead, it will raise an event to signify that an overlap occurred. For example, you can use this to determine when your player enters a specific region. +* `EnableCollisionEvents`: this is similar to the previous flag, but still allows the body to push other bodies normally. The events that the simulation raises, then can be used to determine how objects are colliding -- this would, for example, allow you to play sound events. + +### The Collision Filter + +Each collider also has a `CollisionFilter` which allows you to control what objects are permitted to collide with each other. The properties on this object allow you to categorize objects in relation to what types they collide with. For example, you might want to mark certain colliders as "transparent" so that when performing a raycast test to determine if two characters can see each other, they are able to "see through" colliders which have the transparent bit set. + +The default values for collision filter ensure that every object collides with every other object. By configuring the filter in particular ways, you are able to opt-out of select collisions, depending on what you want from gamecode. + +# Dynamic bodies + +By itself, a world containing entities with `PhysicsCollider` components won't actually _do_ anything. This is because the bodys we declared are all treated as static -- they cannot move, and from the perspective of collision, they have infinite mass. In order to make our simulations more interesting, we'll want to add the ability for body transforms to change. + +Adding a `PhysicsVelocity` component makes the physics simulation aware that the collider can have some linear and angular speed and that the collider should move. You can change the values of this component yourself if you wish to control how a collider is moving, but during the physics simulation, we also compute a new value for velocity (from gravity and forces from contacts/joints) and update the component. + +# Mass + +So now, suppose you have a scene with two physics colliders; one of which has a velocity pointing towards the static bollider. When you press play, the moving collider moves right through the static one. You haven't changed the collision filter, or made one a trigger, so what happened? This is a case of an unstoppable force meeting an immovable object. As we discussed before, the collider without a velocity cannot move. When the two collide, we would expect forces to be applied between them. + +The problem is that, even though one collider is moving, the simulation does not know how _heavy_ it is, so does not know how it would respond to collisions. In this case, we treat the moving collidable as if it had _infinite_ mass, so it will just push every object out of the way. + +This kind of behaviour is useful in some scenarios. Suppose you had an elevator object in-game. It makes sense that it should follow your desired path _exactly_ -- it should move no matter how many characters are in the lift and it should not get stuck on "snags" inside a bumpy elevator shaft. This behaviour is sometimes called "kinematic" or "keyframed" in other physics simulations. + +To inform the simulation of masses, we use the `PhysicsMass` component. This tells physics how an collider reacts to an impulse. It stores the mass and [inertia tensor](https://en.wikipedia.org/wiki/Moment_of_inertia) for that entity as well as a transform describing the orientation of the inertia tensor and center of mass. + +_Note_ some of these values are stored as inverses, which speeds up many of the internal physics calculations. It also allows you to specify infinite values, by setting the relevant component to zero. + +While you can provide these values yourself, it is not necessary in many cases; the `ICollider` for a collider has a `MassProperties` property, where appropriate values are calculated for you automatically. You might find it more useful to use the calculated `MassProperties` as a starting point, and then scale them -- for example, by multiplying the mass by ten for an extra-heavy gameplay object. + + + +# Other components + +Some additional components allow you to change the simulation behavior: + +* `PhysicsStep` allows you to overide the default simulation parameters for the whole scene, for example, by changing the direction of gravity or increasing the solver iteration count, making it more rigid (at the cost of performance) +* `PhysicsDamping` allows you to add a per-collider "slow-down" factor. Every step, a collider with this component will have it's velocities scaled down. This could slow down objects, making them more stable, or be used as a cheap approximation of aerodynamic drag. +* `PhysicsGravityFactor` allows you to scale the amount of gravity applied to an individual collider. Some objects _feel_ better if they appear to fall faster. Some objects (for example, hot air balloons) appear to fall _up_, which can be emulated with a negative gravity factor. + + +# Joints + +A `PhysicsJoint` component is a little different from the others described here. It _links_ two entities together with some sort of constraint, for example, a door hinge. These entities should have physics collider components, and at least one of them should have a velocity -- otherwise the joint would have no effect. During the physics step, we solve the joint as well as the contacts that affect each entity. + +The behavior of the joint is described by the `JointData` property; like the collider component, this is a `BlobAssetReference` to a `JointData`. The precise behavior of each joint depends on the type of this data. We have pre-created several types of joint, but do not yet have a complete repetroire of joints -- as such, some of this information is likely to change in the near future. For the joint types which are implemented, there are static creation functions in `Physics.JointData` and, like with shapes, the input parameters vary between different types. For example, the `CreateBallAndSocket` simply needs to know where the joint is located relative to each body, while `CreateLimitedHinge()` additionally needs to know what axis the bodies are permitted to rotate about, and what the minimum and maximum limit for this rotation is. + +In addition to specifying the joint data and the entities, an important setting is `EnableCollision` -- this defaults to _off_, which is the recommended setting. If you have two bodies constrained together (such as a door, attached to a car) it is very likely that they overlap each other to some degree. When this happens, you can imagine that the joint pulls the objects together, while the collision detection is pushing them apart. This leads to "fighting" between the joint and collision, resulting in unstable simulation or too many events being raised. When `EnableCollision` is _off_, the physics simulation will not perform collision detection between the two bodies, even if the collider's collision filter would normally say they should collide. _Note_ if you have multiple joints between a pair of bodies, collisions will be enabled if _any_ of the joints have requested collision. + +[Back to Index](index.md) diff --git a/package/Documentation~/design.md b/package/Documentation~/design.md new file mode 100755 index 000000000..0f4d895c6 --- /dev/null +++ b/package/Documentation~/design.md @@ -0,0 +1,33 @@ +# Unity Physics + +## Design philosophy + +We want to provide you with a complete deterministic rigid body dynamics and spatial query system written entirely in high performance C# using DOTS best practices. The design of Unity Physics follows from the overall DOTS philosophy of minimal dependencies and complete control. Many experiences today do not need a full monolithic physic package like PhysX or Havok. Instead you often want simpler subset of features that you can freely control and customize to achieve your vision. Unity Physics is designed to balance this control without sacrificing on performance in areas we feel it is critical. In order to achieve this goal we've made a number of design decisions described here + +### Stateless + +Modern physics engines maintain large amounts of cached state in order to achieve high performance and simulation robustness. This comes at the cost of added complexity in the simulation pipeline which can be a barrier to modifying code. It also complicates use cases like networking where you may want to roll back and forward physics state. Unity Physics forgoes this caching in favor of simplicity and control. + +### Modular + +Core algorithms are deliberately decoupled from jobs and ECS to encourage their reuse and to free you from the underlying framework and stepping strategy. As an example of this see the immediate mode sample which steps a physical simulation multiple times in a single frame, independent of the job system or ECS, to show future predictions. + +### Highly performant + +For any physics features that do not cache state, e.g. raw collision queries, we expect Unity Physics performance to be on par with, or outperform, similar functionality from commercially available physics engines. + +### Interoperable + +We keep data compatibility between Unity Physics and the Havok Physics integration into unity (HPI). This give an easy upgrade path for those cases where you do want the full performance and robustness of a commercial solution. We split a single simulation step into discrete parts allowing you to write important user code like contact modification or trigger callbacks once and reuse this with either engine. In the future we intend to allow both Unity Physics and HPI to run side by side. + +## Code base structure + +|Collider|Description| +|---|---| +| Base | Containers and Math used throughout Unity.Physics | +| Collision | Contains all code for collision detection and spatial queries | +| Dynamics | Contains all code for integration, constraint solving and world stepping | +| Extensions | Optional components for characters, vehicles, debugging helpers etc. | +| ECS | Contain the components and systems for exposing Unity Physics to ECS | + +[Back to Index](index.md) diff --git a/package/Documentation~/getting_started.md b/package/Documentation~/getting_started.md new file mode 100755 index 000000000..f3876131a --- /dev/null +++ b/package/Documentation~/getting_started.md @@ -0,0 +1,306 @@ + +# Simulating + +Since the simulation is *state-less*, that is to say does not cache anything frame to frame, the very first thing it needs to do is get all the current state from the components on the body entities. This is the building phase at the start. The simulation then, at a high level, takes all bodies active in the scene and sees what shapes are overlapping. This is the *broadphase* of the simulation. Any that are overlapping, if they should collide based on the *filtering* information on them, we work out the exact point of contact. This is call *narrowphase*. Based on those collisions we can work out a response taking into account all the mass properties (eg inertia), friction and restitution (bounciness), and where they are colliding. We *solve* that collision along with any other joints that may be restricting the body. This solver stage produces new velocities for the affected bodies. We then *integrate* all dynamic bodies forward in time, that is to say move the dynamic bodies by the linear and angular velocities taking the current time step into account. That new transform for the body is then applied to the Entity that represents the body. + +## Setting up a simulation in Editor + +First you will need a static body so that your dynamic bodies can hit something and not just fall out of the world. Make a normal 'GameObject->3D Object->Cube'. Set the scale to 20,1,20 say. Remove the 'Box Collider' component, that is the older Physics representation, we don't need it. Instead add a **'Physics Shape'** component and set its Shape Type drop down to 'Box'. Then select 'Fit to Render Mesh'. The wireframe outline of the collider shape should match up with the graphics version. If we don't add a 'Physics Body' here it will assume it wants to be a static body, which is fine for this object. We do need to add a **'Convert to Entity'** component though, to tell Unity that the Game Object we see in the editor should become an Entity. + +Now add something to collide with the ground. Create 'GameObject->3D Object->Sphere', again remove the Collider that was added. Add a **'Physics Shape'** component as before, this time set Shape Type to 'sphere' and do a 'Fit to Render Mesh'. Since we want to set the body to be dynamic, we need to add a **'Physics Body'** to the object. You will want Motion Type to be 'Dynamic' with a mass of say 1. We again will need a **'Convert to Entity'**. It should look like the following picture: + +![collider_cast](images/DynamicBody.png) + +To simulate, just press Play in the Editor and it should fall and collide off the floor as expected. You have now just created your first Unity Physics simulation! + +## Exploring materials + +With the scene you just made, select the Sphere and change its Physics Shape->Material->Restitution to say 1 and play again. It should now bounce back from collision to nearly where it began. If you change the Linear Damping in the Physics Body to 0, then should get even closer. Play around with the friction (change the rotation of the floor to allow the sphere to roll) and restitution to get a feel of what they can do. The higher the friction the more the sphere will catch and roll rather than just slide, and the higher the restitution the more it will bounce on contact. + +## Exploring shapes + +So far we have used Box and Sphere for the collision types. Currently six distinct shapes are supported (and then *compound*, so multiple of these in one body, to allow more complex arrangements). Here they are in rough order of how expensive they are to use, from fastest to slowest: + +* **Sphere** : a point with radius, so the simplest form of collision shape. +* **Capsule** : a line with radius, so a little bit more expensive than a sphere. +* **Plane** : a plane bounded four sides, so like a quad in graphics, but all 4 corner vertices are coplanar. +* **Box** : an oriented box. +* **Cylinder** : currently this is actually a convex hull so will have the same cost as a similar complexity hull, so see next. +* **Convex Hull** : a 'convex hull' is as if you wrapped the object in paper, all the holes and concave parts will not be represented, just the outer most features. Collision detection algorithms for something you know is convex is much faster than concave. This is what a convex hull looks like for a teapot: +![collider_cast](images/Convex_Hull_Teapot.png) + +* **Mesh** : a arbitrary triangle soup. The most expensive and is assumed to be concave. All the convex shapes above all can collide with each other, and they can collide with Mesh, but Mesh can't collide with other Mesh in Unity Physics. So best used for static objects, but if you know your dynamic object Mesh will not need to collide with other Mesh it can be dynamic, but make sure to setup your collision filtering appropriately. + +### What is 'convex radius' + +In a lot of the shapes you will see a 'convex radius' field. That is an optimization for collision detection so that it can extend the hull of the shape out by a bit, like an outer shell of that size around the object. That has the side effect of rounding out corners, so you don't want it too big, and is normally quite small with respect to the size of the object. + +## Compound Shapes + +In order to have more than one shape on a body, and to allow arranging the shapes into a hierarchy that you can transform each shape in the body, we support the notion of Compound shapes. Simply have the gameobject in the Editor with the Physics Body contain children with Physics Shapes. They will all be merged into one compound PhysicsCollider for the body, where the leaf children of that compound collider are the colliders you specified in the Editor. This allows you to make complex rigid bodies made of multiple simpler representations. So collision detection will stay fast and you don't have to settle for say a convex hull around the whole object. + +Here is a simple example, where Body is a Physics Body (and a Physics Shape sphere in this case, so shape not only in children but itself too here), and then the children are the Physics Shape box limbs and sphere head. You should only need the normal Convert to Entity on the parent Physics Body, the other gameobjects with just the Physics Shapes should be found by that conversion as is. + +![compound_objs](images/CompundsH.png) + +![compooud_sim](images/Compunds.gif +) + +## More control over the Physics Step + +You can add a **Physics Step** to an object in the scene (just one object in the scene, say an empty object called Physics off the root you can set these values on). Don't forget to add the **Convert to Entity** on the object too. + +![collider_cast](images/StepPhysics.png) + +Try adding that to the sphere scene you had above, and change gravity to be 10 in y instead of the -9.81 default. On play the sphere should 'fall' upwards, instead of down. Can have any value you like for the world gravity, and note also for gravity that you can alter it per body via the Gravity Factor scalar (default is +1, so 1*WorldGravity, but can be any signed float value). + +For now 'Unity Physics' is the only choice you should use in the Simulation Type drop down. +'Havok Physics' will be an option soon to allow the exact same scene setup to be run through the full Havok Physics SDK instead. + + + +# Interacting with physics bodies + +Since Unity Physics is purely based in DOTS, rigid bodies are represented component data on the Entities. The simplified 'physics body' and 'physics shape' view you have in the Editor is actual composed of multiple components under the hood at runtime, to allow more efficient access and say save space for static bodies which do not require some of the data. + +The current set for a rigid body is: + +* PhysicsCollider : the shape of the body. Needed for all bodies that collide. +* PhysicsVelocity : the current velocities (linear and angular) of a dynamic body. (Dynamic body only) +* PhysicsMass : the current mass properties (inertia) of a dynamic body. (Dynamic body only) +* PhysicsDamping : the amount of damping to apply to the motion of the body. (Dynamic body only) +* PhysicsGravityFactor : scalar for how much gravity should affect the body. Assumed to be 1 if not on a body. (Dynamic body only) + +For all bodies we require the Unity.Transforms Translation and Rotation component data. This will represent the transform of the body. + +So for example to alter velocity of a body, query for its PhysicsVelocity and just set new values as you want. Lets make an example that just attracts all bodies close to a point. We will see later on in collision queries that we dont need to iterate over all bodies to get that subset to affect, but for simplicity lets just go with all here. + +First we need to make a ComponentSystem that on update will iterate over all bodies with PhysicsVelocity. We then will check the distance to the Translation of the body and see if close enough to affect. Then just alter the velocity as we want: + +```csharp +using Unity.Entities; +using Unity.Transforms; +using Unity.Physics; +using Unity.Mathematics; +using UnityEngine; + +public class AttractSystem : ComponentSystem +{ + public float3 center; + public float maxDistanceSqrd; + public float strength; + + protected override unsafe void OnUpdate() + { + ForEach( + ( ref PhysicsVelocity velocity, + ref Translation position, + ref Rotation rotation) => + { + float3 diff = center - position.Value; + float distSqrd = math.lengthsq(diff); + if (distSqrd < maxDistanceSqrd) + { + // Alter linear velocity + velocity.Linear += strength * (diff / math.sqrt(distSqrd)); + } + }); + } +}; +``` +If we wanted the bodies to go directly to the 'center' point above, we could just set velocity.Linear to be exactly the difference in position. That would get the body there in 1 second, assuming it does not hit anything or be affected by gravity etc en route. If you wanted to get there in one step, you need to know the physics delta time step, say Time.fixedDeltaTime, and divide the linear velocity by that. Note we are not setting position directly, but just altering velocity to get us to where we want to be. That way it can still interact with all other objects in the scene correctly, rather than just teleporting to a given position and hoping for the best. + +To add that test to our scene, the simplest if not pure ECS way, is just to add a MonoBehaviour that alters the system's values. + +```csharp +public class AttractComponent : MonoBehaviour +{ + public float maxDistance = 3; + public float strength = 1; + + void Update() + { + var vortex = World.Active.GetOrCreateManager(); + vortex.center = transform.position; + vortex.maxDistanceSqrd = maxDistance * maxDistance; + vortex.strength = strength; + } +} +``` + +Add that to a new empty GameObject positioned near the sphere (maybe just above it) and see what happens. Should sort of orbit that point in space (since we are adding a velocity towards the center but not cancelling out gravity down etc or any existing velocity on the object). + +### How it should be done + +The correct DOTS way to do this would be something along the lines of using IConvertGameObjectToEntity on the AttractComponent MonoBehavior we made, and in the IConvertGameObjectToEntity.Convert create an AttractData ComponentData with the center and strength etc that you attach to the Entity representing the point to attract to. Then in the AttractSystem do ForEach over all AttractData instead. The most efficient way to get all bodies close to that point is then to use the Unity.Physics.CollisionWorld OverlapAabb or if you want something more exact CalculateDistance (with MaxDistance in it set to our maxDistance above). We will cover such queries later on, so dont worry about it too much here. + +## Impulses + +We have seen now how to alter velocity in code, but it can be tricky to work out what velocity values to set to get a desired outcome. A common thing to want to do is apply an impulse at a given point on the body and have it react. To work out the affect on angular velocity from a given linear impulse. So say if shooting the object with a gun. We provide a few Unity.Physics.Extensions.ComponentExtensions to do the math for you, for instance ApplyImpulse(): + +```csharp + public static void ApplyImpulse(ref PhysicsVelocity pv, PhysicsMass pm, + Translation t, Rotation r, float3 impulse, float3 point) + { + // Linear + pv.Linear += impulse; + + // Angular + { + // Calculate point impulse + var worldFromEntity = new RigidTransform(r.Value, t.Value); + var worldFromMotion = math.mul(worldFromEntity, pm.Transform); + float3 angularImpulseWorldSpace = math.cross(point - worldFromMotion.pos, impulse); + float3 angularImpulseInertiaSpace = math.rotate(math.inverse(worldFromMotion.rot), angularImpulseWorldSpace); + + pv.Angular += angularImpulseInertiaSpace * pm.InverseInertia; + } + } +``` +Favour the form of calls that take the raw PhysicsVelocity etc rather than ones that query for them, to try an encourage the code to be more efficient and work over arrays of the data in efficient DOTS style. +Overtime we will provide more, but they will all work off the same PhysicsVelocity and PhysicsMass components you have today, so feel free to implement your own as you need. + +# Creating bodies in code + +So we have seen how to create bodies in the editor and how to alter their properties in code, but what about creating them dynamically on the fly. + +If you have a prefab setup with the body, then the SpawnRandomPhysicsBodies used in some of our Unity.Physics samples is a good place to start. Once we ConvertGameObjectHierarchy on the prefab, we get a Entity that we can Instantiate as many times as we like, setting the Translation and Rotation on it. In theory that alone will be fine, but if you want to be super fast you can also let Unity.Physics know they all share the same Collider. The PhysicsCollider, especially in the case of Convex Hull or Mesh shape colliders, can be expensive to create when first seen, so if you know is the same as another body and can share, just set the PhysicsCollider to say so as we do here. + + +```csharp +using System; +using Unity.Physics; +using Unity.Entities; +using Unity.Mathematics; +using Unity.Collections; +using UnityEngine; +using Unity.Transforms; +using Collider = Unity.Physics.Collider; + +public class SpawnRandomPhysicsBodies : MonoBehaviour +{ + public GameObject prefab; + public float3 range; + public int count; + + void OnEnable() { } + + public static void RandomPointsOnCircle(float3 center, float3 range, ref NativeArray positions, ref NativeArray rotations) + { + var count = positions.Length; + // initialize the seed of the random number generator + Unity.Mathematics.Random random = new Unity.Mathematics.Random(); + random.InitState(10); + for (int i = 0; i < count; i++) + { + positions[i] = center + random.NextFloat3(-range, range); + rotations[i] = random.NextQuaternionRotation(); + } + } + + void Start() + { + if (!enabled) return; + + // Create entity prefab from the game object hierarchy once + Entity sourceEntity = GameObjectConversionUtility.ConvertGameObjectHierarchy(prefab, World.Active); + var entityManager = World.Active.EntityManager; + + var positions = new NativeArray(count, Allocator.Temp); + var rotations = new NativeArray(count, Allocator.Temp); + RandomPointsOnCircle(transform.position, range, ref positions, ref rotations); + + BlobAssetReference sourceCollider = entityManager.GetComponentData(sourceEntity).Value; + for (int i = 0; i < count; i++) + { + var instance = entityManager.Instantiate(sourceEntity); + entityManager.SetComponentData(instance, new Translation { Value = positions[i] }); + entityManager.SetComponentData(instance, new Rotation { Value = rotations[i] }); + entityManager.SetComponentData(instance, new PhysicsCollider { Value = sourceCollider }); + } + + positions.Dispose(); + rotations.Dispose(); + } +} +``` +Save say the Sphere GameObject with rigid body setup you made earlier as a Prefab (as in just drag it from the scene hierarchy into the Assets view) and add a GameObject with the above SpawnRandomPhysicsBodies on it. Make count 1000 and set range to 2,2,2 and press play. You have now just created your first Unity.Physics runtime effect! + + +## Creating bodies from scratch + +The above used a pre existing prefab we setup in Editor, so glosses over some of what you needed to add, but was close. Here is code similar to CreateBody from the BasicPhysicsDemos in the samples. Our Colliders are stored in highly optimized structures, so some of the interaction with them is currently via 'unsafe' raw pointers, so you will have to bear with that for now here. + +```csharp +using Unity.Entities; +using Unity.Transforms; +using Unity.Physics; +using Unity.Mathematics; +using Unity.Rendering; + +public unsafe Entity CreateBody(RenderMesh displayMesh, float3 position, quaternion orientation, + BlobAssetReference collider, + float3 linearVelocity, float3 angularVelocity, float mass, bool isDynamic) +{ + EntityManager entityManager = EntityManager; + ComponentType[] componentTypes = new ComponentType[isDynamic ? 7 : 4]; + + componentTypes[0] = typeof(RenderMesh); + componentTypes[1] = typeof(TranslationProxy); + componentTypes[2] = typeof(RotationProxy); + componentTypes[3] = typeof(PhysicsCollider); + if (isDynamic) + { + componentTypes[4] = typeof(PhysicsVelocity); + componentTypes[5] = typeof(PhysicsMass); + componentTypes[6] = typeof(PhysicsDamping); + } + Entity entity = entityManager.CreateEntity(componentTypes); + + entityManager.SetSharedComponentData(entity, displayMesh); + + entityManager.AddComponentData(entity, new Translation { Value = position }); + entityManager.AddComponentData(entity, new Rotation { Value = orientation }); + entityManager.SetComponentData(entity, new PhysicsCollider { Value = collider }); + if (isDynamic) + { + Collider* colliderPtr = (Collider*)collider.GetUnsafePtr(); + entityManager.SetComponentData(entity, PhysicsMass.CreateDynamic(colliderPtr->MassProperties, mass)); + + float3 angularVelocityLocal = math.mul(math.inverse(colliderPtr->MassProperties.MassDistribution.Transform.rot), angularVelocity); + entityManager.SetComponentData(entity, new PhysicsVelocity() + { + Linear = linearVelocity, + Angular = angularVelocityLocal + }); + entityManager.SetComponentData(entity, new PhysicsDamping() + { + Linear = 0.01f, + Angular = 0.05f + }); + } + + return entity; +} +``` + +Getting your RenderMesh etc is up to you, and can create a Collider through the Create() function on each of the different Collider types, something like: + +```csharp + public Entity CreateDynamicSphere(RenderMesh displayMesh, float radius, float3 position, quaternion orientation) + { + // Sphere with default filter and material. Add to Create() call if you want non default: + BlobAssetReference spCollider = Unity.Physics.SphereCollider.Create(float3.zero, radius); + return CreateBody(displayMesh, position, orientation, spCollider, float3.zero, float3.zero, 1.0f, true); + }; +``` + +# Next steps + +Now that you can create bodies, it would be a good time to learn about collision queries and filtering. At that stage you should be all set for at least intermediate used of Unity Physics and can progress onto the more advanced topics such as modifying the simulation as it is running via callbacks and getting a deeper understanding of how the code works. + +[Collision queries](collision_queries.md) + +[(Back to Index)](index.md) + + diff --git a/package/Documentation~/glossary.md b/package/Documentation~/glossary.md new file mode 100755 index 000000000..c44aee7cb --- /dev/null +++ b/package/Documentation~/glossary.md @@ -0,0 +1,14 @@ +Glossary +========= + +| Term | Description | +|--|---| +|Inertia Tensor| Describes the mass distribtuion of a rigid body. In paricular it effects how the angular velocity of a body can change. See [Moment of inertia](https://en.wikipedia.org/wiki/Moment_of_inertia) | +|Center of mass (COM) | Forces applied to the [center of mass](https://en.wikipedia.org/wiki/Center_of_mass) will push the body in the direction of the force without causing rotation +|Convex shape| A convex shape or volume has the property that a line segment drawn between any two points inside the volume never leaves the volume. We exploit this property to accelerate the performance of collision detection and spatial queries. +| Degree of Freedom (DOF) | A describtion of how a linear system is free to move. In rigid body dynamcis we usualy refer to the 6 degrees of freedom a body has when moving in free space. These are 3 linear and 3 angular degress of freedom. The correspond to the axes you specify when describing a [constraints](joints_and_constraints.md/#Constraint) | +| Convex Radius | To improve the performance and robustness of collision detection we allow contacts to be generated within a small tolerance of the surface of a convex shape. This tolerance is referred to as convex radius | +|Jacobian|Typically describes a constraint used during constraint solving. See [Jaconian]( https://en.wikipedia.org/wiki/Jacobian_matrix_and_determinant)| + + +[Back to Index](index.md) diff --git a/package/Documentation~/images/BoxShape.png b/package/Documentation~/images/BoxShape.png new file mode 100755 index 000000000..291e0c770 Binary files /dev/null and b/package/Documentation~/images/BoxShape.png differ diff --git a/package/Documentation~/images/Compunds.gif b/package/Documentation~/images/Compunds.gif new file mode 100755 index 000000000..d3eb2b386 Binary files /dev/null and b/package/Documentation~/images/Compunds.gif differ diff --git a/package/Documentation~/images/CompundsH.png b/package/Documentation~/images/CompundsH.png new file mode 100755 index 000000000..20f7bb74d Binary files /dev/null and b/package/Documentation~/images/CompundsH.png differ diff --git a/package/Documentation~/images/Convex_Hull_Teapot.png b/package/Documentation~/images/Convex_Hull_Teapot.png new file mode 100755 index 000000000..90f552c3e Binary files /dev/null and b/package/Documentation~/images/Convex_Hull_Teapot.png differ diff --git a/package/Documentation~/images/DynamicBody.png b/package/Documentation~/images/DynamicBody.png new file mode 100755 index 000000000..b3db4b32b Binary files /dev/null and b/package/Documentation~/images/DynamicBody.png differ diff --git a/package/Documentation~/images/StaticBody.png b/package/Documentation~/images/StaticBody.png new file mode 100755 index 000000000..356fd6ca2 Binary files /dev/null and b/package/Documentation~/images/StaticBody.png differ diff --git a/package/Documentation~/images/StepPhysics.png b/package/Documentation~/images/StepPhysics.png new file mode 100755 index 000000000..85b1cdf75 Binary files /dev/null and b/package/Documentation~/images/StepPhysics.png differ diff --git a/package/Documentation~/images/closest_points.gif b/package/Documentation~/images/closest_points.gif new file mode 100755 index 000000000..56a02989f Binary files /dev/null and b/package/Documentation~/images/closest_points.gif differ diff --git a/package/Documentation~/images/closest_points_all_hits.gif b/package/Documentation~/images/closest_points_all_hits.gif new file mode 100755 index 000000000..9384999c8 Binary files /dev/null and b/package/Documentation~/images/closest_points_all_hits.gif differ diff --git a/package/Documentation~/images/collider_cast_queries.gif b/package/Documentation~/images/collider_cast_queries.gif new file mode 100755 index 000000000..1797d1a4c Binary files /dev/null and b/package/Documentation~/images/collider_cast_queries.gif differ diff --git a/package/Documentation~/index.md b/package/Documentation~/index.md new file mode 100755 index 000000000..2230b1a04 --- /dev/null +++ b/package/Documentation~/index.md @@ -0,0 +1,17 @@ +Introduction to Unity Physics +====== + +Unity Physics is a deterministic rigid body dynamics system and spatial query system written from the ground up using the Unity data oriented tech stack. Unity Physics originates from a close collaboration between Unity and the Havok Physics team at Microsoft. + +## About these documents + +These are very early stage documents which will be extended over time. Please read through the Design Philosophy and Getting Started at least, which should give you an good overview of how to use the physics system at a high level. As the code settles down we will add more explanation on how to modify the simulation and expand on how the algorithms work. The most up to date use of Unity Physics is the Samples for it in the main DOTs sample repository and they cover a wide range of use cases not yet covered in these documents, such as character controllers, ray cast driven cars, and examples of all the joints. + +## Read + +* [Design Philosophy](design.md) +* [Getting started](getting_started.md) +* [Collision queries](collision_queries.md) +* [Core components](core_components.md) +* [Modifying simulation results](simulation_modification.md) +* [Glossary](glossary.md) diff --git a/package/Documentation~/runtime_overview.md b/package/Documentation~/runtime_overview.md new file mode 100755 index 000000000..4a22bc02b --- /dev/null +++ b/package/Documentation~/runtime_overview.md @@ -0,0 +1,4 @@ + +Please come back later, work in progress. + +[Back to Index](index.md) diff --git a/package/Documentation~/simulation_modification.md b/package/Documentation~/simulation_modification.md new file mode 100755 index 000000000..4a0037e49 --- /dev/null +++ b/package/Documentation~/simulation_modification.md @@ -0,0 +1,45 @@ +# Overriding intermediate simulation results + +_Note_ hooks for simulation modification are a work in progress. They may be incomplete and the interfaces are very likely to change between releases. The examples project has one scene for each type of modification and it may be best to refer to those in order to see how the current interface behaves and what the API is. + +You can usually consider the physics simulation as a monolithic process whose inputs are components described in [core components](core_components.md) and outputs are updated `Translation`, `Rotation` and `PhysicsVelocity` components. However, internally, the Physics update is broken down into smaller subsections, the output of each becomes the input to the next. We have added the ability for you to read and modify this data, if necessary for your gameplay use-cases. + +In all cases, you should insert a job into the task dependencies for the physics world step. To make this easy to do without modifying the physics code, we allow you to request a delegate to be called when physics is setting up the jobs For each type of simulation result you wish to read or modify, you should implement the `Physics.SImulationCallbacks.Callback` delegate; this function receives a reference to all previously scheduled physics tasks as well as a reference to the simulation interface. In here, you should schedule any jobs you need and return the combined JobHandles of any jobs you schedule -- this will prevent future physics tasks from processing data before you are done with it. + +Then, _every step_ you should inform the physics simulation that you wish your function to be called; you do this by calling `Unity.Physics.Systems.StepPhysicsWorld.EnqueueCallback`, passing the time you wish your function to be called (which affects the kind of data you can access) where it will later be called at an appropriate time. The reason we require you to do this every frame is that you may have some modifications you wish to enable temporarily, based on the presence or properties of some other gameplay component. + +# Simulation phases + +This section briefly describes the sections of the simulation, what you can access and change. + +#### `Unity.Physics.SimulationCallbacks.Phase.PostCreateDispatchPairs` + +At this point, the simulation has generated the list of _potentially_ interacting objects, based on joints and the overlaps at the broadphase level. It has also performed some sorting, making the pairs of interacting objects suitable for multithreading. + +During this stage, you can use a `BodyPairs.Iterator` from `Simulation.BodyPairs` to see all these pairs. If, for some reason you wish to disable an interaction, the iterator is able to disable a pair before any processing is done. + +_Note_ from both a performance and debugging perspective, it is preferable to disable pairs using a collision filter. + +#### `Unity.Physics.SimulationCallbacks.Phase.PostCreateContacts` + +At this point, we have performed the low-level collision information between every pair of bodies by inspecting their colliders. You can access this through `Simulation.Contacts` where the data is in the format of a `ContactManifold` containing some shared properties, followed by a number (`ContactManifold.NumContacts`) of `ContactPoint` contact points. + +Here, you can modify the contacts, changing the separating normal or the contact positions and distances. + +It is also possible to _add_ contacts, which might be useful, for example, if you want to deliberately add "invisible walls" to some regions. + +#### `Unity.Physics.SimulationCallbacks.Phase.PostCreateContactJacobians` + +Now, the joint data from your joint components and contact data from the previous step has been converted to data which is suitable for consumption by the constraint solver. It represents the same as it did before, but has been transformed into a form which optimizes our ability to solve the constraints accurately -- so, the data appears to be significantly more abstract and harder to manipulate. + +Here, you can access the `SimulationData.Jacobians.Iterator` and inspect each of the jacobian datas. Since the data is harder to manage, we have provided some accessors to perform many reasonable modifications. For these, it is best to consult the sample scene. + +#### `Unity.Physics.SimulationCallbacks.Phase.PostSolveJacobians` + +This is nearly the final stage of the physics step. At this point we have solved all the constraints, which provides bodies with new velocities. The last thing to do is to use these velocities to update the positions of all the entities. + +This stage gives you an opportunity to modify the velocities before they feed back to the transforms by modifying `PhysicsWorld.MotionVelocities`. _Note_ this allows you to completely override the results of the simulation, so can cause objects to behave non-physically. You might consider using modifying the `PhysicsVelocity` components outside the simulation step, where the simulation will still get a chance to enforce constraints. + + +[Back to Index](index.md) + diff --git a/package/Documentation~/simulation_results.md b/package/Documentation~/simulation_results.md new file mode 100755 index 000000000..4a22bc02b --- /dev/null +++ b/package/Documentation~/simulation_results.md @@ -0,0 +1,4 @@ + +Please come back later, work in progress. + +[Back to Index](index.md) diff --git a/package/LICENSE.md b/package/LICENSE.md new file mode 100755 index 000000000..4c2154cc1 --- /dev/null +++ b/package/LICENSE.md @@ -0,0 +1,30 @@ +Unity Companion License (“License”) +Software Copyright © 2017 Unity Technologies ApS + +Unity Technologies ApS (“Unity”) grants to you a worldwide, non-exclusive, no-charge, and royalty-free copyright license to reproduce, prepare derivative works of, publicly display, publicly perform, sublicense, and distribute the software that is made available under this License (“Software”), subject to the following terms and conditions: + +1. Unity Companion Use Only. Exercise of the license granted herein is limited to exercise for the creation, use, and/or distribution of applications, software, or other content pursuant to a valid Unity content authoring and rendering engine software license (“Engine License”). That means while use of the Software is not limited to use in the software licensed under the Engine License, the Software may not be used for any purpose other than the creation, use, and/or distribution of Engine License-dependent applications, software, or other content. No other exercise of the license granted herein is permitted, and in no event may the Software be used for competitive analysis or to develop a competing product or service. + +2. No Modification of Engine License. Neither this License nor any exercise of the license granted herein modifies the Engine License in any way. + +3. Ownership & Grant Back to You. + +3.1 You own your content. In this License, “derivative works” means derivatives of the Software itself--works derived only from the Software by you under this License (for example, modifying the code of the Software itself to improve its efficacy); “derivative works” of the Software do not include, for example, games, apps, or content that you create using the Software. You keep all right, title, and interest to your own content. + +3.2 Unity owns its content. While you keep all right, title, and interest to your own content per the above, as between Unity and you, Unity will own all right, title, and interest to all intellectual property rights (including patent, trademark, and copyright) in the Software and derivative works of the Software, and you hereby assign and agree to assign all such rights in those derivative works to Unity. + +3.3 You have a license to those derivative works. Subject to this License, Unity grants to you the same worldwide, non-exclusive, no-charge, and royalty-free copyright license to derivative works of the Software you create as is granted to you for the Software under this License. + +4. Trademarks. You are not granted any right or license under this License to use any trademarks, service marks, trade names, products names, or branding of Unity or its affiliates (“Trademarks”). Descriptive uses of Trademarks are permitted; see, for example, Unity’s Branding Usage Guidelines at https://unity3d.com/public-relations/brand. + +5. Notices & Third-Party Rights. This License, including the copyright notice associated with the Software, must be provided in all substantial portions of the Software and derivative works thereof (or, if that is impracticable, in any other location where such notices are customarily placed). Further, if the Software is accompanied by a Unity “third-party notices” or similar file, you acknowledge and agree that software identified in that file is governed by those separate license terms. + +6. DISCLAIMER, LIMITATION OF LIABILITY. THE SOFTWARE AND ANY DERIVATIVE WORKS THEREOF IS PROVIDED ON AN "AS IS" BASIS, AND IS PROVIDED WITHOUT WARRANTY OF ANY KIND, WHETHER EXPRESS OR IMPLIED, INCLUDING ANY WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE, AND/OR NONINFRINGEMENT. IN NO EVENT SHALL ANY COPYRIGHT HOLDER OR AUTHOR BE LIABLE FOR ANY CLAIM, DAMAGES (WHETHER DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL, INCLUDING PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES, LOSS OF USE, DATA, OR PROFITS, AND BUSINESS INTERRUPTION), OR OTHER LIABILITY WHATSOEVER, WHETHER IN AN ACTION OF CONTRACT, TORT, OR OTHERWISE, ARISING FROM OR OUT OF, OR IN CONNECTION WITH, THE SOFTWARE OR ANY DERIVATIVE WORKS THEREOF OR THE USE OF OR OTHER DEALINGS IN SAME, EVEN WHERE ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +7. USE IS ACCEPTANCE and License Versions. Your receipt and use of the Software constitutes your acceptance of this License and its terms and conditions. Software released by Unity under this License may be modified or updated and the License with it; upon any such modification or update, you will comply with the terms of the updated License for any use of any of the Software under the updated License. + +8. Use in Compliance with Law and Termination. Your exercise of the license granted herein will at all times be in compliance with applicable law and will not infringe any proprietary rights (including intellectual property rights); this License will terminate immediately on any breach by you of this License. + +9. Severability. If any provision of this License is held to be unenforceable or invalid, that provision will be enforced to the maximum extent possible and the other provisions will remain in full force and effect. + +10. Governing Law and Venue. This License is governed by and construed in accordance with the laws of Denmark, except for its conflict of laws rules; the United Nations Convention on Contracts for the International Sale of Goods will not apply. If you reside (or your principal place of business is) within the United States, you and Unity agree to submit to the personal and exclusive jurisdiction of and venue in the state and federal courts located in San Francisco County, California concerning any dispute arising out of this License (“Dispute”). If you reside (or your principal place of business is) outside the United States, you and Unity agree to submit to the personal and exclusive jurisdiction of and venue in the courts located in Copenhagen, Denmark concerning any Dispute. \ No newline at end of file diff --git a/package/LICENSE.md.meta b/package/LICENSE.md.meta new file mode 100755 index 000000000..f8396d4fd --- /dev/null +++ b/package/LICENSE.md.meta @@ -0,0 +1,7 @@ +fileFormatVersion: 2 +guid: 22e65f34478919449b3105ce7bcafd7f +TextScriptImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Tests.meta b/package/Tests.meta new file mode 100755 index 000000000..396c6db22 --- /dev/null +++ b/package/Tests.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: 64b5f8d329245f64690333db0f502f88 +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Tests/EditModeTests.meta b/package/Tests/EditModeTests.meta new file mode 100755 index 000000000..c4eafba8b --- /dev/null +++ b/package/Tests/EditModeTests.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: 4884d8cd334524229a8c541c5fe85a43 +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Tests/EditModeTests/ManipulatorUtility_UnitTests.cs b/package/Tests/EditModeTests/ManipulatorUtility_UnitTests.cs new file mode 100755 index 000000000..4c2c367ef --- /dev/null +++ b/package/Tests/EditModeTests/ManipulatorUtility_UnitTests.cs @@ -0,0 +1,37 @@ +using System.Linq; +using NUnit.Framework; +using Unity.Mathematics; +using Unity.Physics.Editor; +using UnityEngine; + +namespace Unity.Physics.EditModeTests +{ + class ManipulatorUtility_UnitTests + { + static TestCaseData[] k_GetMatrixStateReturnValueTestCases = + { + new TestCaseData(new float4x4()).Returns(MatrixState.NotValidTRS).SetName("Not valid TRS"), + new TestCaseData(new float4x4 { c3 = new float4 { w = 1f } }).Returns(MatrixState.ZeroScale).SetName("Zero scale"), + new TestCaseData(float4x4.Scale(3f)).Returns(MatrixState.UniformScale).SetName("Uniform scale"), + new TestCaseData(float4x4.Scale(3f, 2f, 1f)).Returns(MatrixState.NonUniformScale).SetName("Non-uniform scale") + }; + + [TestCaseSource(nameof(k_GetMatrixStateReturnValueTestCases))] + public MatrixState GetMatrixState_ReturnsExpectedState(float4x4 localToWorld) + { + return ManipulatorUtility.GetMatrixState(ref localToWorld); + } + + static TestCaseData[] k_GetMatrixStatMutateTestCases => k_GetMatrixStateReturnValueTestCases + .Select(testCase => new TestCaseData(testCase.Arguments).SetName($"{testCase.TestName} does not mutate")) + .ToArray(); + + [TestCaseSource(nameof(k_GetMatrixStatMutateTestCases))] + public void GetMatrixState_DoesNotMutateLTWArgument(float4x4 localToWorld) + { + var previous = localToWorld; + ManipulatorUtility.GetMatrixState(ref localToWorld); + Assert.That(localToWorld, Is.EqualTo(previous)); + } + } +} diff --git a/package/Tests/EditModeTests/ManipulatorUtility_UnitTests.cs.meta b/package/Tests/EditModeTests/ManipulatorUtility_UnitTests.cs.meta new file mode 100755 index 000000000..19c47eb22 --- /dev/null +++ b/package/Tests/EditModeTests/ManipulatorUtility_UnitTests.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 610534b2777b8473c8e4e3830f69ef1d +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Tests/EditModeTests/MatrixGUIUtility_UnitTests.cs b/package/Tests/EditModeTests/MatrixGUIUtility_UnitTests.cs new file mode 100755 index 000000000..b1e2ec861 --- /dev/null +++ b/package/Tests/EditModeTests/MatrixGUIUtility_UnitTests.cs @@ -0,0 +1,31 @@ +using System.Collections; +using System.Collections.Generic; +using NUnit.Framework; +using Unity.Physics.Editor; +using UnityEditor; + +namespace Unity.Physics.EditModeTests +{ + class MatrixGUIUtility_UnitTests + { + static IEnumerable k_GetMatrixStatusMessageTestCases = new[] + { + new TestCaseData(new[] { MatrixState.UniformScale, MatrixState.ZeroScale }, "zero").Returns(MessageType.Warning).SetName("At least one zero"), + new TestCaseData(new[] { MatrixState.UniformScale, MatrixState.NonUniformScale }, "(non-uniform|performance)").Returns(MessageType.Warning).SetName("At least one non-uniform"), + new TestCaseData(new[] { MatrixState.UniformScale, MatrixState.NotValidTRS }, "(not |in)valid").Returns(MessageType.Error).SetName("At least one invalid"), + new TestCaseData(new[] { MatrixState.UniformScale, MatrixState.UniformScale }, "^$").Returns(MessageType.None).SetName("All uniform") + }; + + [TestCaseSource(nameof(k_GetMatrixStatusMessageTestCases))] + public MessageType GetMatrixStatusMessage_WithStateCombination_MessageContainsExpectedKeywords( + IReadOnlyList matrixStates, string keywords + ) + { + var result = MatrixGUIUtility.GetMatrixStatusMessage(matrixStates, out var message); + + Assert.That(message, Does.Match(keywords)); + + return result; + } + } +} diff --git a/package/Tests/EditModeTests/MatrixGUIUtility_UnitTests.cs.meta b/package/Tests/EditModeTests/MatrixGUIUtility_UnitTests.cs.meta new file mode 100755 index 000000000..57f79d396 --- /dev/null +++ b/package/Tests/EditModeTests/MatrixGUIUtility_UnitTests.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 3529159326c7c4a94b09f29072dc1995 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Tests/EditModeTests/Unity.Physics.EditModeTests.asmdef b/package/Tests/EditModeTests/Unity.Physics.EditModeTests.asmdef new file mode 100755 index 000000000..157ca9531 --- /dev/null +++ b/package/Tests/EditModeTests/Unity.Physics.EditModeTests.asmdef @@ -0,0 +1,21 @@ +{ + "name": "Unity.Physics.EditModeTests", + "references": [ + "Unity.Mathematics", + "Unity.Physics", + "Unity.Physics.Editor" + ], + "optionalUnityReferences": [ + "TestAssemblies" + ], + "includePlatforms": [ + "Editor" + ], + "excludePlatforms": [], + "allowUnsafeCode": true, + "overrideReferences": false, + "precompiledReferences": [], + "autoReferenced": true, + "defineConstraints": [], + "versionDefines": [] +} diff --git a/package/Tests/EditModeTests/Unity.Physics.EditModeTests.asmdef.meta b/package/Tests/EditModeTests/Unity.Physics.EditModeTests.asmdef.meta new file mode 100755 index 000000000..77a931840 --- /dev/null +++ b/package/Tests/EditModeTests/Unity.Physics.EditModeTests.asmdef.meta @@ -0,0 +1,7 @@ +fileFormatVersion: 2 +guid: be6a71f25de594eeaa9961cb67e64fa1 +AssemblyDefinitionImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Tests/PlayModeTests.meta b/package/Tests/PlayModeTests.meta new file mode 100755 index 000000000..030ddc542 --- /dev/null +++ b/package/Tests/PlayModeTests.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: c53e73c2808324b60a9893d1dce625f3 +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Tests/PlayModeTests/Authoring.meta b/package/Tests/PlayModeTests/Authoring.meta new file mode 100755 index 000000000..f2dc3703d --- /dev/null +++ b/package/Tests/PlayModeTests/Authoring.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: 195de18527e9d42cbb007b03402d4e1e +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Tests/PlayModeTests/Authoring/BaseHierarchyConversionTest.cs b/package/Tests/PlayModeTests/Authoring/BaseHierarchyConversionTest.cs new file mode 100755 index 000000000..26b94e885 --- /dev/null +++ b/package/Tests/PlayModeTests/Authoring/BaseHierarchyConversionTest.cs @@ -0,0 +1,35 @@ +using System; +using NUnit.Framework; +using UnityEngine; + +namespace Unity.Physics.Tests.Authoring +{ + abstract class BaseHierarchyConversionTest + { + protected void CreateHierarchy( + Type[] rootComponentTypes, Type[] parentComponentTypes, Type[] childComponentTypes + ) + { + Root = new GameObject("Root", rootComponentTypes); + Parent = new GameObject("Parent", parentComponentTypes); + Child = new GameObject("Child", childComponentTypes); + Child.transform.parent = Parent.transform; + Parent.transform.parent = Root.transform; + } + + protected GameObject Root { get; private set; } + protected GameObject Parent { get; private set; } + protected GameObject Child { get; private set; } + + [TearDown] + public void TearDown() + { + if (Child != null) + GameObject.DestroyImmediate(Child); + if (Parent != null) + GameObject.DestroyImmediate(Parent); + if (Root != null) + GameObject.DestroyImmediate(Root); + } + } +} diff --git a/package/Tests/PlayModeTests/Authoring/BaseHierarchyConversionTest.cs.meta b/package/Tests/PlayModeTests/Authoring/BaseHierarchyConversionTest.cs.meta new file mode 100755 index 000000000..11bcc2095 --- /dev/null +++ b/package/Tests/PlayModeTests/Authoring/BaseHierarchyConversionTest.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 803804f9bc33b47d4923ccb0c34b9b15 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Tests/PlayModeTests/Authoring/BodyAndShapeConversionSystems_IntegrationTests.cs b/package/Tests/PlayModeTests/Authoring/BodyAndShapeConversionSystems_IntegrationTests.cs new file mode 100755 index 000000000..f6d0654da --- /dev/null +++ b/package/Tests/PlayModeTests/Authoring/BodyAndShapeConversionSystems_IntegrationTests.cs @@ -0,0 +1,140 @@ +using System; +using NUnit.Framework; +using Unity.Collections; +using Unity.Entities; +using Unity.Physics.Authoring; +using UnityEngine; + +namespace Unity.Physics.Tests.Authoring +{ + class BodyAndShapeConversionSystems_IntegrationTests : BaseHierarchyConversionTest + { + [Test] + public void ConversionSystems_WhenGOHasPhysicsBodyAndRigidbody_EntityUsesPhysicsBodyMass() + { + CreateHierarchy(new[] { typeof(Rigidbody), typeof(PhysicsBody) }, Array.Empty(), Array.Empty()); + Root.GetComponent().Mass = 100f; + Root.GetComponent().mass = 50f; + + TestConvertedData(mass => Assert.That(mass.InverseMass, Is.EqualTo(0.01f))); + } + + [Test] + public void ConversionSystems_WhenGOHasPhysicsBodyAndRigidbody_EntityUsesPhysicsBodyDamping() + { + CreateHierarchy(new[] { typeof(Rigidbody), typeof(PhysicsBody) }, Array.Empty(), Array.Empty()); + Root.GetComponent().LinearDamping = 1f; + Root.GetComponent().drag = 0.5f; + + TestConvertedData(damping => Assert.That(damping.Linear, Is.EqualTo(1f))); + } + + [Test] + public void ConversionSystems_WhenGOHasDynamicPhysicsBodyWithCustomGravity_AndKinematicRigidbody_EntityUsesPhysicsBodyGravityFactor() + { + CreateHierarchy(new[] { typeof(Rigidbody), typeof(PhysicsBody) }, Array.Empty(), Array.Empty()); + Root.GetComponent().MotionType = BodyMotionType.Dynamic; + Root.GetComponent().GravityFactor = 2f; + Root.GetComponent().isKinematic = true; + + TestConvertedData(gravity => Assert.That(gravity.Value, Is.EqualTo(2f))); + } + + [Test] + public void ConversionSystems_WhenGOHasKinematicPhysicsBody_AndDynamicRigidbody_EntityUsesPhysicsBodyGravityFactor() + { + CreateHierarchy(new[] { typeof(Rigidbody), typeof(PhysicsBody) }, Array.Empty(), Array.Empty()); + Root.GetComponent().MotionType = BodyMotionType.Kinematic; + Root.GetComponent().isKinematic = false; + + TestConvertedData(gravity => Assert.That(gravity.Value, Is.EqualTo(0f))); + } + + [Test] + public void ConversionSystems_WhenGOHasDynamicPhysicsBodyWithDefaultGravity_AndDynamicRigidbodyWithCustomGravity_EntityHasNoGravityFactor() + { + CreateHierarchy(new[] { typeof(Rigidbody), typeof(PhysicsBody) }, Array.Empty(), Array.Empty()); + Root.GetComponent().MotionType = BodyMotionType.Dynamic; + Root.GetComponent().isKinematic = true; + + VerifyNoDataProduced(); + } + + [Test] + public void ConversionSystems_WhenGOHasDynamicPhysicsBody_AndKinematicRigidbody_EntityUsesPhysicsBodyMass() + { + CreateHierarchy(new[] { typeof(Rigidbody), typeof(PhysicsBody) }, Array.Empty(), Array.Empty()); + Root.GetComponent().MotionType = BodyMotionType.Dynamic; + Root.GetComponent().Mass = 100f; + Root.GetComponent().isKinematic = true; + Root.GetComponent().mass = 50f; + + TestConvertedData(mass => Assert.That(mass.InverseMass, Is.EqualTo(0.01f))); + } + + [Test] + public void ConversionSystems_WhenGOHasKinematicPhysicsBody_AndDynamicRigidbody_EntityUsesPhysicsBodyMass() + { + CreateHierarchy(new[] { typeof(Rigidbody), typeof(PhysicsBody) }, Array.Empty(), Array.Empty()); + Root.GetComponent().MotionType = BodyMotionType.Kinematic; + Root.GetComponent().Mass = 100f; + Root.GetComponent().isKinematic = false; + Root.GetComponent().mass = 50f; + + TestConvertedData(mass => Assert.That(mass.InverseMass, Is.EqualTo(0f))); + } + + [Test] + public void ConversionSystems_WhenGOHasStaticPhysicsBody_AndDynamicRigidbody_EntityHasNoGravityFactor() + { + CreateHierarchy(new[] { typeof(Rigidbody), typeof(PhysicsBody) }, Array.Empty(), Array.Empty()); + Root.GetComponent().MotionType = BodyMotionType.Static; + Root.GetComponent().isKinematic = false; + + VerifyNoDataProduced(); + } + + void TestConvertedData(Action checkValue) where T : struct, IComponentData + { + var world = new World("Test world"); + + try + { + GameObjectConversionUtility.ConvertGameObjectHierarchy(Root, world); + + using (var group = world.EntityManager.CreateComponentGroup(typeof(T))) + { + using (var bodies = group.ToComponentDataArray(Allocator.Persistent)) + { + Assume.That(bodies, Has.Length.EqualTo(1)); + var componentData = bodies[0]; + + checkValue(componentData); + } + } + } + finally + { + world.Dispose(); + } + } + + void VerifyNoDataProduced() where T : struct, IComponentData + { + var world = new World("Test world"); + + try + { + GameObjectConversionUtility.ConvertGameObjectHierarchy(Root, world); + + using (var group = world.EntityManager.CreateComponentGroup(typeof(T))) + using (var bodies = group.ToComponentDataArray(Allocator.Persistent)) + Assert.That(bodies.Length, Is.EqualTo(0), $"Conversion pipeline produced {typeof(T).Name}"); + } + finally + { + world.Dispose(); + } + } + } +} diff --git a/package/Tests/PlayModeTests/Authoring/BodyAndShapeConversionSystems_IntegrationTests.cs.meta b/package/Tests/PlayModeTests/Authoring/BodyAndShapeConversionSystems_IntegrationTests.cs.meta new file mode 100755 index 000000000..2a75e3d0a --- /dev/null +++ b/package/Tests/PlayModeTests/Authoring/BodyAndShapeConversionSystems_IntegrationTests.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 2e59eae1083dc4a5eac34e5f81c99a19 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Tests/PlayModeTests/Authoring/PhysicsShapeConversionSystem_IntegrationTests.cs b/package/Tests/PlayModeTests/Authoring/PhysicsShapeConversionSystem_IntegrationTests.cs new file mode 100755 index 000000000..e3d351d62 --- /dev/null +++ b/package/Tests/PlayModeTests/Authoring/PhysicsShapeConversionSystem_IntegrationTests.cs @@ -0,0 +1,141 @@ +using System; +using System.Linq; +using NUnit.Framework; +using Unity.Collections; +using Unity.Entities; +using Unity.Mathematics; +using Unity.Physics.Authoring; +using UnityEngine; + +namespace Unity.Physics.Tests.Authoring +{ + class PhysicsShapeConversionSystem_IntegrationTests : BaseHierarchyConversionTest + { + [Test] + public void PhysicsShapeConversionSystem_WhenBodyHasOneSiblingShape_CreatesPrimitive() + { + CreateHierarchy( + new[] { typeof(ConvertToEntity), typeof(PhysicsBody), typeof(PhysicsShape) }, + new[] { typeof(ConvertToEntity) }, + new[] { typeof(ConvertToEntity) } + ); + Root.GetComponent().SetBox(float3.zero, new float3(1,1,1), quaternion.identity); + + var world = new World("Test world"); + GameObjectConversionUtility.ConvertGameObjectHierarchy(Root, world); + using (var group = world.EntityManager.CreateComponentGroup(typeof(PhysicsCollider))) + { + using (var colliders = group.ToComponentDataArray(Allocator.Persistent)) + { + Assume.That(colliders, Has.Length.EqualTo(1)); + var collider = colliders[0].Value; + + Assert.That(collider.Value.Type, Is.EqualTo(ColliderType.Box)); + } + } + } + + [Test] + public void PhysicsShapeConversionSystem_WhenBodyHasOneDescendentShape_CreatesCompound() + { + CreateHierarchy( + new[] { typeof(ConvertToEntity), typeof(PhysicsBody) }, + new[] { typeof(ConvertToEntity), typeof(PhysicsShape) }, + new[] { typeof(ConvertToEntity) } + ); + Parent.GetComponent().SetBox(float3.zero, new float3(1, 1, 1), quaternion.identity); + + var world = new World("Test world"); + GameObjectConversionUtility.ConvertGameObjectHierarchy(Root, world); + using (var group = world.EntityManager.CreateComponentGroup(typeof(PhysicsCollider))) + { + using (var colliders = group.ToComponentDataArray(Allocator.Persistent)) + { + Assume.That(colliders, Has.Length.EqualTo(1)); + var collider = colliders[0].Value; + + Assert.That(collider.Value.Type, Is.EqualTo(ColliderType.Compound)); + unsafe + { + var compoundCollider = (CompoundCollider*)(collider.GetUnsafePtr()); + Assert.That(compoundCollider->Children, Has.Length.EqualTo(1)); + Assert.That(compoundCollider->Children[0].Collider->Type, Is.EqualTo(ColliderType.Box)); + } + } + } + } + + [Test] + public void PhysicsShapeConversionSystem_WhenBodyHasMultipleDescendentShapes_CreatesCompound() + { + CreateHierarchy( + new[] { typeof(ConvertToEntity), typeof(PhysicsBody) }, + new[] { typeof(ConvertToEntity), typeof(PhysicsShape) }, + new[] { typeof(ConvertToEntity), typeof(PhysicsShape) } + ); + Parent.GetComponent().SetBox(float3.zero, new float3(1, 1, 1), quaternion.identity); + Child.GetComponent().SetSphere(float3.zero, 1.0f, quaternion.identity); + + var world = new World("Test world"); + GameObjectConversionUtility.ConvertGameObjectHierarchy(Root, world); + using (var group = world.EntityManager.CreateComponentGroup(typeof(PhysicsCollider))) + { + using (var colliders = group.ToComponentDataArray(Allocator.Persistent)) + { + Assume.That(colliders, Has.Length.EqualTo(1)); + var collider = colliders[0].Value; + + Assert.That(collider.Value.Type, Is.EqualTo(ColliderType.Compound)); + unsafe + { + var compoundCollider = (CompoundCollider*)(collider.GetUnsafePtr()); + + var childTypes = Enumerable.Range(0, compoundCollider->NumChildren) + .Select(i => compoundCollider->Children[i].Collider->Type) + .ToArray(); + Assert.That(childTypes, Is.EquivalentTo(new[] { ColliderType.Box, ColliderType.Sphere })); + } + } + } + } + + [Ignore("GameObjectConversionUtility does not yet support multiples of the same component type.")] + [TestCase(2)] + [TestCase(5)] + [TestCase(10)] + public void PhysicsShapeConversionSystem_WhenBodyHasMultipleSiblingShapes_CreatesCompound(int shapeCount) + { + CreateHierarchy( + new[] { typeof(ConvertToEntity), typeof(PhysicsBody) }, + new[] { typeof(ConvertToEntity) }, + new[] { typeof(ConvertToEntity) } + ); + for (int i = 0; i < shapeCount; ++i) + { + Root.AddComponent().SetBox(float3.zero, new float3(1, 1, 1), quaternion.identity); + } + + var world = new World("Test world"); + GameObjectConversionUtility.ConvertGameObjectHierarchy(Root, world); + using (var group = world.EntityManager.CreateComponentGroup(typeof(PhysicsCollider))) + { + using (var colliders = group.ToComponentDataArray(Allocator.Persistent)) + { + Assume.That(colliders, Has.Length.EqualTo(1)); + var collider = colliders[0].Value; + + Assert.That(collider.Value.Type, Is.EqualTo(ColliderType.Compound)); + unsafe + { + var compoundCollider = (CompoundCollider*)(collider.GetUnsafePtr()); + Assert.That(compoundCollider->Children, Has.Length.EqualTo(shapeCount)); + for (int i = 0; i < compoundCollider->Children.Length; i++) + { + Assert.That(compoundCollider->Children[i].Collider->Type, Is.EqualTo(ColliderType.Box)); + } + } + } + } + } + } +} diff --git a/package/Tests/PlayModeTests/Authoring/PhysicsShapeConversionSystem_IntegrationTests.cs.meta b/package/Tests/PlayModeTests/Authoring/PhysicsShapeConversionSystem_IntegrationTests.cs.meta new file mode 100755 index 000000000..9edcde50f --- /dev/null +++ b/package/Tests/PlayModeTests/Authoring/PhysicsShapeConversionSystem_IntegrationTests.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 59ccccbad70c1fb42966e9e86e6b8a10 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Tests/PlayModeTests/Authoring/PhysicsShapeExtensions_IntegrationTests.cs b/package/Tests/PlayModeTests/Authoring/PhysicsShapeExtensions_IntegrationTests.cs new file mode 100755 index 000000000..599c1b778 --- /dev/null +++ b/package/Tests/PlayModeTests/Authoring/PhysicsShapeExtensions_IntegrationTests.cs @@ -0,0 +1,37 @@ +using System; +using NUnit.Framework; +using Unity.Physics.Authoring; +using UnityEngine; + +namespace Unity.Physics.Tests.Authoring +{ + class PhysicsShapeExtensions_IntegrationTests : BaseHierarchyConversionTest + { + [Test] + public void GetPrimaryBody_WhenHierarchyContainsMultipleBodies_ReturnsFirstParent( + [Values(typeof(Rigidbody), typeof(PhysicsBody))]Type rootBodyType, + [Values(typeof(Rigidbody), typeof(PhysicsBody))]Type parentBodyType + ) + { + CreateHierarchy(new[] { rootBodyType }, new[] { parentBodyType }, Array.Empty()); + + var primaryBody = PhysicsShapeExtensions.GetPrimaryBody(Child); + + Assert.That(primaryBody, Is.EqualTo(Parent)); + } + + [Test] + public void GetPrimaryBody_WhenHierarchyContainsNoBodies_ReturnsTopMostShape( + [Values(typeof(UnityEngine.BoxCollider), typeof(PhysicsShape))]Type rootShapeType, + [Values(typeof(UnityEngine.BoxCollider), typeof(PhysicsShape))]Type parentShapeType, + [Values(typeof(UnityEngine.BoxCollider), typeof(PhysicsShape))]Type childShapeType + ) + { + CreateHierarchy(new[] { rootShapeType }, new[] { parentShapeType }, new[] { childShapeType }); + + var primaryBody = PhysicsShapeExtensions.GetPrimaryBody(Child); + + Assert.That(primaryBody, Is.EqualTo(Root)); + } + } +} diff --git a/package/Tests/PlayModeTests/Authoring/PhysicsShapeExtensions_IntegrationTests.cs.meta b/package/Tests/PlayModeTests/Authoring/PhysicsShapeExtensions_IntegrationTests.cs.meta new file mode 100755 index 000000000..84d945958 --- /dev/null +++ b/package/Tests/PlayModeTests/Authoring/PhysicsShapeExtensions_IntegrationTests.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: d6b20bd7ca38a4a21899323bfbf13ada +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Tests/PlayModeTests/Authoring/PhysicsShapeExtensions_UnitTests.cs b/package/Tests/PlayModeTests/Authoring/PhysicsShapeExtensions_UnitTests.cs new file mode 100755 index 000000000..c443d3d27 --- /dev/null +++ b/package/Tests/PlayModeTests/Authoring/PhysicsShapeExtensions_UnitTests.cs @@ -0,0 +1,42 @@ +using System; +using NUnit.Framework; +using Unity.Mathematics; +using Unity.Physics.Authoring; + +namespace Unity.Physics.Tests.Authoring +{ + class PhysicsShapeExtensions_UnitTests + { + static readonly TestCaseData[] k_DeviantAxisTestCases = + { + new TestCaseData(new float3( 0f, 1f, 1f)).Returns(0).SetName("Smallest axis, other axes identical"), + new TestCaseData(new float3( 1f, 2f, 1f)).Returns(1).SetName("Largest axis, other axes identical"), + new TestCaseData(new float3( 5f, 8f, 1f)).Returns(2).SetName("Smallest axis, other axes differ"), + new TestCaseData(new float3( 9f, 2f, 3f)).Returns(0).SetName("Largest axis, other axes differ"), + new TestCaseData(new float3(-1f, -1f, 1f)).Returns(2).SetName("Only positive axis, other axes identical"), + new TestCaseData(new float3( 1f, -2f, 1f)).Returns(1).SetName("Only negative axis, other axes identical") + }; + + [TestCaseSource(nameof(k_DeviantAxisTestCases))] + public int GetDeviantAxis_ReturnsTheMostDifferentAxis(float3 v) + { + return PhysicsShapeExtensions.GetDeviantAxis(v); + } + + static readonly TestCaseData[] k_MaxAxisTestCases = + { + new TestCaseData(new float3( 3f, 2f, 1f)).Returns(0).SetName("X-axis (all positive)"), + new TestCaseData(new float3( 3f, 2f, -4f)).Returns(0).SetName("X-axis (one negative with greater magnitude)"), + new TestCaseData(new float3( 2f, 3f, 1f)).Returns(1).SetName("Y-axis (all positive)"), + new TestCaseData(new float3(-4f, 3f, 2f)).Returns(1).SetName("Y-axis (one negative with greater magnitude)"), + new TestCaseData(new float3( 1f, 2f, 3f)).Returns(2).SetName("Z-axis (all positive)"), + new TestCaseData(new float3( 2f, -4f, 3f)).Returns(2).SetName("Z-axis (one negative with greater magnitude)"), + }; + + [TestCaseSource(nameof(k_MaxAxisTestCases))] + public int GetMaxAxis_ReturnsLargestAxis(float3 v) + { + return PhysicsShapeExtensions.GetMaxAxis(v); + } + } +} \ No newline at end of file diff --git a/package/Tests/PlayModeTests/Authoring/PhysicsShapeExtensions_UnitTests.cs.meta b/package/Tests/PlayModeTests/Authoring/PhysicsShapeExtensions_UnitTests.cs.meta new file mode 100755 index 000000000..978bd3de6 --- /dev/null +++ b/package/Tests/PlayModeTests/Authoring/PhysicsShapeExtensions_UnitTests.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: a8078037ba4934db0b8515b7d63e30c5 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Tests/PlayModeTests/Authoring/PhysicsShape_UnitTests.cs b/package/Tests/PlayModeTests/Authoring/PhysicsShape_UnitTests.cs new file mode 100755 index 000000000..7194c109e --- /dev/null +++ b/package/Tests/PlayModeTests/Authoring/PhysicsShape_UnitTests.cs @@ -0,0 +1,178 @@ +using NUnit.Framework; +using Unity.Mathematics; +using Unity.Physics.Authoring; +using UnityEngine; + +namespace Unity.Physics.Tests.Authoring +{ + class PhysicsShape_UnitTests + { + const float k_Tolerance = 0.001f; + + PhysicsShape m_Shape; + + [SetUp] + public void SetUp() => m_Shape = new GameObject("Shape").AddComponent(); + + [TearDown] + public void TearDown() + { + if (m_Shape != null) + GameObject.DestroyImmediate(m_Shape.gameObject); + } + + [Test] + public void SetBoxProperties_WithSizeLessThanZero_ClampsToZero() + { + m_Shape.SetBox(0f, -3f, quaternion.identity); + + m_Shape.GetBoxProperties(out var center, out var size, out quaternion orientation); + + Assert.That(size, Is.EqualTo(new float3(0f))); + } + + [Test] + public void GetCapsuleProperties_WhenShapeIsBox_HeightIsMaxDimension( + [Values(0f, 1f, 2f, 3f)]float sizeX, + [Values(0f, 1f, 2f, 3f)]float sizeY, + [Values(0f, 1f, 2f, 3f)]float sizeZ + ) + { + var size = new float3(sizeX, sizeY, sizeZ); + m_Shape.SetBox(0f, size, quaternion.identity); + + m_Shape.GetCapsuleProperties(out var center, out var height, out var radius, out quaternion orientation); + + Assert.That(height, Is.EqualTo(math.cmax(size))); + } + + [Test] + public void GetCapsuleProperties_WhenShapeIsBox_RadiusIsHalfSecondMaxDimension( + [Values(0f, 1f, 2f, 3f)]float sizeX, + [Values(0f, 1f, 2f, 3f)]float sizeY, + [Values(0f, 1f, 2f, 3f)]float sizeZ + ) + { + var size = new float3(sizeX, sizeY, sizeZ); + m_Shape.SetBox(0f, size, quaternion.identity); + + m_Shape.GetCapsuleProperties(out var center, out var height, out var radius, out quaternion orientation); + + var cmaxI = size.GetMaxAxis(); + var expectedRadius = 0.5f * math.cmax(cmaxI == 0 ? size.yz : cmaxI == 1 ? size.xz : size.xy); + Assert.That(radius, Is.EqualTo(expectedRadius)); + } + + static readonly TestCaseData[] k_CapsuleOrientationTestCases = + { + new TestCaseData(new float3(2f, 1f, 1f), new float3(1f, 0f, 0f)).SetName("Aligned to x-axis"), + new TestCaseData(new float3(1f, 2f, 1f), new float3(0f, 1f, 0f)).SetName("Aligned to y-axis"), + new TestCaseData(new float3(1f, 1f, 2f), new float3(0f, 0f, 1f)).SetName("Aligned to z-axis") + }; + [TestCaseSource(nameof(k_CapsuleOrientationTestCases))] + public void GetCapsuleProperties_WhenShapeIsElongatedBox_OrientationPointsDownLongAxis(float3 boxSize, float3 expectedLookVector) + { + m_Shape.SetBox(0f, boxSize, quaternion.identity); + + m_Shape.GetCapsuleProperties(out var center, out var height, out var radius, out quaternion orientation); + + var lookVector = math.mul(orientation, new float3(0f, 0f, 1f)); + Assert.That( + math.dot(lookVector, expectedLookVector), Is.EqualTo(1f).Within(k_Tolerance), + $"Expected {expectedLookVector} but got {lookVector}" + ); + } + + [Test] + public void GetCylinderProperties_WhenShapeIsBox_HeightIsDeviantDimension( + [Values(0f, 1f, 2f, 3f)]float sizeX, + [Values(0f, 1f, 2f, 3f)]float sizeY, + [Values(0f, 1f, 2f, 3f)]float sizeZ + ) + { + var size = new float3(sizeX, sizeY, sizeZ); + m_Shape.SetBox(0f, size, quaternion.identity); + + m_Shape.GetCylinderProperties(out var center, out var height, out var radius, out quaternion orientation); + + var heightAxis = size.GetDeviantAxis(); + Assert.That(height, Is.EqualTo(size[heightAxis])); + } + + [Test] + public void GetCylinderProperties_WhenShapeIsBox_RadiusIsHalfMaxHomogenousDimension( + [Values(0f, 1f, 2f, 3f)]float sizeX, + [Values(0f, 1f, 2f, 3f)]float sizeY, + [Values(0f, 1f, 2f, 3f)]float sizeZ + ) + { + var size = new float3(sizeX, sizeY, sizeZ); + m_Shape.SetBox(0f, size, quaternion.identity); + + m_Shape.GetCylinderProperties(out var center, out var height, out var radius, out quaternion orientation); + + var heightAxis = size.GetDeviantAxis(); + var expectedRadius = 0.5f * math.cmax(heightAxis == 0 ? size.yz : heightAxis == 1 ? size.xz : size.xy); + Assert.That(radius, Is.EqualTo(expectedRadius)); + } + + [Test] + public void GetSphereProperties_WhenShapeIsBox_RadiusIsHalfMaxDimension( + [Values(0f, 1f, 2f, 3f)] float sizeX, + [Values(0f, 1f, 2f, 3f)] float sizeY, + [Values(0f, 1f, 2f, 3f)] float sizeZ + ) + { + var size = new float3(sizeX, sizeY, sizeZ); + m_Shape.SetBox(0f, size, quaternion.identity); + + m_Shape.GetSphereProperties(out var center, out var radius, out quaternion orientation); + + var expectedRadius = 0.5f * math.cmax(size); + Assert.That(radius, Is.EqualTo(expectedRadius)); + } + + static readonly TestCaseData[] k_PlaneSizeTestCases = + { + new TestCaseData(new float3(2f, 3f, 1f), 0, 1).SetName("xy"), + new TestCaseData(new float3(2f, 1f, 3f), 0, 2).SetName("xz"), + new TestCaseData(new float3(1f, 2f, 3f), 1, 2).SetName("yz") + }; + + [TestCaseSource(nameof(k_PlaneSizeTestCases))] + public void GetPlaneProperties_WhenShapeIsBox_SizeIsTwoGreatestDimensions(float3 boxSize, int ax1, int ax2) + { + m_Shape.SetBox(0f, boxSize, quaternion.identity); + + m_Shape.GetPlaneProperties(out var center, out var size, out quaternion orientation); + + Assert.That( + new[] { size.x, size.y }, Is.EquivalentTo(new[] { boxSize[ax1], boxSize[ax2] }), + "Plane dimensions did not match two greatest dimensions of original box" + ); + } + + static readonly TestCaseData[] k_PlaneOrientationTestCases = + { + new TestCaseData(new float3(3f, 2f, 1f), quaternion.LookRotation(new float3(1f, 0f, 0f), new float3(0f, 0f, 1f))).SetName("look x, up z"), + new TestCaseData(new float3(2f, 3f, 1f), quaternion.LookRotation(new float3(0f, 1f, 0f), new float3(0f, 0f, 1f))).SetName("look y, up z"), + new TestCaseData(new float3(3f, 1f, 2f), quaternion.LookRotation(new float3(1f, 0f, 0f), new float3(0f, 1f, 0f))).SetName("look x, up y"), + new TestCaseData(new float3(2f, 1f, 3f), quaternion.LookRotation(new float3(0f, 0f, 1f), new float3(0f, 1f, 0f))).SetName("look z, up y"), + new TestCaseData(new float3(1f, 3f, 2f), quaternion.LookRotation(new float3(0f, 1f, 0f), new float3(1f, 0f, 0f))).SetName("look y, up x"), + new TestCaseData(new float3(1f, 2f, 3f), quaternion.LookRotation(new float3(0f, 0f, 1f), new float3(1f, 0f, 0f))).SetName("look z, up x") + }; + + [TestCaseSource(nameof(k_PlaneOrientationTestCases))] + public void GetPlaneProperties_WhenShapeIsBox_OrientationPointsDownLongAxisUpFlatAxis(float3 boxSize, quaternion expected) + { + m_Shape.SetBox(0f, boxSize, quaternion.identity); + + m_Shape.GetPlaneProperties(out var center, out var size, out quaternion orientation); + + Assert.That( + math.abs(math.dot(orientation, expected)), Is.EqualTo(1f).Within(k_Tolerance), + $"Expected {expected} but got {orientation}" + ); + } + } +} diff --git a/package/Tests/PlayModeTests/Authoring/PhysicsShape_UnitTests.cs.meta b/package/Tests/PlayModeTests/Authoring/PhysicsShape_UnitTests.cs.meta new file mode 100755 index 000000000..259154940 --- /dev/null +++ b/package/Tests/PlayModeTests/Authoring/PhysicsShape_UnitTests.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 454cdfc1184724740bd85831648fa4af +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Tests/PlayModeTests/Base.meta b/package/Tests/PlayModeTests/Base.meta new file mode 100755 index 000000000..778d178e6 --- /dev/null +++ b/package/Tests/PlayModeTests/Base.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: 558bb9a5a031b2d49823070f7b1956f6 +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Tests/PlayModeTests/Base/Containers.meta b/package/Tests/PlayModeTests/Base/Containers.meta new file mode 100755 index 000000000..a2d962a7c --- /dev/null +++ b/package/Tests/PlayModeTests/Base/Containers.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: e9ba521f81b6bdb449f9d97bb0bc469a +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Tests/PlayModeTests/Base/Containers/BlockStreamTests.cs b/package/Tests/PlayModeTests/Base/Containers/BlockStreamTests.cs new file mode 100755 index 000000000..1d719b930 --- /dev/null +++ b/package/Tests/PlayModeTests/Base/Containers/BlockStreamTests.cs @@ -0,0 +1,173 @@ +using System; +using NUnit.Framework; +using Unity.Collections; +using Unity.Jobs; +using Unity.Jobs.LowLevel.Unsafe; +using UnityEngine; + +namespace Unity.Physics.Tests.Base.Containers +{ + public class BlockStreamTests + { + struct WriteInts : IJobParallelFor + { + public BlockStream.Writer Writer; + + public void Execute(int index) + { + Writer.BeginForEachIndex(index); + for (int i = 0; i != index; i++) + Writer.Write(i); + Writer.EndForEachIndex(); + } + } + + struct ReadInts : IJobParallelFor + { + public BlockStream.Reader Reader; + + public void Execute(int index) + { + int count = Reader.BeginForEachIndex(index); + Assert.AreEqual(count, index); + + for (int i = 0; i != index; i++) + { + Assert.AreEqual(index - i, Reader.RemainingItemCount); + var peekedValue = Reader.Peek(); + var value = Reader.Read(); + Assert.AreEqual(i, value); + Assert.AreEqual(i, peekedValue); + } + } + } + + [Test] + public void CreateAndDestroy([Values(1, 100, 200)] int count) + { + var stream = new BlockStream(count, 0x9b98651b); + + Assert.IsTrue(stream.IsCreated); + Assert.IsTrue(stream.ForEachCount == count); + Assert.IsTrue(stream.ComputeItemCount() == 0); + + stream.Dispose(); + Assert.IsFalse(stream.IsCreated); + } + + [Test] + public void PopulateInts([Values(1, 100, 200)] int count, [Values(1, 3, 10)] int batchSize) + { + var stream = new BlockStream(count, 0x9b98651c); + var fillInts = new WriteInts { Writer = stream }; + fillInts.Schedule(count, batchSize).Complete(); + + var compareInts = new ReadInts { Reader = stream }; + var res0 = compareInts.Schedule(count, batchSize); + var res1 = compareInts.Schedule(count, batchSize); + + res0.Complete(); + res1.Complete(); + + stream.Dispose(); + } + + // These tests are only valid if BLOCK_STREAM_DEBUG is defined in BlockStream.cs +#if BLOCK_STREAM_DEBUG + + [Test] + public void OutOfBoundsWrite() + { + var stream = new BlockStream(1); + BlockStream.Writer writer = stream; + Assert.Throws(() => writer.BeginForEachIndex(-1)); + Assert.Throws(() => writer.BeginForEachIndex(2)); + + stream.Dispose(); + } + + [Test] + public void IncorrectTypedReads() + { + var stream = new BlockStream(1); + BlockStream.Writer writer = stream; + writer.BeginForEachIndex(0); + writer.Write(5); + writer.EndForEachIndex(); + + BlockStream.Reader reader = stream; + + reader.BeginForEachIndex(0); + Assert.Throws(() => reader.Read()); + + stream.Dispose(); + } +#endif + + [Test] + public void ItemCount([Values(1, 100, 200)] int count, [Values(1, 3, 10)] int batchSize) + { + var stream = new BlockStream(count, 0xd3e8afdd); + var fillInts = new WriteInts { Writer = stream }; + fillInts.Schedule(count, batchSize).Complete(); + + Assert.AreEqual(count * (count - 1) / 2, stream.ComputeItemCount()); + + stream.Dispose(); + } + + [Test] + public void ToArray([Values(1, 100, 200)] int count, [Values(1, 3, 10)] int batchSize) + { + var stream = new BlockStream(count, 0x11843789); + var fillInts = new WriteInts { Writer = stream }; + fillInts.Schedule(count, batchSize).Complete(); + + var array = stream.ToNativeArray(); + int itemIndex = 0; + + for (int i = 0; i != count; ++i) + { + for (int j = 0; j < i; ++j) + { + Assert.AreEqual(j, array[itemIndex]); + itemIndex++; + } + } + + array.Dispose(); + + stream.Dispose(); + } + + [Test] + public void DisposeAfterSchedule() + { + var stream = new BlockStream(100, 0xd3e8afdd); + var fillInts = new WriteInts { Writer = stream }; + var writerJob = fillInts.Schedule(100, 16); + + var disposeJob = stream.ScheduleDispose(writerJob); + + Assert.IsFalse(stream.IsCreated); + + disposeJob.Complete(); + } + +#if ENABLE_UNITY_COLLECTIONS_CHECKS + [Test] + public void ParallelWriteThrows() + { + var stream = new BlockStream(100, 0xd3e8afdd); + var fillInts = new WriteInts { Writer = stream }; + + var writerJob = fillInts.Schedule(100, 16); + Assert.Throws(() => fillInts.Schedule(100, 16) ); + + writerJob.Complete(); + stream.Dispose(); + } +#endif + } +} + diff --git a/package/Tests/PlayModeTests/Base/Containers/BlockStreamTests.cs.meta b/package/Tests/PlayModeTests/Base/Containers/BlockStreamTests.cs.meta new file mode 100755 index 000000000..4f85c9ca5 --- /dev/null +++ b/package/Tests/PlayModeTests/Base/Containers/BlockStreamTests.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 72af5d5074e59b14a8b3a8fb5cc1d44d +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Tests/PlayModeTests/Base/Containers/ElementPoolTests.cs b/package/Tests/PlayModeTests/Base/Containers/ElementPoolTests.cs new file mode 100755 index 000000000..de0b6f900 --- /dev/null +++ b/package/Tests/PlayModeTests/Base/Containers/ElementPoolTests.cs @@ -0,0 +1,127 @@ +using System.Runtime.InteropServices; +using NUnit.Framework; +using Unity.Collections; +using Unity.Collections.LowLevel.Unsafe; +using Unity.Jobs; +using Unity.Jobs.LowLevel.Unsafe; +using UnityEngine; +using Assert = UnityEngine.Assertions.Assert; + +namespace Unity.Physics.Tests.Base.Containers +{ + public class ElementPoolTests + { + // must be blittable, so we cannot use bool as member, + //see https://docs.microsoft.com/en-us/dotnet/framework/interop/blittable-and-non-blittable-types + public struct PoolTestElement : IPoolElement + { + private int _allocated; + + public int TestIndex { get; set; } + + public bool IsAllocated + { + get => _allocated == 0; + set => _allocated = value ? 1 : 0; + } + + void IPoolElement.MarkFree(int nextFree) + { + IsAllocated = false; + NextFree = nextFree; + } + + public int NextFree { get; set; } + } + + [Test] + public void CreateEmpty([Values(1, 100, 200)] int count) + { + var pool = new ElementPool(count, Allocator.Persistent); + + var numElems = 0; + foreach (var elem in pool.Elements) + { + numElems++; + } + + Assert.IsTrue(numElems == 0); + Assert.IsTrue(pool.Capacity == count); + Assert.IsTrue(pool.GetFirstIndex() == -1); + + pool.Dispose(); + } + + [Test] + public void InsertAndClear([Values(1, 100, 200)] int count) + { + var pool = new ElementPool(count, Allocator.Persistent); + + + for (var i = 0; i < count; ++i) + { + pool.Allocate(new PoolTestElement{ TestIndex = i}); + Assert.IsTrue(pool[i].IsAllocated); + } + + Assert.IsTrue(pool.Capacity == count); + + var numElems = 0; + foreach (var elem in pool.Elements) + { + Assert.IsTrue(pool[numElems].TestIndex == numElems); + numElems++; + } + + Assert.IsTrue(numElems == count); + Assert.IsTrue(pool.PeakCount == count); + Assert.IsTrue(pool.GetFirstIndex() == 0); + Assert.IsTrue(pool.GetNextIndex(0) == (count == 1 ? -1 : 1)); + Assert.IsTrue(pool.GetNextIndex(count) == -1); + + pool.Clear(); + + numElems = 0; + foreach (var elem in pool.Elements) + { + numElems++; + } + + Assert.IsTrue(numElems == 0); + Assert.IsTrue(pool.PeakCount == 0); + Assert.IsTrue(pool.GetFirstIndex() == -1); + + pool.Dispose(); + } + + [Test] + public void Copy([Values(1, 100, 200)] int count) + { + var pool = new ElementPool(count, Allocator.Persistent); + var anotherPool = new ElementPool(count, Allocator.Persistent); + + Assert.IsTrue(pool.Capacity == anotherPool.Capacity); + + for (var i = 0; i < count; ++i) + { + pool.Allocate(new PoolTestElement { TestIndex = i }); + Assert.IsTrue(pool[i].IsAllocated); + } + + anotherPool.CopyFrom(pool); + + Assert.IsTrue(pool.PeakCount == anotherPool.PeakCount); + Assert.IsTrue(pool.GetFirstIndex() == anotherPool.GetFirstIndex()); + + for (var i = 0; i < count; ++i) + { + Assert.IsTrue(pool[i].TestIndex == i); + Assert.IsTrue(anotherPool[i].TestIndex==i); + Assert.IsTrue(anotherPool[i].IsAllocated); + } + + pool.Dispose(); + anotherPool.Dispose(); + } + } +} diff --git a/package/Tests/PlayModeTests/Base/Containers/ElementPoolTests.cs.meta b/package/Tests/PlayModeTests/Base/Containers/ElementPoolTests.cs.meta new file mode 100755 index 000000000..5cf0b3c73 --- /dev/null +++ b/package/Tests/PlayModeTests/Base/Containers/ElementPoolTests.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: a3fa5d5e6aea03948a7750442a27df13 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Tests/PlayModeTests/Base/Math.meta b/package/Tests/PlayModeTests/Base/Math.meta new file mode 100755 index 000000000..24db4ae2e --- /dev/null +++ b/package/Tests/PlayModeTests/Base/Math.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: a0ecce21712528a4b8a0e63381882bc7 +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Tests/PlayModeTests/Base/Math/AabbTests.cs b/package/Tests/PlayModeTests/Base/Math/AabbTests.cs new file mode 100755 index 000000000..981334b5f --- /dev/null +++ b/package/Tests/PlayModeTests/Base/Math/AabbTests.cs @@ -0,0 +1,100 @@ +using NUnit.Framework; +using Unity.Mathematics; +using static Unity.Mathematics.math; +using Assert = UnityEngine.Assertions.Assert; +using float3 = Unity.Mathematics.float3; +using quaternion = Unity.Mathematics.quaternion; +using RigidTransform = Unity.Mathematics.RigidTransform; +using TestUtils = Unity.Physics.Tests.Utils.TestUtils; + +namespace Unity.Physics.Tests.Base.Math +{ + public class AabbTests + { + const float k_pi2 = 1.57079632679489f; + + [Test] + public void TestAabb() + { + float3 v0 = float3(100, 200, 300); + float3 v1 = float3(200, 300, 400); + float3 v2 = float3(50, 100, 350); + + Aabb a0; a0.Min = float3.zero; a0.Max = v0; + Aabb a1; a1.Min = float3.zero; a1.Max = v1; + Aabb a2; a2.Min = v2; a2.Max = v1; + Aabb a3; a3.Min = v2; a3.Max = v0; + + Assert.IsTrue(a0.IsValid); + Assert.IsTrue(a1.IsValid); + Assert.IsTrue(a2.IsValid); + Assert.IsFalse(a3.IsValid); + + Assert.IsTrue(a1.Contains(a0)); + Assert.IsFalse(a0.Contains(a1)); + Assert.IsTrue(a1.Contains(a2)); + Assert.IsFalse(a2.Contains(a1)); + Assert.IsFalse(a0.Contains(a2)); + Assert.IsFalse(a2.Contains(a0)); + + // Test Expand / Contains + { + Aabb a5; a5.Min = v2; a5.Max = v1; + float3 testPoint = float3(v2.x - 1.0f, v1.y + 1.0f, .5f * (v2.z + v1.z)); + Assert.IsFalse(a5.Contains(testPoint)); + + a5.Expand(1.5f); + Assert.IsTrue(a5.Contains(testPoint)); + } + + // Test transform + { + Aabb ut; ut.Min = v0; ut.Max = v1; + + // Identity transform should not modify aabb + Aabb outAabb = Unity.Physics.Math.TransformAabb(RigidTransform.identity, ut); + + TestUtils.AreEqual(ut.Min, outAabb.Min, 1e-3f); + + // Test translation + outAabb = Unity.Physics.Math.TransformAabb(new RigidTransform(quaternion.identity, float3(100.0f, 0.0f, 0.0f)), ut); + + Assert.AreEqual(outAabb.Min.x, 200); + Assert.AreEqual(outAabb.Min.y, 200); + Assert.AreEqual(outAabb.Max.x, 300); + Assert.AreEqual(outAabb.Max.z, 400); + + // Test rotation + quaternion rot = quaternion.EulerXYZ(0.0f, 0.0f, k_pi2); + outAabb = Unity.Physics.Math.TransformAabb(new RigidTransform(rot, float3.zero), ut); + + TestUtils.AreEqual(outAabb.Min, float3(-300.0f, 100.0f, 300.0f), 1e-3f); + TestUtils.AreEqual(outAabb.Max, float3(-200.0f, 200.0f, 400.0f), 1e-3f); + TestUtils.AreEqual(outAabb.SurfaceArea, ut.SurfaceArea, 1e-2f); + } + } + + [Test] + public void TestAabbTransform() + { + Random rnd = new Random(0x12345678); + for (int i = 0; i < 100; i++) + { + quaternion r = rnd.NextQuaternionRotation(); + float3 t = rnd.NextFloat3(); + + Aabb orig = new Aabb(); + orig.Include(rnd.NextFloat3()); + orig.Include(rnd.NextFloat3()); + + Aabb outAabb1 = Unity.Physics.Math.TransformAabb(new RigidTransform(r, t), orig); + + Physics.Math.MTransform bFromA = new Physics.Math.MTransform(r, t); + Aabb outAabb2 = Unity.Physics.Math.TransformAabb(bFromA, orig); + + TestUtils.AreEqual(outAabb1.Min, outAabb2.Min, 1e-3f); + TestUtils.AreEqual(outAabb1.Max, outAabb2.Max, 1e-3f); + } + } + } +} diff --git a/package/Tests/PlayModeTests/Base/Math/AabbTests.cs.meta b/package/Tests/PlayModeTests/Base/Math/AabbTests.cs.meta new file mode 100755 index 000000000..99cf9cdaa --- /dev/null +++ b/package/Tests/PlayModeTests/Base/Math/AabbTests.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 4d2b09ccb4feea04bafd40a5825be2b1 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Tests/PlayModeTests/Collision.meta b/package/Tests/PlayModeTests/Collision.meta new file mode 100755 index 000000000..62b5c432c --- /dev/null +++ b/package/Tests/PlayModeTests/Collision.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: 21037744847367d4796150cfd7579793 +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Tests/PlayModeTests/Collision/Colliders.meta b/package/Tests/PlayModeTests/Collision/Colliders.meta new file mode 100755 index 000000000..9591b95dc --- /dev/null +++ b/package/Tests/PlayModeTests/Collision/Colliders.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: 4fa0fa7571f3e1248ab410f793757a52 +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Tests/PlayModeTests/Collision/Colliders/BoxColliderTests.cs b/package/Tests/PlayModeTests/Collision/Colliders/BoxColliderTests.cs new file mode 100755 index 000000000..6398ef6d0 --- /dev/null +++ b/package/Tests/PlayModeTests/Collision/Colliders/BoxColliderTests.cs @@ -0,0 +1,286 @@ +using NUnit.Framework; +using Unity.Mathematics; +using UnityEngine; +using Assert = UnityEngine.Assertions.Assert; +using TestUtils = Unity.Physics.Tests.Utils.TestUtils; +using Unity.Collections.LowLevel.Unsafe; + +namespace Unity.Physics.Tests.Collision.Colliders +{ + /// + /// Test class containing tests for the + /// + public class BoxColliderTests + { + #region Construction + + /// + /// Create a and check that all attributes are set as expected + /// + [Test] + public unsafe void TestBoxColliderCreate() + { + float3 center = new float3(-10.10f, 10.12f, 0.01f); + quaternion orientation = quaternion.AxisAngle(math.normalize(new float3(1.4f, 0.2f, 1.1f)), 38.50f); + float3 size = new float3(0.01f, 120.40f, 5.4f); + float convexRadius = 0.43f; + var collider = BoxCollider.Create(center, orientation, size, convexRadius); + var boxCollider = UnsafeUtilityEx.AsRef(collider.GetUnsafePtr()); + + Assert.AreEqual(center, boxCollider.Center); + Assert.AreEqual(orientation, boxCollider.Orientation); + Assert.AreEqual(size, boxCollider.Size); + Assert.AreEqual(convexRadius, boxCollider.ConvexRadius); + Assert.AreEqual(CollisionType.Convex, boxCollider.CollisionType); + Assert.AreEqual(ColliderType.Box, boxCollider.Type); + } + + /// + /// Create with invalid center/orientation/size/convexRadius and + /// check that exceptions are being thrown for invalid values + /// + [Test] + public void TestBoxColliderCreateInvalid() + { + float3 center = new float3(1.0f, 0.0f, 0.0f); + quaternion orientation = quaternion.AxisAngle(math.normalize(new float3(4.3f, 1.2f, 0.1f)), 1085.0f); + float3 size = new float3(1.0f, 2.0f, 3.0f); + float convexRadius = 0.45f; + + // Invalid center, positive infinity + { + float3 invalidCenter = new float3(float.PositiveInfinity, 1.0f, 1.0f); + TestUtils.ThrowsException( + () => BoxCollider.Create(invalidCenter, orientation, size, convexRadius) + ); + } + + // Invalid center, positive infinity + { + float3 invalidCenter = new float3(float.NegativeInfinity, 1.0f, 1.0f); + TestUtils.ThrowsException( + () => BoxCollider.Create(invalidCenter, orientation, size, convexRadius) + ); + } + + // Invalid center, nan + { + float3 invalidCenter = new float3(float.NaN, 1.0f, 1.0f); + TestUtils.ThrowsException( + () => BoxCollider.Create(invalidCenter, orientation, size, convexRadius) + ); + } + + // Negative size + { + float3 invalidSize = new float3(-1.0f, 1.0f, 1.0f); + TestUtils.ThrowsException( + () => BoxCollider.Create(center, orientation, invalidSize, convexRadius) + ); + } + + // Invalid size, positive inf + { + float3 invalidSize = new float3(float.PositiveInfinity, 1.0f, 1.0f); + TestUtils.ThrowsException( + () => BoxCollider.Create(center, orientation, invalidSize, convexRadius) + ); + } + + // Invalid size, negative inf + { + float3 invalidSize = new float3(float.NegativeInfinity, 1.0f, 1.0f); + TestUtils.ThrowsException( + () => BoxCollider.Create(center, orientation, invalidSize, convexRadius) + ); + } + + // Invalid size, nan + { + float3 invalidSize = new float3(float.NaN, 1.0f, 1.0f); + TestUtils.ThrowsException( + () => BoxCollider.Create(center, orientation, invalidSize, convexRadius) + ); + } + + // Negative convex radius + { + float invalidConvexRadius = -0.0001f; + TestUtils.ThrowsException( + () => BoxCollider.Create(center, orientation, size, invalidConvexRadius) + ); + } + + // Invalid convex radius, +inf + { + float invalidConvexRadius = float.PositiveInfinity; + TestUtils.ThrowsException( + () => BoxCollider.Create(center, orientation, size, invalidConvexRadius) + ); + } + + // Invalid convex radius, -inf + { + float invalidConvexRadius = float.NegativeInfinity; + TestUtils.ThrowsException( + () => BoxCollider.Create(center, orientation, size, invalidConvexRadius) + ); + } + + // Invalid convex radius, nan + { + float invalidConvexRadius = float.NaN; + TestUtils.ThrowsException( + () => BoxCollider.Create(center, orientation, size, invalidConvexRadius) + ); + } + + } + + #endregion + + #region IConvexCollider + + /// + /// Test that a translated box collider generates the correct local AABB + /// + /// + /// The following code was used to produce reference data for Aabbs: + /// + /// private Aabb CalculateBoxAabbNaive(float3 center, quaternion orientation, float3 size, quaternion bRotation, float3 bTranslation) + ///{ + /// float3[] points = { + /// 0.5f * new float3(-size.x, -size.y, -size.z), + /// 0.5f * new float3(-size.x, -size.y, size.z), + /// 0.5f * new float3(-size.x, size.y, -size.z), + /// 0.5f * new float3(-size.x, size.y, size.z), + /// 0.5f * new float3(size.x, -size.y, -size.z), + /// 0.5f * new float3(size.x, -size.y, size.z), + /// 0.5f * new float3(size.x, size.y, -size.z), + /// 0.5f * new float3(size.x, size.y, size.z) + /// }; + /// + /// for (int i = 0; i < 8; ++i) + /// { + /// points[i] = center + math.mul(orientation, points[i]); + /// points[i] = bTranslation + math.mul(bRotation, points[i]); + /// } + /// + /// Aabb result = Aabb.CreateFromPoints(new float3x4(points[0], points[1], points[2], points[3])); + /// for (int i = 4; i < 8; ++i) + /// { + /// result.Include(points[i]); + /// } + /// return result; + ///} + /// + /// + [Test] + public void TestBoxColliderCalculateAabbLocalTranslation() + { + // Expected values in this test were generated using CalculateBoxAabbNaive above + { + float3 center = new float3(-0.59f, 0.36f, 0.35f); + quaternion orientation = quaternion.identity; + float3 size = new float3(2.32f, 10.87f, 16.49f); + float convexRadius = 0.25f; + + Aabb expectedAabb = new Aabb + { + Min = new float3(-1.75f, -5.075f, -7.895f), + Max = new float3(0.57f, 5.795f, 8.595f) + }; + + var boxCollider = BoxCollider.Create(center, orientation, size, convexRadius); + Aabb aabb = boxCollider.Value.CalculateAabb(); + Debug.Log($"Expected: Min: {expectedAabb.Min}, Max: {expectedAabb.Max}, Was: Min: {aabb.Min}, Max: {aabb.Max}"); + TestUtils.AreEqual(expectedAabb.Min, aabb.Min, 1e-3f); + TestUtils.AreEqual(expectedAabb.Max, aabb.Max, 1e-3f); + } + } + + /// + /// Test that a translated and rotated generates the correct local AABB + /// + /// + /// for the code used to generate reference data. + /// + [Test] + public void TestBoxColliderCalculateAabbLocalTranslationAndOrientation() + { + float3 center = new float3(-2.56f, -4.33f, 54.30f); + quaternion orientation = quaternion.AxisAngle(math.normalize(new float3(0, 1, 1)), 42.0f); + float3 size = new float3(5.0f, 0.25f, 1.3f); + float convexRadius = 0.25f; + + Aabb expectedAabb = new Aabb + { + Min = new float3(-4.062223f, -6.442692f, 52.3973f), + Max = new float3(-1.057776f, -2.217308f, 56.2027f) + }; + + var boxCollider = BoxCollider.Create(center, orientation, size, convexRadius); + Aabb aabb = boxCollider.Value.CalculateAabb(); + Debug.Log($"Expeced: Min: {expectedAabb.Min}, Max: {expectedAabb.Max}, Was: Min: {aabb.Min}, Max: {aabb.Max}"); + TestUtils.AreEqual(expectedAabb.Min, aabb.Min, 1e-3f); + TestUtils.AreEqual(expectedAabb.Max, aabb.Max, 1e-3f); + } + + /// + /// Create a rotated and translated and check that the AABB of the transformed collider is correct + /// + /// + /// for the code used to generate reference data. + /// + [Test] + public void TestBoxColliderCalculateAabbTransformed() + { + float3 center = new float3(2.54f, -4.86f, 6.90f); + quaternion orientation = quaternion.AxisAngle(math.normalize(new float3(0.5f, 1, 1)), 42.0f); + float3 size = new float3(50.0f, 0.1f, 2.3f); + float convexRadius = 0.25f; + float3 translation = new float3(-2.5f, 15.0f, -0.01f); + quaternion rotation = quaternion.AxisAngle(math.normalize(new float3(4.2f, 0.1f, -3.3f)), 42.1f); + + Aabb expectedAabb = new Aabb + { + Min = new float3(-17.75146f, 7.216872f, -11.04677f), + Max = new float3(11.82336f, 38.96107f, 17.96488f) + }; + + var boxCollider = BoxCollider.Create(center, orientation, size, convexRadius); + Aabb aabb = boxCollider.Value.CalculateAabb(new RigidTransform(rotation, translation)); + Debug.Log($"Expected: Min: {expectedAabb.Min}, Max: {expectedAabb.Max}, Was: Min: {aabb.Min}, Max: {aabb.Max}"); + TestUtils.AreEqual(expectedAabb.Min, aabb.Min, 1e-3f); + TestUtils.AreEqual(expectedAabb.Max, aabb.Max, 1e-3f); + } + + /// + /// Test that the created inertia tensor of the is correct + /// + /// + /// Formula for inertia tensor from here was used: https://en.wikipedia.org/wiki/List_of_moments_of_inertia + /// + [Test] + public void TestBoxColliderMassProperties() + { + float3 center = float3.zero; + quaternion orientation = quaternion.identity; + float3 size = new float3(1.0f, 250.0f, 2.0f); + float convexRadius = 0.25f; + var boxCollider = BoxCollider.Create(center, orientation, size, convexRadius); + + float3 expectedInertiaTensor = 1.0f / 12.0f * new float3( + size.y * size.y + size.z * size.z, + size.x * size.x + size.z * size.z, + size.y * size.y + size.x * size.x); + + MassProperties massProperties = boxCollider.Value.MassProperties; + float3 inertiaTensor = massProperties.MassDistribution.InertiaTensor; + Debug.Log($"Expected Inertia Tensor: {expectedInertiaTensor}, was: {inertiaTensor}"); + TestUtils.AreEqual(expectedInertiaTensor, inertiaTensor, 1e-3f); + } + + #endregion + } +} diff --git a/package/Tests/PlayModeTests/Collision/Colliders/BoxColliderTests.cs.meta b/package/Tests/PlayModeTests/Collision/Colliders/BoxColliderTests.cs.meta new file mode 100755 index 000000000..a376811ba --- /dev/null +++ b/package/Tests/PlayModeTests/Collision/Colliders/BoxColliderTests.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 44f77c357a720cb4180dbe5d0b760e5c +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Tests/PlayModeTests/Collision/Colliders/CapsuleColliderTests.cs b/package/Tests/PlayModeTests/Collision/Colliders/CapsuleColliderTests.cs new file mode 100755 index 000000000..6765b289f --- /dev/null +++ b/package/Tests/PlayModeTests/Collision/Colliders/CapsuleColliderTests.cs @@ -0,0 +1,195 @@ +using NUnit.Framework; +using Unity.Mathematics; +using UnityEngine; +using Assert = UnityEngine.Assertions.Assert; +using TestUtils = Unity.Physics.Tests.Utils.TestUtils; +using Unity.Collections.LowLevel.Unsafe; + +namespace Unity.Physics.Tests.Collision.Colliders +{ + /// + /// Class collecting all tests for the + /// + public class CapsuleColliderTests + { + #region Construction + + /// + /// Test if all attributes are set as expected when creating a new . + /// + [Test] + unsafe public void TestCapsuleColliderCreate() + { + float3 v0 = new float3(1.45f, 0.34f, -8.65f); + float3 v1 = new float3(100.45f, -80.34f, -8.65f); + float radius = 1.45f; + var collider = CapsuleCollider.Create(v0, v1, radius); + var capsuleCollider = UnsafeUtilityEx.AsRef(collider.GetUnsafePtr()); + Assert.AreEqual(ColliderType.Capsule, capsuleCollider.Type); + Assert.AreEqual(CollisionType.Convex, capsuleCollider.CollisionType); + TestUtils.AreEqual(v0, capsuleCollider.Vertex0); + TestUtils.AreEqual(v1, capsuleCollider.Vertex1); + TestUtils.AreEqual(radius, capsuleCollider.Radius); + } + + /// + /// Test if throws expected exceptions when created with invalid arguments + /// + [Test] + public void TestCapsuleColliderCreateInvalid() + { + float3 v0 = new float3(5.66f, -6.72f, 0.12f); + float3 v1 = new float3(0.98f, 8.88f, 9.54f); + float radius = 0.65f; + + // v0, +inf + { + float3 invalidV0 = new float3(float.PositiveInfinity, 0.0f, 0.0f); + TestUtils.ThrowsException(() => CapsuleCollider.Create(invalidV0, v1, radius)); + } + + // v0, -inf + { + float3 invalidV0 = new float3(float.NegativeInfinity, 0.0f, 0.0f); + TestUtils.ThrowsException(() => CapsuleCollider.Create(invalidV0, v1, radius)); + } + + // v0, nan + { + float3 invalidV0 = new float3(float.NaN, 0.0f, 0.0f); + TestUtils.ThrowsException(() => CapsuleCollider.Create(invalidV0, v1, radius)); + } + + // v1, +inf + { + float3 invalidV1 = new float3(float.PositiveInfinity, 0.0f, 0.0f); + TestUtils.ThrowsException(() => CapsuleCollider.Create(v0, invalidV1, radius)); + } + + // v1, -inf + { + float3 invalidV1 = new float3(float.NegativeInfinity, 0.0f, 0.0f); + TestUtils.ThrowsException(() => CapsuleCollider.Create(v0, invalidV1, radius)); + } + + // v1, nan + { + float3 invalidV1 = new float3(float.NaN, 0.0f, 0.0f); + TestUtils.ThrowsException(() => CapsuleCollider.Create(v0, invalidV1, radius)); + } + + // negative radius + { + float invalidRadius = -0.54f; + TestUtils.ThrowsException(() => CapsuleCollider.Create(v0, v1, invalidRadius)); + } + + // radius, +inf + { + float invalidRadius = float.PositiveInfinity; + TestUtils.ThrowsException(() => CapsuleCollider.Create(v0, v1, invalidRadius)); + } + + // radius, -inf + { + float invalidRadius = float.NegativeInfinity; + TestUtils.ThrowsException(() => CapsuleCollider.Create(v0, v1, invalidRadius)); + } + + // radius, nan + { + float invalidRadius = float.NaN; + TestUtils.ThrowsException(() => CapsuleCollider.Create(v0, v1, invalidRadius)); + } + } + + #endregion + + #region IConvexCollider + + /// + /// Test if the local AABB of the is calculated correctly. + /// + [Test] + public void TestCapsuleColliderCalculateAabbLocal() + { + float radius = 2.3f; + float length = 5.5f; + float3 p0 = new float3(1.1f, 2.2f, 3.4f); + float3 p1 = p0 + length * math.normalize(new float3(1, 1, 1)); + var capsuleCollider = CapsuleCollider.Create(p0, p1, radius); + + Aabb expectedAabb = new Aabb(); + expectedAabb.Min = math.min(p0, p1) - new float3(radius); + expectedAabb.Max = math.max(p0, p1) + new float3(radius); + + Aabb aabb = capsuleCollider.Value.CalculateAabb(); + Debug.Log($"Expected Aabb: Min {expectedAabb.Min}, Max {expectedAabb.Max}"); + Debug.Log($"Actual Aabb: Min {aabb.Min}, Max {aabb.Max}"); + TestUtils.AreEqual(expectedAabb.Min, aabb.Min, 1e-3f); + TestUtils.AreEqual(expectedAabb.Max, aabb.Max, 1e-3f); + } + + /// + /// Test whether the AABB of the transformed is calculated correctly. + /// + [Test] + public void TestCapsuleColliderCalculateAabbTransformed() + { + float radius = 2.3f; + float length = 5.5f; + float3 p0 = new float3(1.1f, 2.2f, 3.4f); + float3 p1 = p0 + length * math.normalize(new float3(1, 1, 1)); + var capsuleCollider = CapsuleCollider.Create(p0, p1, radius); + + float3 translation = new float3(-3.4f, 0.5f, 0.0f); + quaternion rotation = quaternion.AxisAngle(math.normalize(new float3(0.4f, 0.0f, 150.0f)), 123.0f); + + Aabb expectedAabb = new Aabb(); + float3 p0Transformed = math.mul(rotation, p0) + translation; + float3 p1Transformed = math.mul(rotation, p1) + translation; + expectedAabb.Min = math.min(p0Transformed, p1Transformed) - new float3(radius); + expectedAabb.Max = math.max(p0Transformed, p1Transformed) + new float3(radius); + + Aabb aabb = capsuleCollider.Value.CalculateAabb(new RigidTransform(rotation, translation)); + Debug.Log($"Expected Aabb: Min {expectedAabb.Min}, Max {expectedAabb.Max}"); + Debug.Log($"Actual Aabb: Min {aabb.Min}, Max {aabb.Max}"); + TestUtils.AreEqual(expectedAabb.Min, aabb.Min, 1e-3f); + TestUtils.AreEqual(expectedAabb.Max, aabb.Max, 1e-3f); + } + + /// + /// Test whether the inertia tensor of the is calculated correctly. + /// + /// + /// Used the formula from the following article as reference: https://www.gamedev.net/articles/programming/math-and-physics/capsule-inertia-tensor-r3856/ + /// NOTE: There is an error in eq. 14 of the article: it should be H^2 / 4 instead of H^2 / 2 in Ixx and Izz. + /// + [Test] + public void TestCapsuleColliderMassProperties() + { + float radius = 2.3f; + float length = 5.5f; + float3 p0 = new float3(1.1f, 2.2f, 3.4f); + float3 p1 = p0 + length * math.normalize(new float3(1, 1, 1)); + + float hemisphereMass = 0.5f * 4.0f / 3.0f * (float)math.PI * radius * radius * radius; + float cylinderMass = (float)math.PI * radius * radius * length; + float totalMass = 2.0f * hemisphereMass + cylinderMass; + hemisphereMass /= totalMass; + cylinderMass /= totalMass; + + float itX = cylinderMass * (length * length / 12.0f + radius * radius / 4.0f) + 2.0f * hemisphereMass * (2.0f * radius * radius / 5.0f + length * length / 4.0f + 3.0f * length * radius / 8.0f); + float itY = cylinderMass * radius * radius / 2.0f + 4.0f * hemisphereMass * radius * radius / 5.0f; + float itZ = itX; + float3 expectedInertiaTensor = new float3(itX, itY, itZ); + + var capsuleCollider = CapsuleCollider.Create(p0, p1, radius); + float3 inertiaTensor = capsuleCollider.Value.MassProperties.MassDistribution.InertiaTensor; + Debug.Log($"Expected inertia tensor: {expectedInertiaTensor}, was {inertiaTensor}"); + TestUtils.AreEqual(expectedInertiaTensor, inertiaTensor, 1e-3f); + } + + #endregion + } +} diff --git a/package/Tests/PlayModeTests/Collision/Colliders/CapsuleColliderTests.cs.meta b/package/Tests/PlayModeTests/Collision/Colliders/CapsuleColliderTests.cs.meta new file mode 100755 index 000000000..f696225c9 --- /dev/null +++ b/package/Tests/PlayModeTests/Collision/Colliders/CapsuleColliderTests.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 443f98571f426bb4081c0862b168585f +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Tests/PlayModeTests/Collision/Colliders/CompoundColliderTests.cs b/package/Tests/PlayModeTests/Collision/Colliders/CompoundColliderTests.cs new file mode 100755 index 000000000..7f7d4c329 --- /dev/null +++ b/package/Tests/PlayModeTests/Collision/Colliders/CompoundColliderTests.cs @@ -0,0 +1,55 @@ +using NUnit.Framework; +using Unity.Collections; +using Unity.Mathematics; +using Unity.Physics.Tests.Utils; +using Assert = UnityEngine.Assertions.Assert; + +namespace Unity.Physics.Tests.Collision.Colliders +{ + /// + /// Test class containing tests for the + /// + public class CompoundColliderTests + { + [Test] + public void MassProperties_BuiltFromChildren_MatchesExpected() + { + void TestCompoundBox(RigidTransform transform) + { + // Create a unit box + var box = BoxCollider.Create(transform.pos, transform.rot, new float3(1, 1, 1), 0.0f); + + // Create a compound of mini boxes, matching the volume of the single box + var miniBox = BoxCollider.Create(float3.zero, quaternion.identity, new float3(0.5f, 0.5f, 0.5f), 0.0f); + var children = new NativeArray(8, Allocator.Temp) + { + [0] = new CompoundCollider.ColliderBlobInstance { Collider = miniBox, CompoundFromChild = math.mul(transform, new RigidTransform(quaternion.identity, new float3(+0.25f,+0.25f,+0.25f))) }, + [1] = new CompoundCollider.ColliderBlobInstance { Collider = miniBox, CompoundFromChild = math.mul(transform, new RigidTransform(quaternion.identity, new float3(-0.25f,+0.25f,+0.25f))) }, + [2] = new CompoundCollider.ColliderBlobInstance { Collider = miniBox, CompoundFromChild = math.mul(transform, new RigidTransform(quaternion.identity, new float3(+0.25f,-0.25f,+0.25f))) }, + [3] = new CompoundCollider.ColliderBlobInstance { Collider = miniBox, CompoundFromChild = math.mul(transform, new RigidTransform(quaternion.identity, new float3(+0.25f,+0.25f,-0.25f))) }, + [4] = new CompoundCollider.ColliderBlobInstance { Collider = miniBox, CompoundFromChild = math.mul(transform, new RigidTransform(quaternion.identity, new float3(-0.25f,-0.25f,+0.25f))) }, + [5] = new CompoundCollider.ColliderBlobInstance { Collider = miniBox, CompoundFromChild = math.mul(transform, new RigidTransform(quaternion.identity, new float3(+0.25f,-0.25f,-0.25f))) }, + [6] = new CompoundCollider.ColliderBlobInstance { Collider = miniBox, CompoundFromChild = math.mul(transform, new RigidTransform(quaternion.identity, new float3(-0.25f,+0.25f,-0.25f))) }, + [7] = new CompoundCollider.ColliderBlobInstance { Collider = miniBox, CompoundFromChild = math.mul(transform, new RigidTransform(quaternion.identity, new float3(-0.25f,-0.25f,-0.25f))) } + }; + var compound = CompoundCollider.Create(children); + children.Dispose(); + + var boxMassProperties = box.Value.MassProperties; + var compoundMassProperties = compound.Value.MassProperties; + + TestUtils.AreEqual(compoundMassProperties.Volume, boxMassProperties.Volume, 1e-3f); + TestUtils.AreEqual(compoundMassProperties.AngularExpansionFactor, boxMassProperties.AngularExpansionFactor, 1e-3f); + TestUtils.AreEqual(compoundMassProperties.MassDistribution.Transform.pos, boxMassProperties.MassDistribution.Transform.pos, 1e-3f); + //TestUtils.AreEqual(compoundMassProperties.MassDistribution.Orientation, boxMassProperties.MassDistribution.Orientation, 1e-3f); // TODO: Figure out why this differs, and if that is a problem + TestUtils.AreEqual(compoundMassProperties.MassDistribution.InertiaTensor, boxMassProperties.MassDistribution.InertiaTensor, 1e-3f); + } + + // Compare box with compound at various transforms + TestCompoundBox(RigidTransform.identity); + TestCompoundBox(new RigidTransform(quaternion.identity, new float3(1.0f, 2.0f, 3.0f))); + TestCompoundBox(new RigidTransform(quaternion.EulerXYZ(0.5f, 1.0f, 1.5f), float3.zero)); + TestCompoundBox(new RigidTransform(quaternion.EulerXYZ(0.5f, 1.0f, 1.5f), new float3(1.0f, 2.0f, 3.0f))); + } + } +} diff --git a/package/Tests/PlayModeTests/Collision/Colliders/CompoundColliderTests.cs.meta b/package/Tests/PlayModeTests/Collision/Colliders/CompoundColliderTests.cs.meta new file mode 100755 index 000000000..c66752d53 --- /dev/null +++ b/package/Tests/PlayModeTests/Collision/Colliders/CompoundColliderTests.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 92073c43917a12f4188134e4aef77642 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Tests/PlayModeTests/Collision/Colliders/ConvexColliderTests.cs b/package/Tests/PlayModeTests/Collision/Colliders/ConvexColliderTests.cs new file mode 100755 index 000000000..f7071ce4b --- /dev/null +++ b/package/Tests/PlayModeTests/Collision/Colliders/ConvexColliderTests.cs @@ -0,0 +1,294 @@ +using NUnit.Framework; +using Unity.Mathematics; +using UnityEngine; +using Assert = UnityEngine.Assertions.Assert; +using TestUtils = Unity.Physics.Tests.Utils.TestUtils; +using Unity.Collections.LowLevel.Unsafe; +using Unity.Collections; + +namespace Unity.Physics.Tests.Collision.Colliders +{ + /// + /// Contains all unit tests + /// + public class ConvexColliderTests + { + #region Construction + + /// + /// Test that a created with a point cloud has its attributes filled correctly + /// + [Test] + public void TestConvexColliderCreate() + { + var points = new NativeArray(8, Allocator.Temp) + { + [0] = new float3(1.45f, 8.67f, 3.45f), + [1] = new float3(8.75f, 1.23f, 6.44f), + [2] = new float3(100.34f, 5.33f, -2.55f), + [3] = new float3(8.76f, 4.56f, -4.54f), + [4] = new float3(9.75f, -0.45f, -8.99f), + [5] = new float3(7.66f, 3.44f, 0.0f) + }; + var collider = ConvexCollider.Create(points, convexRadius: 0.15f); + points.Dispose(); + + Assert.AreEqual(ColliderType.Convex, collider.Value.Type); + Assert.AreEqual(CollisionType.Convex, collider.Value.CollisionType); + } + + /// + /// That that creating a with invalid point clouds results in exceptions being thrown + /// + [Test] + public void TestConvexColliderCreateInvalid() + { + // Invalid points + { + float convexRadius = 0.15f; + + // invalid point, +inf + { + var invalidPoints = new NativeArray(6, Allocator.Temp) + { + [0] = new float3(1.45f, 8.67f, 3.45f), + [1] = new float3(8.75f, 1.23f, 6.44f), + [2] = new float3(float.PositiveInfinity, 5.33f, -2.55f), + [3] = new float3(8.76f, 4.56f, -4.54f), + [4] = new float3(9.75f, -0.45f, -8.99f), + [5] = new float3(7.66f, 3.44f, 0.0f) + }; + TestUtils.ThrowsException( + () => ConvexCollider.Create(invalidPoints, convexRadius) + ); + invalidPoints.Dispose(); + } + + // invalid point, -inf + { + var invalidPoints = new NativeArray(6, Allocator.Temp) + { + [0] = new float3(1.45f, 8.67f, 3.45f), + [1] = new float3(8.75f, 1.23f, 6.44f), + [2] = new float3(float.NegativeInfinity, 5.33f, -2.55f), + [3] = new float3(8.76f, 4.56f, -4.54f), + [4] = new float3(9.75f, -0.45f, -8.99f), + [5] = new float3(7.66f, 3.44f, 0.0f), + }; + TestUtils.ThrowsException( + () => ConvexCollider.Create(invalidPoints, convexRadius) + ); + invalidPoints.Dispose(); + } + + // invalid point, NaN + { + var invalidPoints = new NativeArray(6, Allocator.Temp) + { + [0] = new float3(1.45f, 8.67f, 3.45f), + [1] = new float3(8.75f, 1.23f, 6.44f), + [2] = new float3(float.NaN, 5.33f, -2.55f), + [3] = new float3(8.76f, 4.56f, -4.54f), + [4] = new float3(9.75f, -0.45f, -8.99f), + [5] = new float3(7.66f, 3.44f, 0.0f) + }; + TestUtils.ThrowsException( + () => ConvexCollider.Create(invalidPoints, convexRadius) + ); + invalidPoints.Dispose(); + } + } + + // invalid convex radius + { + var points = new NativeArray(6, Allocator.Temp) + { + [0] = new float3(1.45f, 8.67f, 3.45f), + [1] = new float3(8.75f, 1.23f, 6.44f), + [2] = new float3(7.54f, 5.33f, -2.55f), + [3] = new float3(8.76f, 4.56f, -4.54f), + [4] = new float3(9.75f, -0.45f, -8.99f), + [5] = new float3(7.66f, 3.44f, 0.0f) + }; + float3 scale = new float3(1.0f, 1.0f, 1.0f); + + // negative convex radius + { + float invalidConvexRadius = -0.30f; + TestUtils.ThrowsException( + () => ConvexCollider.Create(points, invalidConvexRadius) + ); + } + + // +inf convex radius + { + float invalidConvexRadius = float.PositiveInfinity; + TestUtils.ThrowsException( + () => ConvexCollider.Create(points, invalidConvexRadius) + ); + } + + // -inf convex radius + { + float invalidConvexRadius = float.NegativeInfinity; + TestUtils.ThrowsException( + () => ConvexCollider.Create(points, invalidConvexRadius) + ); + } + + // nan convex radius + { + float invalidConvexRadius = float.NaN; + TestUtils.ThrowsException( + () => ConvexCollider.Create(points, invalidConvexRadius) + ); + } + + points.Dispose(); + } + } + + /// + /// Test that the inertia tensor for the convex hull of a point cloud are calculated correctly + /// + /// + /// Code used to generate the reference inertia tensor: + /// + /// hkArray vertices; + /// vertices.pushBack(hkVector4(-1.56f, 8.89f, -10.76f)); + /// vertices.pushBack(hkVector4(-4.74f, 80.11f, 10.56f)); + /// vertices.pushBack(hkVector4(-100.60f, -4.93f, -10.76f)); + /// vertices.pushBack(hkVector4(1.44f, 3.56f, 73.4f)); + /// vertices.pushBack(hkVector4(17.66f, 18.43f, 0.0f)); + /// vertices.pushBack(hkVector4(-1.32f, 9.99f, 80.4f)); + /// vertices.pushBack(hkVector4(-17.45f, 3.22f, -3.22f)); + /// vertices.pushBack(hkVector4(0.0f, 0.0f, 0.03f)); + /// hkStridedVertices stridedVertices; stridedVertices.set(vertices); + /// float convexRadius = 0.15f; + /// + /// hknpConvexShape::BuildConfig buildConfig; + /// buildConfig.m_massConfig.m_inertiaFactor = 1.0f; + /// hknpShape* shape = hknpConvexShape::createFromVertices(stridedVertices, convexRadius, buildConfig); + /// + /// hkMassProperties massProps; + /// shape->getMassProperties(massProps); + /// + /// hkVector4 inertiaTensor; + /// hkRotation principleAxis; + /// hkInertiaTensorComputer::convertInertiaTensorToPrincipleAxis(massProps.m_inertiaTensor, principleAxis); + /// + /// inertiaTensor.setMul(massProps.m_inertiaTensor.getColumn(0), hkVector4::getConstant()); + /// inertiaTensor.addMul(massProps.m_inertiaTensor.getColumn(1), hkVector4::getConstant()); + /// inertiaTensor.addMul(massProps.m_inertiaTensor.getColumn(2), hkVector4::getConstant()); + /// inertiaTensor.mul(hkSimdReal::fromFloat(1.0f / massProps.m_mass)); + /// + /// + [Test] + public void TestConvexColliderMassProperties() + { + var points = new NativeArray(8, Allocator.Temp) + { + [0] = new float3(-1.56f, 8.89f, -10.76f), + [1] = new float3(-4.74f, 80.11f, 10.56f), + [2] = new float3(-100.60f, -4.93f, -10.76f), + [3] = new float3(1.44f, 3.56f, 73.4f), + [4] = new float3(17.66f, 18.43f, 0.0f), + [5] = new float3(-1.32f, 9.99f, 80.4f), + [6] = new float3(-17.45f, 3.22f, -3.22f), + [7] = new float3(0.0f, 0.0f, 0.03f) + }; + + var collider = ConvexCollider.Create(points, convexRadius: 0.15f); + points.Dispose(); + + float3 expectedInertiaTensor = new float3(434.014862f, 824.963989f, 684.776672f); + float3 inertiaTensor = collider.Value.MassProperties.MassDistribution.InertiaTensor; + + // Given the number of FP operations, we do a percentage comparison in this case + Assert.IsTrue(math.abs(expectedInertiaTensor.x - inertiaTensor.x) / expectedInertiaTensor.x < 0.01f); + Assert.IsTrue(math.abs(expectedInertiaTensor.y - inertiaTensor.y) / expectedInertiaTensor.y < 0.01f); + Assert.IsTrue(math.abs(expectedInertiaTensor.z - inertiaTensor.z) / expectedInertiaTensor.z < 0.01f); + } + + #endregion + + #region IConvexCollider + + /// + /// Test that the local AABB is computed correctly for a created with a point cloud + /// + [Test] + public void TestConvexColliderCalculateAabbLocal() + { + var points = new NativeArray(6, Allocator.Temp) + { + [0] = new float3(1.45f, 8.67f, 3.45f), + [1] = new float3(8.75f, 1.23f, 6.44f), + [2] = new float3(100.34f, 5.33f, -2.55f), + [3] = new float3(8.76f, 4.56f, -4.54f), + [4] = new float3(9.75f, -0.45f, -8.99f), + [5] = new float3(7.66f, 3.44f, 0.0f) + }; + float convexRadius = 1.25f; + + Aabb expectedAabb = Aabb.CreateFromPoints(new float3x4(points[0], points[1], points[2], points[3])); + expectedAabb.Include(points[4]); + expectedAabb.Include(points[5]); + + // Currently the convex hull is not shrunk, so we have to expand by the convex radius + expectedAabb.Expand(convexRadius); + + var collider = ConvexCollider.Create(points, convexRadius); + points.Dispose(); + + Aabb actualAabb = collider.Value.CalculateAabb(); + TestUtils.AreEqual(expectedAabb.Min, actualAabb.Min, 1e-3f); + TestUtils.AreEqual(expectedAabb.Max, actualAabb.Max, 1e-3f); + } + + /// + /// Test that the transformed AABB is computed correctly for a created with a point cloud + /// + [Test] + public void TestConvexColliderCalculateAabbTransformed() + { + var points = new NativeArray(6, Allocator.Temp) + { + [0] = new float3(1.45f, 8.67f, 3.45f), + [1] = new float3(8.75f, 1.23f, 6.44f), + [2] = new float3(100.34f, 5.33f, -2.55f), + [3] = new float3(8.76f, 4.56f, -4.54f), + [4] = new float3(9.75f, -0.45f, -8.99f), + [5] = new float3(7.66f, 3.44f, 0.0f) + }; + + float convexRadius = 1.25f; + float3 translation = new float3(43.56f, -87.32f, -0.02f); + quaternion rotation = quaternion.AxisAngle(math.normalize(new float3(8.45f, -2.34f, 0.82f)), 43.21f); + + float3[] transformedPoints = new float3[points.Length]; + for(int i = 0; i < points.Length; ++i) + { + transformedPoints[i] = translation + math.mul(rotation, points[i]); + } + + Aabb expectedAabb = Aabb.CreateFromPoints(new float3x4(transformedPoints[0], transformedPoints[1], transformedPoints[2], transformedPoints[3])); + expectedAabb.Include(transformedPoints[4]); + expectedAabb.Include(transformedPoints[5]); + + // Currently the convex hull is not shrunk, so we have to expand by the convex radius + expectedAabb.Expand(convexRadius); + + var collider = ConvexCollider.Create(points, convexRadius); + points.Dispose(); + + Aabb actualAabb = collider.Value.CalculateAabb(new RigidTransform(rotation, translation)); + + TestUtils.AreEqual(expectedAabb.Min, actualAabb.Min, 1e-3f); + TestUtils.AreEqual(expectedAabb.Max, actualAabb.Max, 1e-3f); + } + + #endregion + } +} + diff --git a/package/Tests/PlayModeTests/Collision/Colliders/ConvexColliderTests.cs.meta b/package/Tests/PlayModeTests/Collision/Colliders/ConvexColliderTests.cs.meta new file mode 100755 index 000000000..3b77e455f --- /dev/null +++ b/package/Tests/PlayModeTests/Collision/Colliders/ConvexColliderTests.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 52979dae16142684baaa5e02667d35e1 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Tests/PlayModeTests/Collision/Colliders/PolygonColliderTests.cs b/package/Tests/PlayModeTests/Collision/Colliders/PolygonColliderTests.cs new file mode 100755 index 000000000..6355bce61 --- /dev/null +++ b/package/Tests/PlayModeTests/Collision/Colliders/PolygonColliderTests.cs @@ -0,0 +1,359 @@ +using NUnit.Framework; +using Unity.Mathematics; +using Assert = UnityEngine.Assertions.Assert; +using TestUtils = Unity.Physics.Tests.Utils.TestUtils; +using Unity.Collections.LowLevel.Unsafe; + +namespace Unity.Physics.Tests.Collision.Colliders +{ + /// + /// Contains all test for the + /// + public class PolygonColliderTests + { + #region Construction + + /// + /// Test whether a triangle collider's attributes are set to the expected values after creation. + /// + [Test] + unsafe public void TestCreateTriangle() + { + float3[] vertices = + { + new float3(-1.4f, 1.4f, 5.6f), + new float3(1.4f, 1.4f, 3.6f), + new float3(0.2f, 1.2f, 5.6f) + }; + float3 normal = math.normalize(math.cross(vertices[1] - vertices[0], vertices[2] - vertices[0])); + + var collider = PolygonCollider.CreateTriangle(vertices[0], vertices[1], vertices[2]); + var triangleCollider = UnsafeUtilityEx.AsRef(collider.GetUnsafePtr()); + Assert.IsTrue(triangleCollider.IsTriangle); + Assert.IsFalse(triangleCollider.IsQuad); + + TestUtils.AreEqual(triangleCollider.Vertices[0], vertices[0], 1e-3f); + TestUtils.AreEqual(triangleCollider.Vertices[1], vertices[1], 1e-3f); + TestUtils.AreEqual(triangleCollider.Vertices[2], vertices[2], 1e-3f); + Assert.AreEqual(2, triangleCollider.Planes.Length); + TestUtils.AreEqual(normal, triangleCollider.Planes[0].Normal, 1e-3f); + TestUtils.AreEqual(-normal, triangleCollider.Planes[1].Normal, 1e-3f); + Assert.AreEqual(ColliderType.Triangle, triangleCollider.Type); + Assert.AreEqual(CollisionType.Convex, triangleCollider.CollisionType); + } + + /// + /// Test whether a quad collider's attributes are set correctly after creation. + /// + [Test] + unsafe public void TestCreateQuad() + { + float3[] vertices = + { + new float3(-4.5f, 0.0f, 1.0f), + new float3(3.4f, 0.7f, 1.0f), + new float3(3.4f, 2.7f, 1.0f), + new float3(-3.4f, 1.2f, 1.0f) + }; + float3 normal = math.normalize(math.cross(vertices[2] - vertices[1], vertices[0] - vertices[1])); + + var collider = PolygonCollider.CreateQuad(vertices[0], vertices[1], vertices[2], vertices[3]); + var quadCollider = UnsafeUtilityEx.AsRef(collider.GetUnsafePtr()); + Assert.IsFalse(quadCollider.IsTriangle); + Assert.IsTrue(quadCollider.IsQuad); + + TestUtils.AreEqual(quadCollider.Vertices[0], vertices[0], 1e-3f); + TestUtils.AreEqual(quadCollider.Vertices[1], vertices[1], 1e-3f); + TestUtils.AreEqual(quadCollider.Vertices[2], vertices[2], 1e-3f); + TestUtils.AreEqual(quadCollider.Vertices[3], vertices[3], 1e-3f); + Assert.AreEqual(2, quadCollider.Planes.Length); + TestUtils.AreEqual(normal, quadCollider.Planes[0].Normal, 1e-3f); + TestUtils.AreEqual(-normal, quadCollider.Planes[1].Normal, 1e-3f); + Assert.AreEqual(ColliderType.Quad, quadCollider.Type); + Assert.AreEqual(CollisionType.Convex, quadCollider.CollisionType); + } + + /// + /// Test that 'unsorted', i.e. neither clockwise nor counter-clockwise winding, quads are created correctly + /// + [Test] + unsafe public void TestCreateQuadUnsorted() + { + float3[] vertices = + { + new float3(-4.5f, 0.0f, 1.0f), + new float3(3.4f, 2.7f, 1.0f), + new float3(3.4f, 0.7f, 1.0f), + new float3(-3.4f, 1.2f, 1.0f) + }; + float3 normal = math.normalize(math.cross(vertices[2] - vertices[1], vertices[0] - vertices[1])); + + var collider = PolygonCollider.CreateQuad(vertices[0], vertices[1], vertices[2], vertices[3]); + var quadCollider = UnsafeUtilityEx.AsRef(collider.GetUnsafePtr()); + Assert.IsFalse(quadCollider.IsTriangle); + Assert.IsTrue(quadCollider.IsQuad); + + TestUtils.AreEqual(quadCollider.Vertices[0], vertices[0], 1e-3f); + TestUtils.AreEqual(quadCollider.Vertices[1], vertices[1], 1e-3f); + TestUtils.AreEqual(quadCollider.Vertices[2], vertices[2], 1e-3f); + TestUtils.AreEqual(quadCollider.Vertices[3], vertices[3], 1e-3f); + Assert.AreEqual(2, quadCollider.Planes.Length); + TestUtils.AreEqual(normal, quadCollider.Planes[0].Normal, 1e-3f); + TestUtils.AreEqual(-normal, quadCollider.Planes[1].Normal, 1e-3f); + Assert.AreEqual(ColliderType.Quad, quadCollider.Type); + Assert.AreEqual(CollisionType.Convex, quadCollider.CollisionType); + } + + /// + /// Test that exceptions are thrown properly for invalid arguments of creation. + /// + [Test] + public void TestCreateInvalid() + { + // +inf vertex + { + float3[] vertices = + { + new float3(-4.5f, float.PositiveInfinity, 1.0f), + new float3(3.4f, 2.7f, 1.0f), + new float3(3.4f, 0.7f, 1.0f), + new float3(-3.4f, 1.2f, 1.1f) + }; + TestUtils.ThrowsException( + () => PolygonCollider.CreateTriangle(vertices[0], vertices[1], vertices[2]) + ); + TestUtils.ThrowsException( + () => PolygonCollider.CreateQuad(vertices[0], vertices[1], vertices[2], vertices[3]) + ); + } + + // -inf vertex + { + float3[] vertices = + { + new float3(-4.5f, float.NegativeInfinity, 1.0f), + new float3(3.4f, 2.7f, 1.0f), + new float3(3.4f, 0.7f, 1.0f), + new float3(-3.4f, 1.2f, 1.1f) + }; + TestUtils.ThrowsException( + () => PolygonCollider.CreateTriangle(vertices[0], vertices[1], vertices[2]) + ); + TestUtils.ThrowsException( + () => PolygonCollider.CreateQuad(vertices[0], vertices[1], vertices[2], vertices[3]) + ); + } + + // nan vertex + { + float3[] vertices = + { + new float3(-4.5f, float.NaN, 1.0f), + new float3(3.4f, 2.7f, 1.0f), + new float3(3.4f, 0.7f, 1.0f), + new float3(-3.4f, 1.2f, 1.1f) + }; + TestUtils.ThrowsException( + () => PolygonCollider.CreateTriangle(vertices[0], vertices[1], vertices[2]) + ); + TestUtils.ThrowsException( + () => PolygonCollider.CreateQuad(vertices[0], vertices[1], vertices[2], vertices[3]) + ); + } + + // non-planar quad + { + float3[] nonPlanarQuad = + { + new float3(-4.5f, 0.0f, 1.0f), + new float3(3.4f, 0.7f, 1.0f), + new float3(3.4f, 2.7f, 1.0f), + new float3(-3.4f, 1.2f, 1.1f) + }; + TestUtils.ThrowsException( + () => PolygonCollider.CreateQuad(nonPlanarQuad[0], nonPlanarQuad[1], nonPlanarQuad[2], nonPlanarQuad[3]) + ); + } + + // non-planar quad unsorted + { + float3[] nonPlanarQuad = + { + new float3(-4.5f, -0.30f, 1.0f), + new float3(3.4f, 2.7f, 1.0f), + new float3(3.4f, -0.7f, 1.0f), + new float3(-3.4f, 1.2f, 1.1f) + }; + TestUtils.ThrowsException( + () => PolygonCollider.CreateQuad(nonPlanarQuad[0], nonPlanarQuad[1], nonPlanarQuad[2], nonPlanarQuad[3]) + ); + } + } + + #endregion + + #region IConvexCollider + + /// + /// Test that the local AABB of a triangle collider is computed correctly + /// + [Test] + public void TestCalculateAabbLocalTriangle() + { + float3[] vertices = + { + new float3(-1.8f, 2.4f, 4.6f), + new float3(1.4f, 1.6f, 1.6f), + new float3(0.2f, 1.2f, 3.6f) + }; + + var collider = PolygonCollider.CreateTriangle(vertices[0], vertices[1], vertices[2]); + Aabb aabb = collider.Value.CalculateAabb(); + + Aabb expected = new Aabb() + { + Min = math.min(math.min(vertices[0], vertices[1]), vertices[2]), + Max = math.max(math.max(vertices[0], vertices[1]), vertices[2]) + }; + + TestUtils.AreEqual(expected.Min, aabb.Min, 1e-3f); + TestUtils.AreEqual(expected.Max, aabb.Max, 1e-3f); + } + + /// + /// Test that the local AABB of a quad collider is calculated correctly + /// + [Test] + public void TestCalculateAabbLocalQuad() + { + float3[] quadVertices = + { + new float3(-4.5f, 0.0f, 1.0f), + new float3(3.4f, 0.7f, 1.0f), + new float3(3.4f, 2.7f, 1.0f), + new float3(-3.4f, 1.2f, 1.0f) + }; + var collider = PolygonCollider.CreateQuad(quadVertices[0], quadVertices[1], quadVertices[2], quadVertices[3]); + Aabb aabb = collider.Value.CalculateAabb(); + Aabb expected = Aabb.CreateFromPoints(new float3x4(quadVertices[0], quadVertices[1], quadVertices[2], quadVertices[3])); + + TestUtils.AreEqual(expected.Min, aabb.Min, 1e-3f); + TestUtils.AreEqual(expected.Max, aabb.Max, 1e-3f); + } + + /// + /// Test that the AABB of a transformed triangle collider is calculated correctly + /// + [Test] + public void TestCalculateAabbTransformedTriangle() + { + float3[] vertices = + { + new float3(-1.8f, 2.4f, 4.6f), + new float3(1.4f, 1.6f, 1.6f), + new float3(0.2f, 1.2f, 3.6f) + }; + + float3 translation = new float3(3.4f, 2.5f, -1.1f); + quaternion rotation = quaternion.AxisAngle(math.normalize(new float3(1.1f, 10.1f, -3.4f)), 78.0f); + + var collider = PolygonCollider.CreateTriangle(vertices[0], vertices[1], vertices[2]); + Aabb aabb = collider.Value.CalculateAabb(new RigidTransform(rotation, translation)); + + for (int i = 0; i < 3; ++i) + { + vertices[i] = translation + math.mul(rotation, vertices[i]); + } + + Aabb expected = new Aabb() + { + Min = math.min(math.min(vertices[0], vertices[1]), vertices[2]), + Max = math.max(math.max(vertices[0], vertices[1]), vertices[2]) + }; + + TestUtils.AreEqual(expected.Min, aabb.Min, 1e-3f); + TestUtils.AreEqual(expected.Max, aabb.Max, 1e-3f); + } + + /// + /// Test that the AABB of a transformed quad collider is calculated correctly + /// + [Test] + public void TestCalculateAabbTransformedQuad() + { + float3[] vertices = +{ + new float3(-4.5f, 0.0f, 1.0f), + new float3(3.4f, 0.7f, 1.0f), + new float3(3.4f, 2.7f, 1.0f), + new float3(-3.4f, 1.2f, 1.0f) + }; + + float3 translation = new float3(-3.4f, -2.5f, -1.1f); + quaternion rotation = quaternion.AxisAngle(math.normalize(new float3(11.1f, 10.1f, -3.4f)), 178.0f); + + var collider = PolygonCollider.CreateQuad(vertices[0], vertices[1], vertices[2], vertices[3]); + Aabb aabb = collider.Value.CalculateAabb(new RigidTransform(rotation, translation)); + + for (int i = 0; i < 4; ++i) + { + vertices[i] = translation + math.mul(rotation, vertices[i]); + } + + Aabb expected = Aabb.CreateFromPoints(new float3x4(vertices[0], vertices[1], vertices[2], vertices[3])); + + TestUtils.AreEqual(expected.Min, aabb.Min, 1e-3f); + TestUtils.AreEqual(expected.Max, aabb.Max, 1e-3f); + } + + //[Test] // #TODO: Add test back in once we have implemented this in Physics + unsafe public void TestMassPropertiesTriangle() + { + // constructing the triangle to be parallel to the xy plane so we don't have to figure out the transformations first + float3[] vertices = + { + new float3(-1.1f, -0.4f, 0.0f), + new float3(0.8f, -0.1f, 0.0f), + new float3(-0.2f, 1.3f, 0.0f) + }; + + var collider = PolygonCollider.CreateTriangle(vertices[0], vertices[1], vertices[2]); + + float3 inertiaTensor = collider.Value.MassProperties.MassDistribution.InertiaTensor; + float3 expectedInertiaTensor = calcTriangleInertiaTensor(vertices[0], vertices[1], vertices[2]); + TestUtils.AreEqual(expectedInertiaTensor, inertiaTensor, 1e-3f); + } + + //[Test] // #TODO: Add test back in once we have implemented this in Physics + public void TestMassPropertiesQuad() + { + float3[] vertices = +{ + new float3(-1.1f, -0.4f, 0.0f), + new float3(0.8f, -0.1f, 0.0f), + new float3(1.2f, 1.3f, 0.0f), + new float3(-0.2f, 1.3f, 0.0f) + }; + + var collider = PolygonCollider.CreateQuad(vertices[0], vertices[1], vertices[2], vertices[3]); + + float3 inertiaTensor = collider.Value.MassProperties.MassDistribution.InertiaTensor; + float3 expectedInertiaTensor = calcQuadInertiaTensor(vertices[0], vertices[1], vertices[2], vertices[3]); + TestUtils.AreEqual(expectedInertiaTensor, inertiaTensor, 1e-3f); + } + + private float3 calcTriangleInertiaTensor(float3 v0, float3 v1, float3 v2) + { + // #TODO: Add function once inertia is properly computed in Physics + return new float3(0, 0, 0); + } + + private float3 calcQuadInertiaTensor(float3 v0, float3 v1, float3 v2, float3 v3) + { + // #TODO: Add function once inertia is properly computed in Physics + return new float3(0, 0, 0); + } + + #endregion + } +} diff --git a/package/Tests/PlayModeTests/Collision/Colliders/PolygonColliderTests.cs.meta b/package/Tests/PlayModeTests/Collision/Colliders/PolygonColliderTests.cs.meta new file mode 100755 index 000000000..459523d04 --- /dev/null +++ b/package/Tests/PlayModeTests/Collision/Colliders/PolygonColliderTests.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 6fdbe2f79acdddd489f332222249d194 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Tests/PlayModeTests/Collision/Colliders/SphereColliderTests.cs b/package/Tests/PlayModeTests/Collision/Colliders/SphereColliderTests.cs new file mode 100755 index 000000000..ffede48c2 --- /dev/null +++ b/package/Tests/PlayModeTests/Collision/Colliders/SphereColliderTests.cs @@ -0,0 +1,170 @@ +using NUnit.Framework; +using Unity.Mathematics; +using UnityEngine; +using Assert = UnityEngine.Assertions.Assert; +using TestUtils = Unity.Physics.Tests.Utils.TestUtils; +using Unity.Collections.LowLevel.Unsafe; + +namespace Unity.Physics.Tests.Collision.Colliders +{ + /// + /// Contains all tests for the + /// + public class SphereColliderTests + { + #region Construction + + /// + /// Tests if a created has its attributes set correctly + /// + [Test] + unsafe public void TestSphereColliderCreate() + { + float3 center = new float3(-8.45f, 9.65f, -0.10f); + float radius = 0.98f; + var collider = SphereCollider.Create(center, radius); + var sphereCollider = UnsafeUtilityEx.AsRef(collider.GetUnsafePtr()); + + TestUtils.AreEqual(center, sphereCollider.Center, 1e-3f); + TestUtils.AreEqual(radius, sphereCollider.Radius, 1e-3f); + Assert.AreEqual(ColliderType.Sphere, sphereCollider.Type); + Assert.AreEqual(CollisionType.Convex, sphereCollider.CollisionType); + } + + /// + /// Create invalid s and check that appropriate exceptions are being thrown + /// + [Test] + public void TestSphereColliderCreateInvalid() + { + float3 center = new float3(-10.34f, 0.0f, -1.54f); + float radius = 1.25f; + + // positive inf center + { + float3 invalidCenter = new float3(float.PositiveInfinity, 0.0f, 0.0f); + TestUtils.ThrowsException( + () => SphereCollider.Create(invalidCenter, radius) + ); + } + + // negative inf center + { + float3 invalidCenter = new float3(float.NegativeInfinity, 0.0f, 0.0f); + TestUtils.ThrowsException( + () => SphereCollider.Create(invalidCenter, radius) + ); + } + + // nan center + { + float3 invalidCenter = new float3(float.NaN, 0.0f, 0.0f); + TestUtils.ThrowsException( + () => SphereCollider.Create(invalidCenter, radius) + ); + } + + // negative radius + { + float invalidRadius = -0.5f; + TestUtils.ThrowsException( + () => SphereCollider.Create(center, invalidRadius) + ); + } + + // positive inf radius + { + float invalidRadius = float.PositiveInfinity; + TestUtils.ThrowsException( + () => SphereCollider.Create(center, invalidRadius) + ); + } + + // negative inf radius + { + float invalidRadius = float.NegativeInfinity; + TestUtils.ThrowsException( + () => SphereCollider.Create(center, invalidRadius) + ); + } + + // nan radius + { + float invalidRadius = float.NaN; + TestUtils.ThrowsException( + () => SphereCollider.Create(center, invalidRadius) + ); + } + } + + #endregion + + #region IConvexCollider + + /// + /// Test that the local AABB of a are calculated correctly + /// + [Test] + public void TestSphereColliderCalculateAabbLocal() + { + float3 center = new float3(-8.4f, 5.63f, -7.2f); + float radius = 2.3f; + var sphereCollider = SphereCollider.Create(center, radius); + + Aabb expected = new Aabb(); + expected.Min = center - new float3(radius, radius, radius); + expected.Max = center + new float3(radius, radius, radius); + + Aabb actual = sphereCollider.Value.CalculateAabb(); + Debug.Log($"Expected aabb: Min: {expected.Min} Max: {expected.Max}, was Min: {actual.Min} Max: {actual.Max}"); + TestUtils.AreEqual(expected.Min, actual.Min, 1e-3f); + TestUtils.AreEqual(expected.Max, actual.Max, 1e-3f); + } + + /// + /// Test that the AABB of a transformed is calculated correctly + /// + [Test] + public void TestSphereColliderCalculateAabbTransformed() + { + float3 center = new float3(-3.4f, 0.63f, -17.2f); + float radius = 5.3f; + var sphereCollider = SphereCollider.Create(center, radius); + + float3 translation = new float3(8.3f, -0.5f, 170.0f); + quaternion rotation = quaternion.AxisAngle(math.normalize(new float3(1.1f, 4.5f, 0.0f)), 146.0f); + + Aabb expected = new Aabb(); + expected.Min = math.mul(rotation, center) + translation - new float3(radius, radius, radius); + expected.Max = math.mul(rotation, center) + translation + new float3(radius, radius, radius); + + Aabb actual = sphereCollider.Value.CalculateAabb(new RigidTransform(rotation, translation)); + Debug.Log($"Expected aabb: Min: {expected.Min} Max: {expected.Max}, was Min: {actual.Min} Max: {actual.Max}"); + TestUtils.AreEqual(expected.Min, actual.Min, 1e-3f); + TestUtils.AreEqual(expected.Max, actual.Max, 1e-3f); + + } + + /// + /// Test that the inertia tensor of a is calculated correctly + /// + /// + /// Inertia tensor formula taken from here: https://en.wikipedia.org/wiki/List_of_moments_of_inertia + /// + [Test] + public void TestSphereColliderMassProperties() + { + float3 center = new float3(-8.4f, 5.63f, 77.2f); + float radius = 2.3f; + var sphereCollider = SphereCollider.Create(center, radius); + + float inertia = 2.0f / 5.0f * radius * radius; + float3 expectedInertiaTensor = new float3(inertia, inertia, inertia); + float3 inertiaTensor = sphereCollider.Value.MassProperties.MassDistribution.InertiaTensor; + Debug.Log($"Expected inertia tensor: {expectedInertiaTensor}, was: {inertiaTensor}"); + TestUtils.AreEqual(expectedInertiaTensor, inertiaTensor, 1e-3f); + } + + #endregion + } +} diff --git a/package/Tests/PlayModeTests/Collision/Colliders/SphereColliderTests.cs.meta b/package/Tests/PlayModeTests/Collision/Colliders/SphereColliderTests.cs.meta new file mode 100755 index 000000000..bb33a3afa --- /dev/null +++ b/package/Tests/PlayModeTests/Collision/Colliders/SphereColliderTests.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 9cbcb0aa5c04bd748aaa37190c4e26b0 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Tests/PlayModeTests/Collision/Filter.meta b/package/Tests/PlayModeTests/Collision/Filter.meta new file mode 100755 index 000000000..81de38a25 --- /dev/null +++ b/package/Tests/PlayModeTests/Collision/Filter.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: a81959e8fa2469c4995a2d0990b849e9 +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Tests/PlayModeTests/Collision/Filter/FilterTests.cs b/package/Tests/PlayModeTests/Collision/Filter/FilterTests.cs new file mode 100755 index 000000000..e9c6c7ad8 --- /dev/null +++ b/package/Tests/PlayModeTests/Collision/Filter/FilterTests.cs @@ -0,0 +1,186 @@ +using NUnit.Framework; + +namespace Unity.Physics.Tests.Collision.Filter +{ + public class FilterTests + { + //CollisionFilter expected behavior: + // uint MaskBits A bit mask describing which layers this object belongs to. + // uint CategoryBits A bit mask describing which layers this object can collide with. + // int GroupIndex An optional override for the bit mask checks. + // If the value in both objects is equal and positive, the objects always collide. + // If the value in both objects is equal and negative, the objects never collide. + + [Test] + public void CollisionFilterTestLayerSelfCollision() + { + var filter0 = new CollisionFilter(); + + var filter1 = new CollisionFilter + { + MaskBits = 1, + CategoryBits = 1 + }; + + var filter2 = new CollisionFilter + { + MaskBits = 0xffffffff, + CategoryBits = 0xffffffff + }; + + Assert.IsFalse(CollisionFilter.IsCollisionEnabled(filter0, filter0)); + Assert.IsFalse(CollisionFilter.IsCollisionEnabled(CollisionFilter.Zero, CollisionFilter.Zero)); + Assert.IsTrue(filter0.Equals(CollisionFilter.Zero)); + + Assert.IsTrue(CollisionFilter.IsCollisionEnabled(filter1, filter1)); + Assert.IsTrue(CollisionFilter.IsCollisionEnabled(filter2, filter2)); + Assert.IsTrue(CollisionFilter.IsCollisionEnabled(CollisionFilter.Default, CollisionFilter.Default)); + Assert.IsTrue(filter2.Equals(CollisionFilter.Default)); + } + + [Test] + public void CollisionFilterTestLayerAndCategoryBits() + { + var filterA = new CollisionFilter + { + MaskBits = 1, + CategoryBits = 2 + }; + + var filterB = new CollisionFilter + { + MaskBits = 2, + CategoryBits = 1 + }; + + Assert.IsFalse(CollisionFilter.IsCollisionEnabled(filterA, filterA)); + Assert.IsFalse(CollisionFilter.IsCollisionEnabled(filterB, filterB)); + + Assert.IsTrue(CollisionFilter.IsCollisionEnabled(filterA, filterB)); + Assert.IsTrue(CollisionFilter.IsCollisionEnabled(filterB, filterA)); + + Assert.IsTrue(CollisionFilter.IsCollisionEnabled(filterA, CollisionFilter.Default)); + Assert.IsTrue(CollisionFilter.IsCollisionEnabled(filterB, CollisionFilter.Default)); + } + + [Test] + public void CollisionFilterTestGroupIndexSimple() + { + var filter0 = new CollisionFilter + { + GroupIndex = 1 + }; + + var filter1 = new CollisionFilter + { + MaskBits = 1, + CategoryBits = 1, + GroupIndex = -1 + }; + + Assert.IsFalse(CollisionFilter.IsCollisionEnabled(filter0, CollisionFilter.Zero)); + Assert.IsFalse(CollisionFilter.IsCollisionEnabled(filter0, CollisionFilter.Default)); + + Assert.IsFalse(CollisionFilter.IsCollisionEnabled(filter1, CollisionFilter.Zero)); + Assert.IsTrue(CollisionFilter.IsCollisionEnabled(filter1, CollisionFilter.Default)); + + Assert.IsTrue(CollisionFilter.IsCollisionEnabled(filter0, filter0)); + Assert.IsFalse(CollisionFilter.IsCollisionEnabled(filter1, filter1)); + } + + [Test] + public void CollisionFilterTestGroupIndex() + { + var filter0 = new CollisionFilter + { + GroupIndex = 1 + }; + + var filter1 = new CollisionFilter + { + MaskBits = 1, + CategoryBits = 1, + GroupIndex = -1 + }; + + var filter2 = new CollisionFilter + { + MaskBits = 1, + CategoryBits = 1, + GroupIndex = 1 + }; + + var filter3 = new CollisionFilter + { + MaskBits = 0, + CategoryBits = 0, + GroupIndex = -1 + }; + + Assert.IsTrue(CollisionFilter.IsCollisionEnabled(filter0, filter2)); + Assert.IsTrue(CollisionFilter.IsCollisionEnabled(filter2, filter0)); + + Assert.IsTrue(CollisionFilter.IsCollisionEnabled(filter1, filter2)); + Assert.IsTrue(CollisionFilter.IsCollisionEnabled(filter2, filter1)); + + Assert.IsFalse(CollisionFilter.IsCollisionEnabled(filter3, filter1)); + Assert.IsFalse(CollisionFilter.IsCollisionEnabled(filter1, filter3)); + } + + [Test] + public void CollisionFilterTestCreateUnion() + { + //Union: GroupIndex will only be not 0 if both operands have the same GroupIndex + + var filter0 = new CollisionFilter + { + GroupIndex = 1 + }; + + var filter1 = new CollisionFilter + { + MaskBits = 1, + CategoryBits = 1, + GroupIndex = -1 + }; + + var filter2 = new CollisionFilter + { + MaskBits = 1, + CategoryBits = 1, + GroupIndex = 1 + }; + + var filter3 = new CollisionFilter + { + MaskBits = 0, + CategoryBits = 0, + GroupIndex = -1 + }; + + var filter4 = new CollisionFilter + { + MaskBits = 1, + CategoryBits = 1, + GroupIndex = 0 + }; + + Assert.IsTrue(CollisionFilter.CreateUnion(CollisionFilter.Zero, CollisionFilter.Default) + .Equals(CollisionFilter.Default)); + + Assert.IsTrue(CollisionFilter.CreateUnion(filter0, CollisionFilter.Zero).Equals(CollisionFilter.Zero)); + Assert.IsTrue(CollisionFilter.CreateUnion(filter1, CollisionFilter.Zero).Equals(filter4)); + Assert.IsTrue(CollisionFilter.CreateUnion(filter2, CollisionFilter.Zero).Equals(filter4)); + Assert.IsTrue(CollisionFilter.CreateUnion(filter3, CollisionFilter.Zero).Equals(CollisionFilter.Zero)); + Assert.IsTrue(CollisionFilter.CreateUnion(filter4, CollisionFilter.Zero).Equals(filter4)); + + Assert.IsTrue(CollisionFilter.CreateUnion(filter0, filter1).Equals(filter4)); + Assert.IsTrue(CollisionFilter.CreateUnion(filter1, filter3).Equals(filter1)); + Assert.IsTrue(CollisionFilter.CreateUnion(filter1, filter2).Equals(filter4)); + Assert.IsTrue(CollisionFilter.CreateUnion(filter2, filter3).Equals(filter4)); + Assert.IsTrue(CollisionFilter.CreateUnion(filter3, filter4).Equals(filter4)); + } + + } +} + diff --git a/package/Tests/PlayModeTests/Collision/Filter/FilterTests.cs.meta b/package/Tests/PlayModeTests/Collision/Filter/FilterTests.cs.meta new file mode 100755 index 000000000..74519a390 --- /dev/null +++ b/package/Tests/PlayModeTests/Collision/Filter/FilterTests.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: b783fe2c08e9e0a47a0ce86f47961bc7 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Tests/PlayModeTests/Collision/Geometry.meta b/package/Tests/PlayModeTests/Collision/Geometry.meta new file mode 100755 index 000000000..667132381 --- /dev/null +++ b/package/Tests/PlayModeTests/Collision/Geometry.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: d1fb0c8417f9d41468f29c46d8903546 +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Tests/PlayModeTests/Collision/Geometry/BoundingVolumeHierarchyBuilderTests.cs b/package/Tests/PlayModeTests/Collision/Geometry/BoundingVolumeHierarchyBuilderTests.cs new file mode 100755 index 000000000..54656f464 --- /dev/null +++ b/package/Tests/PlayModeTests/Collision/Geometry/BoundingVolumeHierarchyBuilderTests.cs @@ -0,0 +1,557 @@ +using NUnit.Framework; +using System.Collections.Generic; +using Unity.Burst; +using Unity.Collections; +using Unity.Collections.LowLevel.Unsafe; +using Unity.Jobs; +using Unity.Mathematics; +using Unity.PerformanceTesting; +using UnityEngine; +using static Unity.Physics.BoundingVolumeHierarchy; +using static Unity.Physics.BoundingVolumeHierarchy.Builder; +using Assert = UnityEngine.Assertions.Assert; +using Random = UnityEngine.Random; + +namespace Unity.Physics.Tests.Collision.Geometry +{ + public class BoundingVolumeHierarchyBuilderTests + { + public void InitInputArrays(NativeArray points, NativeArray aabbs, NativeArray filters) + { + Random.InitState(1234); + + const int posRange = 1000; + const int radiusRangeMin = 1; + const int radiusRangeMax = 10; + + for (int i = 0; i < points.Length; i++) + { + float3 pos; + pos.x = Random.Range(-posRange, posRange); + pos.y = Random.Range(-posRange, posRange); + pos.z = Random.Range(-posRange, posRange); + points[i] = new PointAndIndex { Position = pos, Index = i }; + + float3 radius = new float3(Random.Range(radiusRangeMin, radiusRangeMax)); + aabbs[i] = new Aabb { Min = pos - radius, Max = pos + radius }; + + filters[i] = CollisionFilter.Default; + } + } + + public void InitInputWithCopyArrays(NativeArray points, NativeArray aabbs, NativeArray filters) + { + Random.InitState(1234); + + const int posRange = 1000; + const int radiusRangeMin = 1; + const int radiusRangeMax = 10; + + for (int i = 0; i < points.Length; i++) + { + float3 pos; + pos.x = Random.Range(-posRange, posRange); + pos.y = Random.Range(-posRange, posRange); + pos.z = Random.Range(-posRange, posRange); + points[i] = new PointAndIndex { Position = pos, Index = i }; + + float3 radius = new float3(Random.Range(radiusRangeMin, radiusRangeMax)); + aabbs[i] = new Aabb { Min = pos - radius, Max = pos + radius }; + + points[i + 1] = new PointAndIndex { Position = pos, Index = i + 1 }; + + aabbs[i + 1] = new Aabb { Min = pos - radius, Max = pos + radius }; + + filters[i] = new CollisionFilter + { + GroupIndex = 0, + CategoryBits = (uint)Random.Range(0, 16), + MaskBits = (uint)Random.Range(0, 16) + }; + + filters[i + 1] = new Physics.CollisionFilter + { + GroupIndex = 0, + CategoryBits = (uint)Random.Range(0, 16), + MaskBits = (uint)Random.Range(0, 16) + }; + + i++; + } + } + + [Test] + public void BuildTree([Values(2, 10, 100, 1000)] int elementCount) + { + int numNodes = elementCount / 3 * 2 + 4; + var points = new NativeArray(elementCount, Allocator.Temp, NativeArrayOptions.UninitializedMemory); + var aabbs = new NativeArray(elementCount, Allocator.Temp, NativeArrayOptions.UninitializedMemory); + var filters = new NativeArray(elementCount, Allocator.TempJob, NativeArrayOptions.UninitializedMemory); + + InitInputArrays(points, aabbs, filters); + + var nodes = new NativeArray(numNodes, Allocator.Temp, NativeArrayOptions.UninitializedMemory); + + var bvh = new BoundingVolumeHierarchy(nodes); + bvh.Build(points, aabbs, out int numNodesOut); + bvh.CheckIntegrity(); + + points.Dispose(); + filters.Dispose(); + aabbs.Dispose(); + nodes.Dispose(); + } + + [Test] + public void BuildTreeByBranches([Values(2, 10, 33, 100, 1000)] int elementCount) + { + const int threadCount = 8; + int numNodes = elementCount + Constants.MaxNumTreeBranches; + + var points = new NativeArray(elementCount, Allocator.Temp, NativeArrayOptions.UninitializedMemory); + var aabbs = new NativeArray(elementCount, Allocator.Temp, NativeArrayOptions.UninitializedMemory); + var filters = new NativeArray(elementCount, Allocator.Temp, NativeArrayOptions.UninitializedMemory); + InitInputArrays(points, aabbs, filters); + + var nodes = new NativeArray(numNodes, Allocator.Temp, NativeArrayOptions.UninitializedMemory); + + var ranges = new NativeArray(numNodes, Allocator.Temp, NativeArrayOptions.UninitializedMemory); + var branchNodeOffsets = new NativeArray(numNodes, Allocator.Temp, NativeArrayOptions.UninitializedMemory); + + var bvh = new BoundingVolumeHierarchy(nodes); + + bvh.BuildFirstNLevels(points, ranges, branchNodeOffsets, threadCount, out int branchCount); + + int minBranchNodeIndex = branchNodeOffsets[0]; + for (int i = 0; i < branchCount; i++) + { + bvh.BuildBranch(points, aabbs, ranges[i], branchNodeOffsets[i]); + minBranchNodeIndex = math.min(branchNodeOffsets[i], minBranchNodeIndex); + } + + bvh.Refit(aabbs, 1, minBranchNodeIndex); + + bvh.CheckIntegrity(); + + points.Dispose(); + filters.Dispose(); + aabbs.Dispose(); + nodes.Dispose(); + + ranges.Dispose(); + branchNodeOffsets.Dispose(); + } + + [Test] + public unsafe void BuildTreeTasks([Values(2, 10, 33, 100, 1000)] int elementCount) + { + const int threadCount = 8; + int numNodes = elementCount + Constants.MaxNumTreeBranches; + + var points = new NativeArray(elementCount, Allocator.TempJob, NativeArrayOptions.UninitializedMemory); + var aabbs = new NativeArray(elementCount, Allocator.TempJob, NativeArrayOptions.UninitializedMemory); + var filters = new NativeArray(elementCount, Allocator.TempJob, NativeArrayOptions.UninitializedMemory); + InitInputArrays(points, aabbs, filters); + + var nodes = new NativeArray(numNodes, Allocator.TempJob, NativeArrayOptions.UninitializedMemory); + + var ranges = new NativeArray(Constants.MaxNumTreeBranches, Allocator.TempJob, NativeArrayOptions.UninitializedMemory); + var branchNodeOffset = new NativeArray(Constants.MaxNumTreeBranches, Allocator.TempJob, NativeArrayOptions.UninitializedMemory); + var branchCount = new NativeArray(1, Allocator.TempJob, NativeArrayOptions.UninitializedMemory); + + var handle = new BuildFirstNLevelsJob + { + Points = points, + Nodes = (Node*)nodes.GetUnsafePtr(), + Ranges = ranges, + BranchNodeOffsets = branchNodeOffset, + BranchCount = branchCount, + ThreadCount = threadCount + }.Schedule(); + + var task2 = new BuildBranchesJob + { + Points = points, + Aabbs = aabbs, + BodyFilters = filters, + Nodes = (Node*)nodes.GetUnsafePtr(), + NodeFilters = null, + Ranges = ranges, + BranchNodeOffsets = branchNodeOffset, + BranchCount = branchCount + }; + + var task3 = new FinalizeTreeJob + { + Aabbs = aabbs, + Nodes = (Node*)nodes.GetUnsafePtr(), + BranchNodeOffsets = branchNodeOffset, + NumNodes = nodes.Length, + LeafFilters = filters, + BranchCount = branchCount + }; + + handle = task2.Schedule(Constants.MaxNumTreeBranches, 1, handle); + handle = task3.Schedule(handle); + handle.Complete(); + + var bvh = new BoundingVolumeHierarchy(nodes); + bvh.CheckIntegrity(); + + filters.Dispose(); + nodes.Dispose(); + ranges.Dispose(); + branchCount.Dispose(); + } + + [Test] + public unsafe void BuildTreeAndOverlapTasks([Values(2, 10, 33, 100)] int elementCount) + { + const int threadCount = 8; + elementCount *= 2; + int numNodes = elementCount + Constants.MaxNumTreeBranches; + + var points = new NativeArray(elementCount, Allocator.TempJob, NativeArrayOptions.UninitializedMemory); + var aabbs = new NativeArray(elementCount, Allocator.TempJob, NativeArrayOptions.UninitializedMemory); + var filters = new NativeArray(elementCount, Allocator.TempJob, NativeArrayOptions.UninitializedMemory); + var nodefilters = new NativeArray(numNodes, Allocator.TempJob, NativeArrayOptions.UninitializedMemory); + var branchCount = new NativeArray(1, Allocator.TempJob, NativeArrayOptions.UninitializedMemory); + + InitInputWithCopyArrays(points, aabbs, filters); + + // Override filter data with default filters. + for (int i = 0; i < filters.Length; i++) + { + filters[i] = CollisionFilter.Default; + } + + for (int i = 0; i < nodefilters.Length; i++) + { + nodefilters[i] = CollisionFilter.Default; + } + + var nodes = new NativeArray(numNodes, Allocator.TempJob, NativeArrayOptions.UninitializedMemory); + + var ranges = new NativeArray(Constants.MaxNumTreeBranches, Allocator.TempJob, NativeArrayOptions.UninitializedMemory); + var branchNodeOffset = new NativeArray(Constants.MaxNumTreeBranches, Allocator.TempJob, NativeArrayOptions.UninitializedMemory); + + JobHandle handle = new BuildFirstNLevelsJob + { + Points = points, + Nodes = (Node*)nodes.GetUnsafePtr(), + Ranges = ranges, + BranchNodeOffsets = branchNodeOffset, + BranchCount = branchCount, + ThreadCount = threadCount, + }.Schedule(); + + handle = new BuildBranchesJob + { + Points = points, + Aabbs = aabbs, + BodyFilters = filters, + Nodes = (Node*)nodes.GetUnsafePtr(), + Ranges = ranges, + BranchNodeOffsets = branchNodeOffset, + BranchCount = branchCount + }.Schedule(Constants.MaxNumTreeBranches, 1, handle); + + handle = new FinalizeTreeJob + { + Aabbs = aabbs, + LeafFilters = filters, + Nodes = (Node*)nodes.GetUnsafePtr(), + BranchNodeOffsets = branchNodeOffset, + NumNodes = numNodes, + BranchCount = branchCount + }.Schedule(handle); + + handle.Complete(); + + int numBranchOverlapPairs = branchCount[0] * (branchCount[0] + 1) / 2; + var nodePairIndices = new NativeArray(numBranchOverlapPairs, Allocator.TempJob, NativeArrayOptions.UninitializedMemory); + var collisionPairs = new BlockStream(numBranchOverlapPairs, 0xb08c3d78); + + handle = new Broadphase.DynamicVsDynamicBuildBranchNodePairsJob + { + Ranges = ranges, + NumBranches = branchCount[0], + NodePairIndices = nodePairIndices + }.Schedule(); + + handle = new Broadphase.DynamicVsDynamicFindOverlappingPairsJob + { + DynamicNodes = nodes, + PairWriter = collisionPairs, + BodyFilters = filters, + NodePairIndices = nodePairIndices, + DynamicNodeFilters = nodefilters, + }.Schedule(numBranchOverlapPairs, numBranchOverlapPairs, handle); + + handle.Complete(); + + int numPairs = collisionPairs.ComputeItemCount(); + + Debug.Log($"Num colliding pairs: {numPairs}"); + + var bvh = new BoundingVolumeHierarchy(nodes); + bvh.CheckIntegrity(); + + Assert.IsTrue(elementCount / 2 == numPairs); + + filters.Dispose(); + nodefilters.Dispose(); + nodes.Dispose(); + ranges.Dispose(); + collisionPairs.Dispose(); + branchCount.Dispose(); + } + + // Util writer which saves every body pair to an HashSet. + struct EverythingWriter : BoundingVolumeHierarchy.ITreeOverlapCollector + { + public void AddPairs(int l, int4 r, int countR) { AddPairs(new int4(l, l, l, l), r, countR); } + public void AddPairs(int4 pairLeft, int4 r, int count) + { + for (int i = 0; i < count; i++) + { + SeenPairs.Add(new BodyIndexPair { BodyAIndex = pairLeft[0], BodyBIndex = r[0] }); + } + } + public void FlushIfNeeded() { } + public HashSet SeenPairs; + } + + [Test] + public unsafe void OverlapTaskFilteringTest([Values(2, 10, 33, 100)] int elementCount) + { + elementCount *= 2; + int numNodes = elementCount + Constants.MaxNumTreeBranches; + + var points = new NativeArray(elementCount, Allocator.Temp, NativeArrayOptions.UninitializedMemory); + var aabbs = new NativeArray(elementCount, Allocator.Temp, NativeArrayOptions.UninitializedMemory); + var bodyFilters = new NativeArray(elementCount, Allocator.Temp, NativeArrayOptions.UninitializedMemory); + InitInputWithCopyArrays(points, aabbs, bodyFilters); + + var nodes = new NativeArray(numNodes, Allocator.Temp, NativeArrayOptions.UninitializedMemory); + Node* nodesPtr = (Node*)nodes.GetUnsafePtr(); + + var seenUnfiltered = new HashSet(); + { + var bvhUnfiltered = new BoundingVolumeHierarchy(nodes); + bvhUnfiltered.Build(points, aabbs, out int numNodesOut); + bvhUnfiltered.CheckIntegrity(); + + EverythingWriter pairWriter = new EverythingWriter { SeenPairs = seenUnfiltered }; + BoundingVolumeHierarchy.TreeOverlap(ref pairWriter, nodesPtr, nodesPtr); + } + + var nodeFilters = new NativeArray(numNodes, Allocator.Temp, NativeArrayOptions.UninitializedMemory); + var bvhFiltered = new BoundingVolumeHierarchy(nodes, nodeFilters); + int numNodesFilteredTree; + bvhFiltered.Build(points, aabbs, out numNodesFilteredTree); + bvhFiltered.BuildCombinedCollisionFilter(bodyFilters, 0, numNodesFilteredTree - 1); + + var filteredCollisionPairs = new BlockStream(1, 0xec87b613); + BlockStream.Writer filteredPairWriter = filteredCollisionPairs; + filteredPairWriter.BeginForEachIndex(0); + CollisionFilter* bodyFiltersPtr = (CollisionFilter*)bodyFilters.GetUnsafePtr(); + var bufferedPairs = new Broadphase.BodyPairWriter(ref filteredPairWriter, bodyFiltersPtr); + + CollisionFilter* nodeFiltersPtr = (CollisionFilter*)nodeFilters.GetUnsafePtr(); + BoundingVolumeHierarchy.TreeOverlap(ref bufferedPairs, nodesPtr, nodesPtr, nodeFiltersPtr, nodeFiltersPtr); + bufferedPairs.Close(); + filteredPairWriter.EndForEachIndex(); + + BlockStream.Reader filteredPairReader = filteredCollisionPairs; + filteredPairReader.BeginForEachIndex(0); + + // Check that every pair in our filtered set also appears in the unfiltered set + while (filteredPairReader.RemainingItemCount > 0) + { + var pair = filteredPairReader.Read(); + + Assert.IsTrue(seenUnfiltered.Contains(pair)); + seenUnfiltered.Remove(pair); // Remove the pair + } + + // Pairs were removed, so the only remaining ones should be filtered + foreach (BodyIndexPair pair in seenUnfiltered) + { + bool shouldCollide = CollisionFilter.IsCollisionEnabled(bodyFilters[pair.BodyAIndex], bodyFilters[pair.BodyBIndex]); + Assert.IsFalse(shouldCollide); + } + + nodeFilters.Dispose(); + nodes.Dispose(); + bodyFilters.Dispose(); + aabbs.Dispose(); + points.Dispose(); + filteredCollisionPairs.Dispose(); + } + + struct PairBuffer : BoundingVolumeHierarchy.ITreeOverlapCollector + { + public List Pairs; // TODO: use NativeList? + + public void AddPairs(int l, int4 r, int countR) + { + for (int i = 0; i < countR; i++) + { + Pairs.Add(new BodyIndexPair { BodyAIndex = l, BodyBIndex = r[i] }); + } + } + + public void AddPairs(int4 pairLeft, int4 r, int count) + { + for (int i = 0; i < count; i++) + { + Pairs.Add(new BodyIndexPair { BodyAIndex = pairLeft[i], BodyBIndex = r[i] }); + } + } + + public void FlushIfNeeded() + { + } + + public int MaxId; + } + + [Test] + public unsafe void BuildTreeAndOverlap([Values(2, 10, 33, 100)] int elementCount) + { + elementCount *= 2; + int numNodes = elementCount / 3 * 2 + 4; + var points = new NativeArray(elementCount, Allocator.Temp, NativeArrayOptions.UninitializedMemory); + var aabbs = new NativeArray(elementCount, Allocator.Temp, NativeArrayOptions.UninitializedMemory); + var filters = new NativeArray(elementCount, Allocator.Temp, NativeArrayOptions.UninitializedMemory); + + InitInputWithCopyArrays(points, aabbs, filters); + + var nodes = new NativeArray(numNodes, Allocator.Temp, NativeArrayOptions.UninitializedMemory); + + var bvh = new BoundingVolumeHierarchy(nodes); + bvh.Build(points, aabbs, out int numNodesOut); + bvh.CheckIntegrity(); + + var buffer = new PairBuffer { Pairs = new List() }; + buffer.MaxId = elementCount - 1; + + Node* nodesPtr = (Node*)nodes.GetUnsafePtr(); + BoundingVolumeHierarchy.TreeOverlap(ref buffer, nodesPtr, nodesPtr); + + int numCollidingPairs = buffer.Pairs.Count; + Debug.Log($"Num colliding pairs: {buffer.Pairs.Count}"); + Assert.IsTrue(elementCount / 2 == numCollidingPairs); + + filters.Dispose(); + points.Dispose(); + aabbs.Dispose(); + nodes.Dispose(); + } + + [BurstCompile(CompileSynchronously=true)] + struct TestTreeOverlapJob : IJob + { + public BlockStream.Writer CollisionPairWriter; + public NativeArray Nodes; + public NativeArray Filter; + public int NumObjects; + // If true, do no work in Execute() - allows us to get timings for a BurstCompiled + // run without profiling the overhead of the compiler + public bool DummyRun; + + public unsafe void Execute() + { + if (DummyRun) + { + return; + } + + CollisionPairWriter.BeginForEachIndex(0); + + CollisionFilter* bodyFilters = (CollisionFilter*)Filter.GetUnsafePtr(); + var pairBuffer = new Broadphase.BodyPairWriter(ref CollisionPairWriter, bodyFilters); + + Node* nodesPtr = (Node*)Nodes.GetUnsafePtr(); + BoundingVolumeHierarchy.TreeOverlap(ref pairBuffer, nodesPtr, nodesPtr); + + pairBuffer.Close(); + + CollisionPairWriter.EndForEachIndex(); + } + } + +#if UNITY_2019_2_OR_NEWER + [Test, Performance] +#else + [PerformanceTest] +#endif + [TestCase(100, true, TestName = "TreeOverlapPerfTest 200")] + [TestCase(1000, true, TestName = "TreeOverlapPerfTest 2000")] + public void TreeOverlapPerfTest(int elementCount, bool newOverlap) + { + // Execute dummy job just to get Burst compilation out of the way. + { + var dummyBlockStream = new BlockStream(1, 0, Allocator.TempJob); + var dummyNodes = new NativeArray(0, Allocator.TempJob); + var dummyFilters = new NativeArray(0, Allocator.TempJob); + new TestTreeOverlapJob + { + CollisionPairWriter = dummyBlockStream, + Nodes = dummyNodes, + Filter = dummyFilters, + NumObjects = 0, + DummyRun = true + }.Run(); + dummyBlockStream.Dispose(); + dummyNodes.Dispose(); + dummyFilters.Dispose(); + } + + elementCount *= 2; + int numNodes = elementCount / 3 * 2 + 4; + var points = new NativeArray(elementCount, Allocator.Temp, NativeArrayOptions.UninitializedMemory); + var aabbs = new NativeArray(elementCount, Allocator.Temp, NativeArrayOptions.UninitializedMemory); + var filters = new NativeArray(elementCount, Allocator.TempJob, NativeArrayOptions.UninitializedMemory); + + // Override filter data with default filters. + for (int i = 0; i < filters.Length; i++) + { + filters[i] = CollisionFilter.Default; + } + + InitInputWithCopyArrays(points, aabbs, filters); + + var nodes = new NativeArray(numNodes, Allocator.TempJob, NativeArrayOptions.UninitializedMemory); + + var bvh = new BoundingVolumeHierarchy(nodes); + bvh.Build(points, aabbs, out int numNodesOut); + bvh.CheckIntegrity(); + + var collisionPairs = new BlockStream(1, 0xd586fc6e); + + var job = new TestTreeOverlapJob + { + Nodes = nodes, + Filter = filters, + NumObjects = elementCount, + CollisionPairWriter = collisionPairs, + DummyRun = false + }; + + Measure.Method(() => + { + job.Run(); + }).Definition(sampleUnit: SampleUnit.Millisecond) + .MeasurementCount(1) + .Run(); + + points.Dispose(); + aabbs.Dispose(); + nodes.Dispose(); + collisionPairs.Dispose(); + filters.Dispose(); + } + } +} diff --git a/package/Tests/PlayModeTests/Collision/Geometry/BoundingVolumeHierarchyBuilderTests.cs.meta b/package/Tests/PlayModeTests/Collision/Geometry/BoundingVolumeHierarchyBuilderTests.cs.meta new file mode 100755 index 000000000..7d5883e0e --- /dev/null +++ b/package/Tests/PlayModeTests/Collision/Geometry/BoundingVolumeHierarchyBuilderTests.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 4c4570fb809717d40ad9e5a48ca53f12 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Tests/PlayModeTests/Collision/Geometry/ConvexHullTests.cs b/package/Tests/PlayModeTests/Collision/Geometry/ConvexHullTests.cs new file mode 100755 index 000000000..9563a15ef --- /dev/null +++ b/package/Tests/PlayModeTests/Collision/Geometry/ConvexHullTests.cs @@ -0,0 +1,28 @@ +using NUnit.Framework; +using Unity.Mathematics; +using UnityEngine; +using Assert = UnityEngine.Assertions.Assert; + +namespace Unity.Physics.Tests.Collision.Geometry +{ + public class ConvexHullTests + { + [Test] + public void BuildConvexHull2D() + { + // Build circle. + using (var hull = new ConvexHullBuilder(8192, 8192 * 2)) + { + var expectedCom = new float3(4, 5, 3); + for (int n = 1024, i = 0; i < n; ++i) + { + var angle = (float)(i / n * 2 * System.Math.PI); + hull.AddPoint(new float3(math.cos(angle), math.sin(angle), 0)); + } + var massProperties = hull.ComputeMassProperties(); + Debug.Log($"COM: {massProperties.CenterOfMass}"); + Debug.Log($"Area: {massProperties.SurfaceArea}"); + } + } + } +} diff --git a/package/Tests/PlayModeTests/Collision/Geometry/ConvexHullTests.cs.meta b/package/Tests/PlayModeTests/Collision/Geometry/ConvexHullTests.cs.meta new file mode 100755 index 000000000..e9619e16b --- /dev/null +++ b/package/Tests/PlayModeTests/Collision/Geometry/ConvexHullTests.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: a11bb53efcec04f488fc8161aa756b39 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Tests/PlayModeTests/Collision/Queries.meta b/package/Tests/PlayModeTests/Collision/Queries.meta new file mode 100755 index 000000000..6a5104496 --- /dev/null +++ b/package/Tests/PlayModeTests/Collision/Queries.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: 8bb4ea827cdb2fd4e8071f63251c04a0 +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Tests/PlayModeTests/Collision/Queries/ConvexConvexDistanceTests.cs b/package/Tests/PlayModeTests/Collision/Queries/ConvexConvexDistanceTests.cs new file mode 100755 index 000000000..91336ce19 --- /dev/null +++ b/package/Tests/PlayModeTests/Collision/Queries/ConvexConvexDistanceTests.cs @@ -0,0 +1,884 @@ +using System; +using System.Collections.Generic; +using System.Diagnostics; +using System.Runtime.CompilerServices; +using Unity.Mathematics; +using UnityEditor; +using UnityEngine; +using static Unity.Physics.Math; +using Debug = UnityEngine.Debug; + +namespace Unity.Physics.Tests.Collision.Queries +{ + public unsafe class ConvexConvexDistanceTest : MonoBehaviour + { + public enum DistributionType + { + Box, + Sphere + } + + public struct Face + { + public Plane Plane; + public int FirstVertex; + public int NumVertices; + } + + public struct VertexFaces + { + public int FirstFace; + public int NumFaces; + } + + public struct HullFaceData + { + public Face[] Faces; + public int[] FaceVertices; + + public VertexFaces[] VertexFaces; + public int[] FaceIndices; + } + + /// + /// KISS Based PRNG (http://www.cs.ucl.ac.uk/staff/d.jones/GoodPracticeRNG.pdf) + /// + public struct PseudoRandomNumberGenerator + { + public PseudoRandomNumberGenerator(UInt32 seed) + { + m_x = 123456789; + m_y = seed == 0 ? 234567891 : seed; + m_z = 345678912; + m_w = 456789123; + m_c = 0; + } + + public UInt32 NextUInt32() + { + m_y ^= (m_y << 5); + m_y ^= (m_y >> 7); + m_y ^= (m_y << 22); + Int32 t = (Int32)(m_z + m_w + m_c); + m_z = m_w; + m_c = (UInt32)(t < 0 ? 1 : 0); + m_w = (UInt32)(t & 2147483647); + m_x += 1411392427; + return m_x + m_y + m_w; + } + + public float NextFloat() + { + return NextUInt32() * (1.0f / 4294967296.0f); + } + + public float NextFloat(float center, float halfExtent) + { + return (NextFloat() * 2 - 1) * halfExtent + center; + } + + public float2 NextFloat2(float center = 0, float halfExtent = 1) + { + return new float2(NextFloat(center, halfExtent), NextFloat(center, halfExtent)); + } + + public float3 NextFloat3(float center = 0, float halfExtent = 1) + { + return new float3(NextFloat2(center, halfExtent), NextFloat(center, halfExtent)); + } + + public float4 NextFloat4(float center = 0, float halfExtent = 1) + { + return new float4(NextFloat3(center, halfExtent), NextFloat(center, halfExtent)); + } + + UInt32 m_x, m_y, m_z, m_w, m_c; + } + + public ConvexHullBuilder Hull = new ConvexHullBuilder(16384, 16384 * 2); + public HullFaceData HullData; + + public ConvexHullBuilder.MassProperties MassProperties; + public UnityEngine.Mesh SourceMesh = null; + public bool UpdateMesh; + public bool CollideOthers = false; + internal ConvexConvexDistanceQueries.PenetrationHandling PenetrationHandling = ConvexConvexDistanceQueries.PenetrationHandling.Exact3D; + public bool ShowCso = false; + public bool ShowFaces = false; + public bool ShowTriangles = false; + public bool ShowVertexNormals = false; + public bool ShowProjection = true; + public bool ShowManifold = false; + public bool ShowLabels = false; + public bool Experimental = false; + public bool TraceQueryResults = false; + public float FaceSimplificationError = 1; + public float FaceMinAngle = 0; + public float VertexSimplificationError = 1; + public float MaxFaceAngle = 0.1f; + public int MinVertices = 0; + public int MaxFaces = 16; + public float VolumeConservation = 1; + public float Offset = 0; + public DistributionType Distribution = DistributionType.Box; + public PseudoRandomNumberGenerator Prng = new PseudoRandomNumberGenerator(0); + + // Use this for initialization + void Start() + { + GetComponent().sharedMesh = new UnityEngine.Mesh(); + UpdateMesh = true; + } + + // Update is called once per frame + void Update() + { +#if UNITY_EDITOR + HandleUtility.Repaint(); +#endif + } + + public void Reset() + { + Prng = new PseudoRandomNumberGenerator(0); + UpdateMesh = false; + Hull.Reset(); + HullData.Faces = null; + HullData.VertexFaces = null; + GetComponent().sharedMesh = new UnityEngine.Mesh(); + } + + public float3[] GetVertexArray() + { + var vtx = new List(); + foreach (var vertex in Hull.Vertices.Elements) + { + vtx.Add(vertex.Position); + } + if (vtx.Count == 0) + { + vtx.Add(float3.zero); + } + return vtx.ToArray(); + } + + public void AddRandomPoints(int count) + { + for (int i = 0; i < count; ++i) + { + var point = Prng.NextFloat3(0, 1); + if (Distribution == DistributionType.Sphere) + { + point = math.normalize(point); + } + Hull.AddPoint(point); + } + UpdateMesh = true; + } + + public void Scale(float3 factors) + { + var points = new List(); + foreach (var vertex in Hull.Vertices.Elements) + { + points.Add(vertex.Position * factors); + } + Hull.Reset(); + foreach (var p in points) + { + Hull.AddPoint(p); + } + UpdateMesh = true; + } + + public void SplitByPlane(Plane plane) + { + plane.Distance = -math.dot(plane.Normal, MassProperties.CenterOfMass); + + ConvexHullBuilder neg, pos; + Hull.SplitByPlane(plane, out neg, out pos, 0); + Hull.CopyFrom(pos); + neg.Dispose(); + pos.Dispose(); + UpdateMesh = true; + } + + private void UpdateMeshNow() + { + MassProperties = Hull.ComputeMassProperties(); + + Hull.CompactVertices(); + Hull.BuildFaceIndices((float)(MaxFaceAngle * System.Math.PI / 180)); + + { + HullData.Faces = new Face[Hull.NumFaces]; + HullData.FaceVertices = new int[Hull.NumFaceVertices]; + int nextVertex = 0; + for (var faceEdge = Hull.GetFirstFace(); faceEdge.IsValid; faceEdge = Hull.GetNextFace(faceEdge)) + { + var triangleIndex = ((ConvexHullBuilder.Edge)faceEdge).TriangleIndex; + Face newFace = new Face + { + Plane = Hull.ComputePlane(triangleIndex), + FirstVertex = nextVertex, + NumVertices = 0 + }; + for (var edge = faceEdge; edge.IsValid; edge = Hull.GetNextFaceEdge(edge)) + { + HullData.FaceVertices[nextVertex++] = Hull.StartVertex(edge); + } + newFace.NumVertices = nextVertex - newFace.FirstVertex; + HullData.Faces[Hull.Triangles[triangleIndex].FaceIndex] = newFace; + } + + var indices = new List(); + var set = new List(); + HullData.VertexFaces = new VertexFaces[Hull.Vertices.PeakCount]; + for (int i = 0; i < Hull.Vertices.PeakCount; ++i) + { + var cardinality = Hull.Vertices[i].Cardinality; + var edge = Hull.GetVertexEdge(i); + for (int j = Hull.Vertices[i].Cardinality; j > 0; --j) + { + int faceIndex = Hull.Triangles[edge.TriangleIndex].FaceIndex; + if (set.IndexOf(faceIndex) == -1) set.Add(faceIndex); + edge = Hull.GetLinkedEdge(edge).Next; + } + set.Sort(); + HullData.VertexFaces[i] = new VertexFaces { FirstFace = indices.Count, NumFaces = set.Count }; + indices.AddRange(set); + set.Clear(); + } + HullData.FaceIndices = indices.ToArray(); + } + + Vector3[] vertices = null; + int[] triangles = null; + switch (Hull.Dimension) + { + case 2: + vertices = new Vector3[Hull.Vertices.PeakCount]; + triangles = new int[(Hull.Vertices.PeakCount - 2) * 2 * 3]; + for (int i = 0; i < Hull.Vertices.PeakCount; ++i) + { + vertices[i] = Hull.Vertices[i].Position; + } + for (int i = 2; i < Hull.Vertices.PeakCount; ++i) + { + int j = (i - 2) * 6; + triangles[j + 0] = 0; triangles[j + 1] = i - 1; triangles[j + 2] = i; + triangles[j + 3] = 0; triangles[j + 4] = i; triangles[j + 5] = i - 1; + } + break; + case 3: + vertices = new Vector3[Hull.Triangles.PeakCount * 3]; + triangles = new int[Hull.Triangles.PeakCount * 3]; + for (int i = 0; i < Hull.Triangles.PeakCount; ++i) + { + if (Hull.Triangles[i].IsAllocated) + { + vertices[i * 3 + 0] = Hull.Vertices[Hull.Triangles[i].Vertex0].Position; + vertices[i * 3 + 1] = Hull.Vertices[Hull.Triangles[i].Vertex1].Position; + vertices[i * 3 + 2] = Hull.Vertices[Hull.Triangles[i].Vertex2].Position; + triangles[i * 3 + 0] = i * 3 + 0; + triangles[i * 3 + 1] = i * 3 + 1; + triangles[i * 3 + 2] = i * 3 + 2; + } + else + { + triangles[i * 3 + 0] = 0; + triangles[i * 3 + 1] = 0; + triangles[i * 3 + 2] = 0; + } + } + break; + } + + var mesh = GetComponent().sharedMesh; + + mesh.Clear(); + mesh.vertices = vertices; + mesh.triangles = triangles; + mesh.RecalculateBounds(); + mesh.RecalculateNormals(); + mesh.RecalculateTangents(); + } + + static Face SelectBestFace(Face[] faces, float3 direction) + { + var bestDot = math.dot(faces[0].Plane.Normal, direction); + var bestFace = 0; + for (int i = 1; i < faces.Length; ++i) + { + var d = math.dot(faces[i].Plane.Normal, direction); + if (d > bestDot) + { + bestDot = d; + bestFace = i; + } + } + return faces[bestFace]; + } + + static Face SelectBestFace(List subset, Face[] faces, float3 direction) + { + var bestDot = math.dot(faces[subset[0]].Plane.Normal, direction); + var bestFace = subset[0]; + for (int i = 1; i < subset.Count; ++i) + { + var d = math.dot(faces[subset[i]].Plane.Normal, direction); + if (d > bestDot) + { + bestDot = d; + bestFace = subset[i]; + } + } + return faces[bestFace]; + } + + static List GetPerVertexFaces(int vertex, ref HullFaceData data) + { + var set = new List(); + VertexFaces vf = data.VertexFaces[vertex]; + for (int j = 0; j < vf.NumFaces; ++j) set.Add(data.FaceIndices[vf.FirstFace + j]); + return set; + } + + static void DrawFace(MTransform trs, ref ConvexHullBuilder hull, Face face, int[] vertices, float offset, Color color) + { + Gizmos.color = color; + var translation = face.Plane.Normal * offset; + for (int i = face.NumVertices - 1, j = 0; j < face.NumVertices; i = j++) + { + var a = hull.Vertices[vertices[face.FirstVertex + i]].Position; + var b = hull.Vertices[vertices[face.FirstVertex + j]].Position; + a = Mul(trs, a + translation); + b = Mul(trs, b + translation); + Gizmos.DrawLine(a, b); + } + } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + static float DistanceToPlaneEps(float4 plane, float3 point, float eps) + { + var d2P = math.dot(plane, new float4(point, 1)); + if ((d2P * d2P) <= eps) + return 0; + return d2P; + } + + static void DrawManifold(bool useExerimentalMethod, ConvexConvexDistanceQueries.Result distance, + MTransform trsA, ref ConvexHullBuilder hullA, ref HullFaceData dataA, + MTransform trsB, ref ConvexHullBuilder hullB, ref HullFaceData dataB) + { + MTransform btoA = Mul(Inverse(trsA), trsB); + MTransform atoB = Mul(Inverse(trsB), trsA); + + Face faceA = SelectBestFace(dataA.Faces, distance.ClosestPoints.NormalInA); + Face faceB = SelectBestFace(dataB.Faces, math.mul(atoB.Rotation, -distance.ClosestPoints.NormalInA)); + if (useExerimentalMethod) + { + var legacyFaceA = faceA; + var legacyFaceB = faceB; + + // Experimental method. + + // Extract faces sub-set of A. + { + float bestDot = -2; + var normal = distance.ClosestPoints.NormalInA; + for (int i = 0; i < distance.SimplexDimension; ++i) + { + int vi = distance.SimplexVertexA(i); + for (int j = 0; j < dataA.VertexFaces[vi].NumFaces; ++j) + { + var k = dataA.FaceIndices[dataA.VertexFaces[vi].FirstFace + j]; + var face = dataA.Faces[k]; + var d = math.dot(face.Plane.Normal, normal); + if (d > bestDot) + { + faceA = face; + bestDot = d; + } + } + } + } + + // Extract faces sub-set of B. + { + float bestDot = -2; + var normal = math.mul(atoB.Rotation, -distance.ClosestPoints.NormalInA); + for (int i = 0; i < distance.SimplexDimension; ++i) + { + int vi = distance.SimplexVertexB(i); + for (int j = 0; j < dataB.VertexFaces[vi].NumFaces; ++j) + { + var k = dataB.FaceIndices[dataB.VertexFaces[vi].FirstFace + j]; + var face = dataB.Faces[k]; + var d = math.dot(face.Plane.Normal, normal); + if (d > bestDot) + { + faceB = face; + bestDot = d; + } + } + } + } + + if (legacyFaceA.FirstVertex != faceA.FirstVertex || legacyFaceB.FirstVertex != faceB.FirstVertex) + { + Debug.LogError("Different face set found."); + } + } + + var facePlaneAinA = faceA.Plane; + var facePlaneBinA = TransformPlane(btoA, faceB.Plane); + + /*drawFace(trsA, ref hullA, faceA, dataA.faceVertices, 0.01f, Color.yellow); + drawFace(trsB, ref hullB, faceB, dataB.faceVertices, 0.01f, Color.yellow);*/ + + const float eps = 1e-6f; + var va = new List(); + var vb = new List(); + for (int i = 0; i < faceA.NumVertices; ++i) + { + va.Add(hullA.Vertices[dataA.FaceVertices[faceA.FirstVertex + i]].Position); + } + for (int i = 0; i < faceB.NumVertices; ++i) + { + vb.Add(Mul(btoA, hullB.Vertices[dataB.FaceVertices[faceB.FirstVertex + i]].Position)); + } + + var vt = new List(); + for (int ai = va.Count - 1, aj = 0; aj < va.Count; ai = aj++) + { + var plane = new float4(math.normalize(math.cross(distance.ClosestPoints.NormalInA, va[ai] - va[aj])), 0); + plane.w = -math.dot(plane.xyz, va[ai]); + + if (vb.Count > 1) + { + int bi = vb.Count - 1; + float d2Pi = DistanceToPlaneEps(plane, vb[bi], eps); + for (int bj = 0; bj < vb.Count; bi = bj++) + { + var d2Pj = DistanceToPlaneEps(plane, vb[bj], eps); + if ((d2Pi * d2Pj) < 0) + { + float isec = d2Pi / (d2Pi - d2Pj); + vt.Add(vb[bi] + (vb[bj] - vb[bi]) * isec); + } + + if (d2Pj <= 0) vt.Add(vb[bj]); + + d2Pi = d2Pj; + } + } + + var temp = vb; + vb = vt; + vt = temp; + vt.Clear(); + } + + if (vb.Count == 0) + { + vb.Add(distance.ClosestPoints.PositionOnBinA); + } + + { + GUIStyle labelStyle = new GUIStyle(); + labelStyle.fontSize = 16; + + var projectionInvDen = 1 / math.dot(distance.ClosestPoints.NormalInA, facePlaneAinA.Normal); + + int i = vb.Count - 1; + var piOnB = vb[i]; + var piD = math.dot(facePlaneAinA, new float4(piOnB, 1)) * projectionInvDen; + var piOnA = piOnB - distance.ClosestPoints.NormalInA * piD; + for (int j = 0; j < vb.Count; i = j++) + { +#if UNITY_EDITOR + var center = Mul(trsA, (piOnA + piOnB) / 2); + Handles.Label(center, $"{piD}", labelStyle); +#endif + var pjOnB = vb[j]; + var pjD = math.dot(facePlaneAinA, new float4(pjOnB, 1)) * projectionInvDen; + var pjOnA = pjOnB - distance.ClosestPoints.NormalInA * pjD; + + Gizmos.DrawLine(Mul(trsA, piOnB), Mul(trsA, pjOnB)); + Gizmos.DrawLine(Mul(trsA, piOnA), Mul(trsA, pjOnA)); + + Gizmos.DrawLine(Mul(trsA, pjOnB), Mul(trsA, pjOnA)); + + piOnB = pjOnB; + piOnA = pjOnA; + piD = pjD; + } + } + } + + void OnDrawGizmos() + { + { + var s = 0.25f; + var com = MassProperties.CenterOfMass; + Gizmos.color = Color.white; + Gizmos.DrawLine(transform.TransformPoint(com + new float3(-s, 0, 0)), transform.TransformPoint(com + new float3(+s, 0, 0))); + Gizmos.DrawLine(transform.TransformPoint(com + new float3(0, -s, 0)), transform.TransformPoint(com + new float3(0, +s, 0))); + Gizmos.DrawLine(transform.TransformPoint(com + new float3(0, 0, -s)), transform.TransformPoint(com + new float3(0, 0, +s))); + } + + if (UpdateMesh) + { + UpdateMesh = false; + UpdateMeshNow(); + } + + // Display faces. + if (ShowFaces && HullData.Faces != null) + { + MTransform trs = new MTransform(transform.rotation, transform.position); + + Gizmos.color = Color.white; + foreach (Face face in HullData.Faces) + { + var offset = face.Plane.Normal * 0.0001f; + for (int i = face.NumVertices - 1, j = 0; j < face.NumVertices; i = j++) + { + var a = Hull.Vertices[HullData.FaceVertices[face.FirstVertex + i]].Position; + var b = Hull.Vertices[HullData.FaceVertices[face.FirstVertex + j]].Position; + a = Mul(trs, a + offset); + b = Mul(trs, b + offset); + Gizmos.DrawLine(a, b); + } + } + } + + // Display triangles. + if (ShowTriangles) + { + MTransform trs = new MTransform(transform.rotation, transform.position); + Gizmos.color = Color.white; + for (int i = Hull.Triangles.GetFirstIndex(); i != -1; i = Hull.Triangles.GetNextIndex(i)) + { + var a = Mul(trs, Hull.Vertices[Hull.Triangles[i].Vertex0].Position); + var b = Mul(trs, Hull.Vertices[Hull.Triangles[i].Vertex1].Position); + var c = Mul(trs, Hull.Vertices[Hull.Triangles[i].Vertex2].Position); + Gizmos.DrawLine(a, b); + Gizmos.DrawLine(b, c); + Gizmos.DrawLine(c, a); + } + } + + // Display vertex normals. + if (ShowVertexNormals) + { + MTransform trs = new MTransform(transform.rotation, transform.position); + Gizmos.color = Color.white; + for (var vertex = Hull.Triangles.GetFirstIndex(); vertex != -1; vertex = Hull.Triangles.GetNextIndex(vertex)) + { + var normal = math.mul(trs.Rotation, Hull.ComputeVertexNormal(vertex)) * 0.25f; + var start = Mul(trs, Hull.Vertices[vertex].Position); + Gizmos.DrawRay(start, normal); + } + } + + // Display labels. + if (ShowLabels) + { + } + + // Compute distance to every other convex hulls. + if (CollideOthers) + { + var thisId = GetInstanceID(); + var cvxs = FindObjectsOfType(); + + MTransform transformA = new MTransform(transform.rotation, transform.position); + float3[] verticesA = GetVertexArray(); + foreach (var cvx in cvxs) + { + if (cvx.GetInstanceID() == thisId) continue; + + MTransform transformB = new MTransform(cvx.transform.rotation, cvx.transform.position); + float3[] verticesB = cvx.GetVertexArray(); + + MTransform btoA = Mul(Inverse(transformA), transformB); + + ConvexConvexDistanceQueries.Result result; + fixed (float3* va = verticesA) + { + fixed (float3* vb = verticesB) + { + result = ConvexConvexDistanceQueries.ConvexConvex(va, verticesA.Length, vb, verticesB.Length, btoA, PenetrationHandling); + } + } + + var from = Mul(transformA, result.ClosestPoints.PositionOnAinA); + var to = Mul(transformA, result.ClosestPoints.PositionOnBinA); + + if (TraceQueryResults) + { + Debug.Log($"Iterations={result.Iterations}, plane={result.ClosestPoints.NormalInA}, distance={result.ClosestPoints.Distance}"); + Debug.Log($"Features A = [{result.Simplex[0] >> 16}, {result.Simplex[1] >> 16}, {result.Simplex[2] >> 16}]"); + Debug.Log($"Features B = [{result.Simplex[0] & 0xffff}, {result.Simplex[1] & 0xffff}, {result.Simplex[2] & 0xffff}]"); + } + + if (ShowManifold && Hull.Dimension == 3 && cvx.Hull.Dimension == 3) + { + DrawManifold(Experimental, result, + transformA, ref Hull, ref HullData, + transformB, ref cvx.Hull, ref cvx.HullData); + } + + if (ShowProjection) + { + Gizmos.color = Color.white; + var trs = Mul(transformA, new MTransform(float3x3.identity, result.ClosestPoints.NormalInA * result.ClosestPoints.Distance)); + { + var tv = stackalloc float3[Hull.Vertices.PeakCount]; + for (var vertex = Hull.Vertices.GetFirstIndex(); vertex != -1; vertex = Hull.Vertices.GetNextIndex(vertex)) + { + tv[vertex] = Mul(trs, Hull.Vertices[vertex].Position); + } + + if (Hull.Dimension == 3) + { + for (var edge = Hull.GetFirstPrimaryEdge(); edge.IsValid; edge = Hull.GetNextPrimaryEdge(edge)) + { + Gizmos.DrawLine(tv[Hull.StartVertex(edge)], tv[Hull.EndVertex(edge)]); + } + } + else if (Hull.Dimension >= 1) + { + for (int i = Hull.Vertices.PeakCount - 1, j = 0; j < Hull.Vertices.PeakCount; i = j++) + { + Gizmos.DrawLine(tv[i], tv[j]); + } + } + } + } + + Gizmos.color = Color.red; + Gizmos.DrawSphere(from, 0.05f); + + Gizmos.color = Color.green; + Gizmos.DrawSphere(to, 0.05f); + + Gizmos.color = Color.white; + Gizmos.DrawLine(from, to); + + if (ShowCso) + { + Gizmos.color = Color.yellow; + + using (var cso = new ConvexHullBuilder(8192, 8192 * 2)) + { + for (int i = 0; i < verticesA.Length; ++i) + { + for (int j = 0; j < verticesB.Length; ++j) + { + cso.AddPoint(verticesA[i] - Mul(btoA, verticesB[j])); + } + } + if (cso.Dimension == 2) + { + for (int n = cso.Vertices.PeakCount, i = n - 1, j = 0; j < n; i = j++) + { + Gizmos.DrawLine(cso.Vertices[i].Position, cso.Vertices[j].Position); + } + } + else if (cso.Dimension == 3) + { + foreach (var triangle in cso.Triangles.Elements) + { + Gizmos.DrawLine(cso.Vertices[triangle.Vertex0].Position, cso.Vertices[triangle.Vertex1].Position); + Gizmos.DrawLine(cso.Vertices[triangle.Vertex1].Position, cso.Vertices[triangle.Vertex2].Position); + Gizmos.DrawLine(cso.Vertices[triangle.Vertex2].Position, cso.Vertices[triangle.Vertex0].Position); + } + } + Gizmos.DrawLine(new float3(-0.1f, 0, 0), new float3(+0.1f, 0, 0)); + Gizmos.DrawLine(new float3(0, -0.1f, 0), new float3(0, +0.1f, 0)); + Gizmos.DrawLine(new float3(0, 0, -0.1f), new float3(0, 0, +0.1f)); + } + } + } + } + + // Draw vertices. +#if UNITY_EDITOR + GUIStyle labelStyle = new GUIStyle(); + labelStyle.fontSize = 24; +#endif + + Gizmos.color = Color.yellow; + for (int i = Hull.Vertices.GetFirstIndex(); i != -1; i = Hull.Vertices.GetNextIndex(i)) + { + var w = transform.TransformPoint(Hull.Vertices[i].Position); +#if UNITY_EDITOR + if (ShowLabels) + { + Handles.color = Color.white; + Handles.Label(w, $"{i}:{Hull.Vertices[i].Cardinality}", labelStyle); + } + else +#endif + { + Gizmos.DrawSphere(w, 0.01f); + } + } + } + } + +#if UNITY_EDITOR + [CustomEditor(typeof(ConvexConvexDistanceTest))] + public class ConvexTestEditor : UnityEditor.Editor + { + public override void OnInspectorGUI() + { + DrawDefaultInspector(); + + ConvexConvexDistanceTest cvx = (ConvexConvexDistanceTest)target; + + + GUILayout.BeginHorizontal(); + if (GUILayout.Button("Reset")) cvx.Reset(); + if (GUILayout.Button("Rebuild faces")) cvx.UpdateMesh = true; + GUILayout.EndHorizontal(); + + GUILayout.BeginHorizontal(); + if (GUILayout.Button("Simplify vertices")) + { + cvx.Hull.SimplifyVertices(cvx.VertexSimplificationError, cvx.MinVertices, cvx.VolumeConservation); + cvx.UpdateMesh = true; + } + if (GUILayout.Button("Simplify faces")) + { + cvx.Hull.SimplifyFaces(cvx.MaxFaces, cvx.FaceSimplificationError, cvx.FaceMinAngle); + cvx.UpdateMesh = true; + } + if (GUILayout.Button("Offset vertices")) + { + cvx.Hull.OffsetVertices(cvx.Offset); + cvx.UpdateMesh = true; + } + GUILayout.EndHorizontal(); + + GUILayout.BeginHorizontal(); + if (GUILayout.Button("X*2")) cvx.Scale(new float3(2, 1, 1)); + if (GUILayout.Button("Y*2")) cvx.Scale(new float3(1, 2, 1)); + if (GUILayout.Button("Z*2")) cvx.Scale(new float3(1, 1, 2)); + GUILayout.EndHorizontal(); + + GUILayout.BeginHorizontal(); + if (GUILayout.Button("X/2")) cvx.Scale(new float3(0.5f, 1, 1)); + if (GUILayout.Button("Y/2")) cvx.Scale(new float3(1, 0.5f, 1)); + if (GUILayout.Button("Z/2")) cvx.Scale(new float3(1, 1, 0.5f)); + GUILayout.EndHorizontal(); + + GUILayout.BeginHorizontal(); + if (GUILayout.Button("Cut X-")) cvx.SplitByPlane(new Plane(new float3(1, 0, 0), 0)); + if (GUILayout.Button("Cut Y-")) cvx.SplitByPlane(new Plane(new float3(0, 1, 0), 0)); + if (GUILayout.Button("Cut Z-")) cvx.SplitByPlane(new Plane(new float3(0, 0, 1), 0)); + GUILayout.EndHorizontal(); + + GUILayout.BeginHorizontal(); + if (GUILayout.Button("Cut X+")) cvx.SplitByPlane(new Plane(new float3(-1, 0, 0), 0)); + if (GUILayout.Button("Cut Y+")) cvx.SplitByPlane(new Plane(new float3(0, -1, 0), 0)); + if (GUILayout.Button("Cut Z+")) cvx.SplitByPlane(new Plane(new float3(0, 0, -1), 0)); + GUILayout.EndHorizontal(); + + GUILayout.BeginHorizontal(); + if (GUILayout.Button("+1 point")) cvx.AddRandomPoints(1); + if (GUILayout.Button("+10 point")) cvx.AddRandomPoints(10); + if (GUILayout.Button("+100 point")) cvx.AddRandomPoints(100); + if (GUILayout.Button("+1000 point")) cvx.AddRandomPoints(1000); + if (GUILayout.Button("Debug insert")) + { + cvx.Hull.AddPoint(new float3(0, 1, 0), 16384); + cvx.UpdateMesh = true; + } + GUILayout.EndHorizontal(); + + GUILayout.BeginHorizontal(); + if (cvx.SourceMesh != null && GUILayout.Button("Mesh")) + { + cvx.Reset(); + var vertices = cvx.SourceMesh.vertices; + var sw = new Stopwatch(); + sw.Start(); + + Aabb aabb = Aabb.Empty; + for (int i = 0; i < vertices.Length; ++i) + { + aabb.Include(vertices[i]); + } + cvx.Hull.IntegerSpaceAabb = aabb; + + for (int i = 0; i < vertices.Length; ++i) + { + cvx.Hull.AddPoint(vertices[i]); + } + Debug.Log($"Build time {sw.ElapsedMilliseconds} ms"); + cvx.UpdateMesh = true; + } + if (GUILayout.Button("Box")) + { + cvx.Reset(); + cvx.Hull.AddPoint(new float3(-1, -1, -1)); + cvx.Hull.AddPoint(new float3(+1, -1, -1)); + cvx.Hull.AddPoint(new float3(+1, +1, -1)); + cvx.Hull.AddPoint(new float3(-1, +1, -1)); + cvx.Hull.AddPoint(new float3(-1, -1, +1)); + cvx.Hull.AddPoint(new float3(+1, -1, +1)); + cvx.Hull.AddPoint(new float3(+1, +1, +1)); + cvx.Hull.AddPoint(new float3(-1, +1, +1)); + cvx.UpdateMesh = true; + } + if (GUILayout.Button("Cylinder")) + { + cvx.Reset(); + var pi2 = math.acos(0) * 4; + for (int i = 0, n = 32; i < n; ++i) + { + var angle = pi2 * i / n; + var xy = new float2(math.sin(angle), math.cos(angle)); + cvx.Hull.AddPoint(new float3(xy, -1)); + cvx.Hull.AddPoint(new float3(xy, +1)); + } + cvx.UpdateMesh = true; + } + + if (GUILayout.Button("Cone")) + { + cvx.Reset(); + var pi2 = math.acos(0) * 4; + for (int i = 0, n = 32; i < n; ++i) + { + var angle = pi2 * i / n; + var xy = new float2(math.sin(angle), math.cos(angle)); + cvx.Hull.AddPoint(new float3(xy, 0)); + } + cvx.Hull.AddPoint(new float3(0, 0, 1)); + cvx.UpdateMesh = true; + } + + if (GUILayout.Button("Circle")) + { + cvx.Reset(); + var pi2 = math.acos(0) * 4; + for (int i = 0, n = 32; i < n; ++i) + { + var angle = pi2 * i / n; + var xy = new float2(math.sin(angle), math.cos(angle)); + cvx.Hull.AddPoint(new float3(xy, 0)); + } + cvx.UpdateMesh = true; + } + GUILayout.EndHorizontal(); + + SceneView.RepaintAll(); + } + } +#endif +} diff --git a/package/Tests/PlayModeTests/Collision/Queries/ConvexConvexDistanceTests.cs.meta b/package/Tests/PlayModeTests/Collision/Queries/ConvexConvexDistanceTests.cs.meta new file mode 100755 index 000000000..c8828e64b --- /dev/null +++ b/package/Tests/PlayModeTests/Collision/Queries/ConvexConvexDistanceTests.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 5b350b9a54a7706408290e956a3cab7e +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Tests/PlayModeTests/Collision/Queries/QueryTests.cs b/package/Tests/PlayModeTests/Collision/Queries/QueryTests.cs new file mode 100755 index 000000000..cb128ba90 --- /dev/null +++ b/package/Tests/PlayModeTests/Collision/Queries/QueryTests.cs @@ -0,0 +1,680 @@ +using NUnit.Framework; +using Unity.Mathematics; +using Unity.Collections; +using Random = Unity.Mathematics.Random; +using static Unity.Physics.Math; +using Unity.Physics.Tests.Utils; + +namespace Unity.Physics.Tests.Collision.Queries +{ + public class QueryTests + { + // These tests mostly work by comparing the results of different methods of calculating the same thing. + // The results will not be exactly the same due to floating point inaccuracy, approximations in methods like convex-convex collider cast, etc. + const float tolerance = 1e-3f; + + // + // Query result validation + // + + static unsafe float3 getSupport(ref ConvexHull hull, float3 direction) + { + float4 best = new float4(hull.Vertices[0], math.dot(hull.Vertices[0], direction)); + for (int i = 1; i < hull.Vertices.Length; i++) + { + float dot = math.dot(hull.Vertices[i], direction); + best = math.select(best, new float4(hull.Vertices[i], dot), dot > best.w); + } + return best.xyz; + } + + static void ValidateDistanceResult(DistanceQueries.Result result, ref ConvexHull a, ref ConvexHull b, MTransform aFromB, float referenceDistance, string failureMessage) + { + float adjustedTolerance = tolerance; + if (result.Distance < a.ConvexRadius + b.ConvexRadius) + { + // Core shape penetration distances are less accurate, see stopThreshold in ConvexConvexDistanceQueries + // Note that the maximum possible inaccuracy scales with the number of vertices, so this may need to be tuned if the tests change + // For rigid body simulation the inaccuracy is generally not noticeable + adjustedTolerance *= 20.0f; + } + + // Check that the distances is correct + Assert.AreEqual(result.Distance, referenceDistance, adjustedTolerance, failureMessage + ": incorrect distance"); + + // Check that the separating normal and closest point are consistent with the distance + float3 tempA = getSupport(ref a, -result.NormalInA); + float3 supportQuery = tempA - result.NormalInA * a.ConvexRadius; + float3 tempB = Math.Mul(aFromB, getSupport(ref b, math.mul(aFromB.InverseRotation, result.NormalInA))); + float3 supportTarget = tempB + result.NormalInA * b.ConvexRadius; + + float supportQueryDot = math.dot(supportQuery, result.NormalInA); + float supportTargetDot = math.dot(supportTarget, result.NormalInA); + float supportDistance = supportQueryDot - supportTargetDot; + Assert.AreEqual(result.Distance, supportDistance, adjustedTolerance, failureMessage + ": incorrect normal"); + + float positionDot = math.dot(result.PositionOnAinA, result.NormalInA); + Assert.AreEqual(supportQueryDot, positionDot, adjustedTolerance, failureMessage + ": incorrect position"); + } + + // + // Reference implementations of queries using simple brute-force methods + // + + static unsafe float RefConvexConvexDistance(ref ConvexHull a, ref ConvexHull b, MTransform aFromB) + { + // Build the minkowski difference in a-space + int maxNumVertices = a.NumVertices * b.NumVertices; + ConvexHullBuilder diff = new ConvexHullBuilder(maxNumVertices, 2 * maxNumVertices, Allocator.Temp); + bool success = true; + Aabb aabb = Aabb.Empty; + for (int iB = 0; iB < b.NumVertices; iB++) + { + float3 vertexB = Math.Mul(aFromB, b.Vertices[iB]); + for (int iA = 0; iA < a.NumVertices; iA++) + { + float3 vertexA = a.Vertices[iA]; + aabb.Include(vertexA - vertexB); + } + } + diff.IntegerSpaceAabb = aabb; + for (int iB = 0; iB < b.NumVertices; iB++) + { + float3 vertexB = Math.Mul(aFromB, b.Vertices[iB]); + for (int iA = 0; iA < a.NumVertices; iA++) + { + float3 vertexA = a.Vertices[iA]; + if (!diff.AddPoint(vertexA - vertexB, (uint)(iA | iB << 16))) + { + // TODO - coplanar vertices are tripping up ConvexHullBuilder, we should fix it but for now fall back to DistanceQueries.ConvexConvex() + success = false; + } + } + } + + float distance; + if (!success || diff.Triangles.GetFirstIndex() == -1) + { + // No triangles unless the difference is 3D, fall back to GJK + // Most of the time this happens for cases like sphere-sphere, capsule-capsule, etc. which have special implementations, + // so comparing those to GJK still validates the results of different API queries against each other. + distance = DistanceQueries.ConvexConvex(ref a, ref b, aFromB).Distance; + } + else + { + // Find the closest triangle to the origin + distance = float.MaxValue; + bool penetrating = true; + for (int t = diff.Triangles.GetFirstIndex(); t != -1; t = diff.Triangles.GetNextIndex(t)) + { + ConvexHullBuilder.Triangle triangle = diff.Triangles[t]; + float3 v0 = diff.Vertices[triangle.GetVertex(0)].Position; + float3 v1 = diff.Vertices[triangle.GetVertex(1)].Position; + float3 v2 = diff.Vertices[triangle.GetVertex(2)].Position; + float3 n = diff.ComputePlane(t).Normal; + DistanceQueries.Result result = DistanceQueries.TriangleSphere(v0, v1, v2, n, float3.zero, 0.0f, MTransform.Identity); + if (result.Distance < distance) + { + distance = result.Distance; + } + penetrating = penetrating & (math.dot(n, -result.NormalInA) < 0.0f); // only penetrating if inside of all planes + } + + if (penetrating) + { + distance = -distance; + } + + distance -= a.ConvexRadius + b.ConvexRadius; + } + + diff.Dispose(); + return distance; + } + + private static unsafe void TestConvexConvexDistance(ConvexCollider* target, ConvexCollider* query, MTransform queryFromTarget, string failureMessage) + { + // Do the query, API version and reference version, then validate the result + DistanceQueries.Result result = DistanceQueries.ConvexConvex((Collider*)query, (Collider*)target, queryFromTarget); + float referenceDistance = RefConvexConvexDistance(ref query->ConvexHull, ref target->ConvexHull, queryFromTarget); + ValidateDistanceResult(result, ref query->ConvexHull, ref target->ConvexHull, queryFromTarget, referenceDistance, failureMessage); + } + + // This test generates random shape pairs, queries the distance between them, and validates some properties of the results: + // - Distance is compared against a slow reference implementation of the closest distance query + // - Closest points are on the plane through the support vertices in the normal direction + // If the test fails, it will report a seed. Set dbgTest to the seed to run the failing case alone. + [Test] + public unsafe void ConvexConvexDistanceTest() + { + Random rnd = new Random(0x12345678); + uint dbgTest = 0; + + int numTests = 5000; + if (dbgTest > 0) + { + numTests = 1; + } + + for (int i = 0; i < numTests; i++) + { + // Save state to repro this query without doing everything that came before it + if (dbgTest > 0) + { + rnd.state = dbgTest; + } + uint state = rnd.state; + + // Generate random query inputs + ConvexCollider* target = (ConvexCollider*)TestUtils.GenerateRandomConvex(ref rnd).GetUnsafePtr(); + ConvexCollider* query = (ConvexCollider*)TestUtils.GenerateRandomConvex(ref rnd).GetUnsafePtr(); + MTransform queryFromTarget = new MTransform( + (rnd.NextInt(10) > 0) ? rnd.NextQuaternionRotation() : quaternion.identity, + rnd.NextFloat3(-3.0f, 3.0f)); + TestConvexConvexDistance(target, query, queryFromTarget, "ConvexConvexDistanceTest failed " + i + " (" + state.ToString() + ")"); + } + } + + // This test generates random shapes and queries distance to themselves using small or identity transforms. This hits edge + // cases in collision detection routines where the two shapes being tested have equal or nearly equal features. The results + // are validated in the same way as those in ConvexConvexDistanceTest(). + // If the test fails, it will report a pair of seeds. Set dbgShape to the first and dbgTest to the second to run the failing case alone. + [Test] + public unsafe void ConvexConvexDistanceEdgeCaseTest() + { + Random rnd = new Random(0x90456148); + uint dbgShape = 0; + uint dbgTest = 0; + + int numShapes = 500; + int numTests = 50; + if (dbgShape > 0) + { + numShapes = 1; + numTests = 1; + } + + for (int iShape = 0; iShape < numShapes; iShape++) + { + if (dbgShape > 0) + { + rnd.state = dbgShape; + } + uint shapeState = rnd.state; + + // Generate a random collider + ConvexCollider* collider = (ConvexCollider*)TestUtils.GenerateRandomConvex(ref rnd).GetUnsafePtr(); + + for (int iTest = 0; iTest < numTests; iTest++) + { + if (dbgTest > 0) + { + rnd.state = dbgTest; + } + uint testState = rnd.state; + + // Generate random transform + float distance = math.pow(10.0f, rnd.NextFloat(-15.0f, -1.0f)); + float angle = math.pow(10.0f, rnd.NextFloat(-15.0f, 0.0f)); + MTransform queryFromTarget = new MTransform( + (rnd.NextInt(10) > 0) ? quaternion.AxisAngle(rnd.NextFloat3Direction(), angle) : quaternion.identity, + (rnd.NextInt(10) > 0) ? rnd.NextFloat3Direction() * distance : float3.zero); + TestConvexConvexDistance(collider, collider, queryFromTarget, "ConvexConvexDistanceEdgeCaseTest failed " + iShape + ", " + iTest + " (" + shapeState+ ", " + testState + ")"); + } + } + } + + static DistanceQueries.Result DistanceResultFromDistanceHit(DistanceHit hit, MTransform queryFromWorld) + { + return new DistanceQueries.Result + { + PositionOnAinA = Math.Mul(queryFromWorld, hit.Position + hit.SurfaceNormal * hit.Distance), + NormalInA = math.mul(queryFromWorld.Rotation, hit.SurfaceNormal), + Distance = hit.Distance + }; + } + + static unsafe void GetHitLeaf(ref Physics.PhysicsWorld world, int rigidBodyIndex, ColliderKey colliderKey, MTransform queryFromWorld, out ChildCollider leaf, out MTransform queryFromTarget) + { + Physics.RigidBody body = world.Bodies[rigidBodyIndex]; + Collider.GetLeafCollider(body.Collider, body.WorldFromBody, colliderKey, out leaf); + MTransform worldFromLeaf = new MTransform(leaf.TransformFromChild); + queryFromTarget = Math.Mul(queryFromWorld, worldFromLeaf); + } + + // Does distance queries and checks some properties of the results: + // - Closest hit returned from the all hits query has the same fraction as the hit returned from the closest hit query + // - Any hit and closest hit queries return a hit if and only if the all hits query does + // - Hit distance is the same as the support distance in the hit normal direction + // - Fetching the shapes from any world query hit and querying them directly gives a matching result + static unsafe void WorldCalculateDistanceTest(ref Physics.PhysicsWorld world, ColliderDistanceInput input, ref NativeList hits, string failureMessage) + { + // Do an all-hits query + hits.Clear(); + world.CalculateDistance(input, ref hits); + + // Check each hit and find the closest + float closestDistance = float.MaxValue; + MTransform queryFromWorld = Math.Inverse(new MTransform(input.Transform)); + for (int iHit = 0; iHit < hits.Length; iHit++) + { + DistanceHit hit = hits[iHit]; + closestDistance = math.min(closestDistance, hit.Distance); + + // Fetch the leaf collider and query it directly + ChildCollider leaf; + MTransform queryFromTarget; + GetHitLeaf(ref world, hit.RigidBodyIndex, hit.ColliderKey, queryFromWorld, out leaf, out queryFromTarget); + float referenceDistance = DistanceQueries.ConvexConvex(input.Collider, leaf.Collider, queryFromTarget).Distance; + + // Compare to the world query result + DistanceQueries.Result result = DistanceResultFromDistanceHit(hit, queryFromWorld); + ValidateDistanceResult(result, ref ((ConvexCollider*)input.Collider)->ConvexHull, ref ((ConvexCollider*)leaf.Collider)->ConvexHull, queryFromTarget, referenceDistance, + failureMessage + ", hits[" + iHit + "]"); + } + + // Do a closest-hit query and check that the distance matches + DistanceHit closestHit; + bool hasClosestHit = world.CalculateDistance(input, out closestHit); + if (hits.Length == 0) + { + Assert.IsFalse(hasClosestHit, failureMessage + ", closestHit: no matching result in hits"); + } + else + { + ChildCollider leaf; + MTransform queryFromTarget; + GetHitLeaf(ref world, closestHit.RigidBodyIndex, closestHit.ColliderKey, queryFromWorld, out leaf, out queryFromTarget); + + DistanceQueries.Result result = DistanceResultFromDistanceHit(closestHit, queryFromWorld); + ValidateDistanceResult(result, ref ((ConvexCollider*)input.Collider)->ConvexHull, ref ((ConvexCollider*)leaf.Collider)->ConvexHull, queryFromTarget, closestDistance, + failureMessage + ", closestHit"); + } + + // Do an any-hit query and check that it is consistent with the others + bool hasAnyHit = world.CalculateDistance(input); + Assert.AreEqual(hasAnyHit, hasClosestHit, failureMessage + ": any hit result inconsistent with the others"); + + // TODO - this test can't catch false misses. We could do brute-force broadphase / midphase search to cover those. + } + + static unsafe void CheckColliderCastHit(ref Physics.PhysicsWorld world, ColliderCastInput input, ColliderCastHit hit, string failureMessage) + { + // Fetch the leaf collider and convert the shape cast result into a distance result at the hit transform + ChildCollider leaf; + MTransform queryFromWorld = Math.Inverse(new MTransform(input.Orientation, input.Position + input.Direction * hit.Fraction)); + GetHitLeaf(ref world, hit.RigidBodyIndex, hit.ColliderKey, queryFromWorld, out leaf, out MTransform queryFromTarget); + DistanceQueries.Result result = new DistanceQueries.Result + { + PositionOnAinA = Math.Mul(queryFromWorld, hit.Position), + NormalInA = math.mul(queryFromWorld.Rotation, hit.SurfaceNormal), + Distance = 0.0f + }; + + // If the fraction is zero then the shapes should penetrate, otherwise they should have zero distance + if (hit.Fraction == 0.0f) + { + // Do a distance query to verify initial penetration + result.Distance = DistanceQueries.ConvexConvex(input.Collider, leaf.Collider, queryFromTarget).Distance; + Assert.Less(result.Distance, tolerance, failureMessage + ": zero fraction with positive distance"); + } + + // Verify the distance at the hit transform + ValidateDistanceResult(result, ref ((ConvexCollider*)input.Collider)->ConvexHull, ref ((ConvexCollider*)leaf.Collider)->ConvexHull, queryFromTarget, result.Distance, failureMessage); + } + + // Does collider casts and checks some properties of the results: + // - Closest hit returned from the all hits query has the same fraction as the hit returned from the closest hit query + // - Any hit and closest hit queries return a hit if and only if the all hits query does + // - Distance between the shapes at the hit fraction is zero + static unsafe void WorldColliderCastTest(ref Physics.PhysicsWorld world, ColliderCastInput input, ref NativeList hits, string failureMessage) + { + // Do an all-hits query + hits.Clear(); + world.CastCollider(input, ref hits); + + // Check each hit and find the earliest + float minFraction = float.MaxValue; + RigidTransform worldFromQuery = new RigidTransform(input.Orientation, input.Position); + for (int iHit = 0; iHit < hits.Length; iHit++) + { + ColliderCastHit hit = hits[iHit]; + minFraction = math.min(minFraction, hit.Fraction); + CheckColliderCastHit(ref world, input, hit, failureMessage + ", hits[" + iHit + "]"); + } + + // Do a closest-hit query and check that the fraction matches + ColliderCastHit closestHit; + bool hasClosestHit = world.CastCollider(input, out closestHit); + if (hits.Length == 0) + { + Assert.IsFalse(hasClosestHit, failureMessage + ", closestHit: no matching result in hits"); + } + else + { + Assert.AreEqual(closestHit.Fraction, minFraction, tolerance * math.length(input.Direction), failureMessage + ", closestHit: fraction does not match"); + CheckColliderCastHit(ref world, input, closestHit, failureMessage + ", closestHit"); + } + + // Do an any-hit query and check that it is consistent with the others + bool hasAnyHit = world.CastCollider(input); + Assert.AreEqual(hasAnyHit, hasClosestHit, failureMessage + ": any hit result inconsistent with the others"); + + // TODO - this test can't catch false misses. We could do brute-force broadphase / midphase search to cover those. + } + + static unsafe void CheckRaycastHit(ref Physics.PhysicsWorld world, RaycastInput input, RaycastHit hit, string failureMessage) + { + // Fetch the leaf collider + ChildCollider leaf; + { + Physics.RigidBody body = world.Bodies[hit.RigidBodyIndex]; + Collider.GetLeafCollider(body.Collider, body.WorldFromBody, hit.ColliderKey, out leaf); + } + + // Check that the hit position matches the fraction + float3 hitPosition = input.Ray.Origin + input.Ray.Direction * hit.Fraction; + Assert.Less(math.length(hitPosition - hit.Position), tolerance, failureMessage + ": inconsistent fraction and position"); + + // Query the hit position and check that it's on the surface of the shape + PointDistanceInput pointInput = new PointDistanceInput + { + Position = math.transform(math.inverse(leaf.TransformFromChild), hit.Position), + MaxDistance = float.MaxValue + }; + DistanceHit distanceHit; + leaf.Collider->CalculateDistance(pointInput, out distanceHit); + if (((ConvexCollider*)leaf.Collider)->ConvexHull.ConvexRadius > 0.0f) + { + // Convex raycast approximates radius, so it's possible that the hit position is not exactly on the shape, but must at least be outside + Assert.Greater(distanceHit.Distance, -tolerance, failureMessage); + } + else + { + Assert.AreEqual(distanceHit.Distance, 0.0f, tolerance, failureMessage); + } + } + + // Does raycasts and checks some properties of the results: + // - Closest hit returned from the all hits query has the same fraction as the hit returned from the closest hit query + // - Any hit and closest hit queries return a hit if and only if the all hits query does + // - All hits are on the surface of the hit shape + static unsafe void WorldRaycastTest(ref Physics.PhysicsWorld world, RaycastInput input, ref NativeList hits, string failureMessage) + { + // Do an all-hits query + hits.Clear(); + world.CastRay(input, ref hits); + + // Check each hit and find the earliest + float minFraction = float.MaxValue; + for (int iHit = 0; iHit < hits.Length; iHit++) + { + RaycastHit hit = hits[iHit]; + minFraction = math.min(minFraction, hit.Fraction); + CheckRaycastHit(ref world, input, hit, failureMessage + ", hits[" + iHit + "]"); + } + + // Do a closest-hit query and check that the fraction matches + RaycastHit closestHit; + bool hasClosestHit = world.CastRay(input, out closestHit); + if (hits.Length == 0) + { + Assert.IsFalse(hasClosestHit, failureMessage + ", closestHit: no matching result in hits"); + } + else + { + Assert.AreEqual(closestHit.Fraction, minFraction, tolerance * math.length(input.Ray.Direction), failureMessage + ", closestHit: fraction does not match"); + CheckRaycastHit(ref world, input, closestHit, failureMessage + ", closestHit"); + } + + // Do an any-hit query and check that it is consistent with the others + bool hasAnyHit = world.CastRay(input); + Assert.AreEqual(hasAnyHit, hasClosestHit, failureMessage + ": any hit result inconsistent with the others"); + + // TODO - this test can't catch false misses. We could do brute-force broadphase / midphase search to cover those. + } + + // This test generates random worlds, queries them, and validates some properties of the query results. + // See WorldCalculateDistanceTest, WorldColliderCastTest, and WorldRaycastTest for details about each query. + // If the test fails, it will report a pair of seeds. Set dbgWorld to the first and dbgTest to the second to run the failing case alone. + [Test] + public unsafe void WorldQueryTest() + { + const uint seed = 0x12345678; + uint dbgWorld = 0; // set dbgWorld, dbgTest to the seed reported from a failure message to repeat the failing case alone + uint dbgTest = 0; + + int numWorlds = 200; + int numTests = 5000; + if (dbgWorld > 0) + { + numWorlds = 1; + numTests = 1; + } + + Random rnd = new Random(seed); + NativeList distanceHits = new NativeList(Allocator.Temp); + NativeList colliderCastHits = new NativeList(Allocator.Temp); + NativeList raycastHits = new NativeList(Allocator.Temp); + + for (int iWorld = 0; iWorld < numWorlds; iWorld++) + { + // Save state to repro this query without doing everything that came before it + if (dbgWorld > 0) + { + rnd.state = dbgWorld; + } + uint worldState = rnd.state; + Physics.PhysicsWorld world = TestUtils.GenerateRandomWorld(ref rnd, rnd.NextInt(1, 20), 10.0f); + + for (int iTest = 0; iTest < (numTests / numWorlds); iTest++) + { + if (dbgTest > 0) + { + rnd.state = dbgTest; + } + uint testState = rnd.state; + string failureMessage = iWorld + ", " + iTest + " (" + worldState.ToString() + ", " + testState.ToString() + ")"; + + // Generate common random query inputs + Collider* collider = (Collider*)TestUtils.GenerateRandomConvex(ref rnd).GetUnsafePtr(); + RigidTransform transform = new RigidTransform + { + pos = rnd.NextFloat3(-10.0f, 10.0f), + rot = (rnd.NextInt(10) > 0) ? rnd.NextQuaternionRotation() : quaternion.identity, + }; + Ray ray = new Ray(transform.pos, rnd.NextFloat3(-5.0f, 5.0f)); + + // Distance test + { + ColliderDistanceInput input = new ColliderDistanceInput + { + Collider = collider, + Transform = transform, + MaxDistance = (rnd.NextInt(4) > 0) ? rnd.NextFloat(5.0f) : 0.0f + }; + WorldCalculateDistanceTest(ref world, input, ref distanceHits, "WorldQueryTest failed CalculateDistance " + failureMessage); + } + + // Collider cast test + { + ColliderCastInput input = new ColliderCastInput + { + Collider = collider, + Position = transform.pos, + Orientation = transform.rot, + Direction = ray.Direction + }; + WorldColliderCastTest(ref world, input, ref colliderCastHits, "WorldQueryTest failed ColliderCast " + failureMessage); + } + + // Ray cast test + { + RaycastInput input = new RaycastInput + { + Ray = ray, + Filter = CollisionFilter.Default + }; + WorldRaycastTest(ref world, input, ref raycastHits, "WorldQueryTest failed Raycast " + failureMessage); + } + } + + world.Dispose(); // TODO leaking memory if the test fails + } + + distanceHits.Dispose(); // TODO leaking memory if the test fails + colliderCastHits.Dispose(); + raycastHits.Dispose(); + } + + // Tests that a contact point is on the surface of its shape + static unsafe void CheckPointOnSurface(ref ChildCollider leaf, float3 position, string failureMessage) + { + float3 positionLocal = math.transform(math.inverse(leaf.TransformFromChild), position); + leaf.Collider->CalculateDistance(new PointDistanceInput { Position = positionLocal, MaxDistance = float.MaxValue, Filter = Physics.CollisionFilter.Default }, out DistanceHit hit); + Assert.Less(hit.Distance, tolerance, failureMessage + ": contact point outside of shape"); + Assert.Greater(hit.Distance, -((ConvexCollider*)leaf.Collider)->ConvexHull.ConvexRadius - tolerance, failureMessage + ": contact point inside of shape"); + } + + // Tests that the points of a manifold are all coplanar + static unsafe void CheckManifoldFlat(ref ConvexConvexManifoldQueries.Manifold manifold, float3 normal, string failureMessage) + { + float3 point0 = manifold[0].Position + normal * manifold[0].Distance; + float3 point1 = manifold[1].Position + normal * manifold[1].Distance; + for (int i = 2; i < manifold.NumContacts; i++) + { + // Try to calculate a plane from points 0, 1, iNormal + float3 point = manifold[i].Position + normal * manifold[i].Distance; + float3 cross = math.cross(point - point0, point - point1); + if (math.lengthsq(cross) > 1e-6f) + { + // Test that each point in the manifold is on the plane + float3 faceNormal = math.normalize(cross); + float dot = math.dot(point0, faceNormal); + for (int j = 2; j < manifold.NumContacts; j++) + { + float3 testPoint = manifold[j].Position + normal * manifold[j].Distance; + Assert.AreEqual(dot, math.dot(faceNormal, testPoint), tolerance, failureMessage + " contact " + j); + } + break; + } + } + } + + // This test generates random worlds, generates manifolds for every pair of bodies in the world, and validates some properties of the manifolds: + // - should contain the closest point + // - each body's contact points should be on that body's surface + // - each body's contact points should all be coplanar + // If the test fails, it will report a seed. Set dbgWorld to that seed to run the failing case alone. + [Test] + public unsafe void ManifoldQueryTest() + { + const uint seed = 0x98765432; + Random rnd = new Random(seed); + int numWorlds = 1000; + + uint dbgWorld = 0; + if (dbgWorld > 0) + { + numWorlds = 1; + } + + for (int iWorld = 0; iWorld < numWorlds; iWorld++) + { + // Save state to repro this query without doing everything that came before it + if (dbgWorld > 0) + { + rnd.state = dbgWorld; + } + uint worldState = rnd.state; + Physics.PhysicsWorld world = TestUtils.GenerateRandomWorld(ref rnd, rnd.NextInt(1, 20), 3.0f); + + // Manifold test + // TODO would be nice if we could change the world collision tolerance + for (int iBodyA = 0; iBodyA < world.NumBodies; iBodyA++) + { + for (int iBodyB = iBodyA + 1; iBodyB < world.NumBodies; iBodyB++) + { + Physics.RigidBody bodyA = world.Bodies[iBodyA]; + Physics.RigidBody bodyB = world.Bodies[iBodyB]; + if (bodyA.Collider->Type == ColliderType.Mesh && bodyB.Collider->Type == ColliderType.Mesh) + { + continue; // TODO - no mesh-mesh manifold support yet + } + + // Build manifolds + BlockStream contacts = new BlockStream(1, 0, Allocator.Temp); + BlockStream.Writer contactWriter = contacts; + contactWriter.BeginForEachIndex(0); + ManifoldQueries.BodyBody(ref world, new BodyIndexPair { BodyAIndex = iBodyA, BodyBIndex = iBodyB }, 1.0f, ref contactWriter); + contactWriter.EndForEachIndex(); + + // Read each manifold + BlockStream.Reader contactReader = contacts; + contactReader.BeginForEachIndex(0); + int manifoldIndex = 0; + while (contactReader.RemainingItemCount > 0) + { + string failureMessage = iWorld + " (" + worldState + ") " + iBodyA + " vs " + iBodyB + " #" + manifoldIndex; + manifoldIndex++; + + // Read the manifold header + ContactHeader header = contactReader.Read(); + ConvexConvexManifoldQueries.Manifold manifold = new ConvexConvexManifoldQueries.Manifold(); + manifold.NumContacts = header.NumContacts; + manifold.Normal = header.Normal; + + // Get the leaf shapes + ChildCollider leafA, leafB; + { + Collider.GetLeafCollider(bodyA.Collider, bodyA.WorldFromBody, header.ColliderKeys.ColliderKeyA, out leafA); + Collider.GetLeafCollider(bodyB.Collider, bodyB.WorldFromBody, header.ColliderKeys.ColliderKeyB, out leafB); + } + + // Read each contact point + int minIndex = 0; + for (int iContact = 0; iContact < header.NumContacts; iContact++) + { + // Read the contact and find the closest + ContactPoint contact = contactReader.Read(); + manifold[iContact] = contact; + if (contact.Distance < manifold[minIndex].Distance) + { + minIndex = iContact; + } + + // Check that the contact point is on or inside the shape + CheckPointOnSurface(ref leafA, contact.Position + manifold.Normal * contact.Distance, failureMessage + " contact " + iContact + " leaf A"); + CheckPointOnSurface(ref leafB, contact.Position, failureMessage + " contact " + iContact + " leaf B"); + } + + // Check the closest point + { + ContactPoint closestPoint = manifold[minIndex]; + RigidTransform aFromWorld = math.inverse(leafA.TransformFromChild); + DistanceQueries.Result result = new DistanceQueries.Result + { + PositionOnAinA = math.transform(aFromWorld, closestPoint.Position + manifold.Normal * closestPoint.Distance), + NormalInA = math.mul(aFromWorld.rot, manifold.Normal), + Distance = closestPoint.Distance + }; + + MTransform aFromB = new MTransform(math.mul(aFromWorld, leafB.TransformFromChild)); + float referenceDistance = DistanceQueries.ConvexConvex(leafA.Collider, leafB.Collider, aFromB).Distance; + ValidateDistanceResult(result, ref ((ConvexCollider*)leafA.Collider)->ConvexHull, ref ((ConvexCollider*)leafB.Collider)->ConvexHull, aFromB, referenceDistance, failureMessage + " closest point"); + } + + // Check that the manifold is flat + CheckManifoldFlat(ref manifold, manifold.Normal, failureMessage + ": non-flat A"); + CheckManifoldFlat(ref manifold, float3.zero, failureMessage + ": non-flat B"); + } + + contacts.Dispose(); + } + } + + world.Dispose(); // TODO leaking memory if the test fails + } + } + } +} diff --git a/package/Tests/PlayModeTests/Collision/Queries/QueryTests.cs.meta b/package/Tests/PlayModeTests/Collision/Queries/QueryTests.cs.meta new file mode 100755 index 000000000..db958da61 --- /dev/null +++ b/package/Tests/PlayModeTests/Collision/Queries/QueryTests.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: a0cc259adff2f274f96c6d0706f38ef1 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Tests/PlayModeTests/Collision/Queries/RaycastTests.cs b/package/Tests/PlayModeTests/Collision/Queries/RaycastTests.cs new file mode 100755 index 000000000..18795fda6 --- /dev/null +++ b/package/Tests/PlayModeTests/Collision/Queries/RaycastTests.cs @@ -0,0 +1,79 @@ +using NUnit.Framework; +using Unity.Mathematics; + +namespace Unity.Physics.Tests.Collision.Queries +{ + public class RayCastTests + { + [Test] + public void RayVsTriangle() + { + // triangle + var v1 = new float3(-1, -1, 0); + var v2 = new float3(0, 1, 0); + var v3 = new float3(1, -1, 0); + + { + var origin = new float3(0, 0, -2); + var direction = new float3(0, 0, 4); + + float fraction = 1; + bool hit = RaycastQueries.RayTriangle(origin, direction, v1, v2, v3, ref fraction, out float3 normal); + Assert.IsTrue(hit); + Assert.IsTrue(fraction == 0.5); + } + + { + var origin = new float3(0, 0, 2); + var direction = new float3(0, 0, -4); + + float fraction = 1; + bool hit = RaycastQueries.RayTriangle(origin, direction, v1, v2, v3, ref fraction, out float3 normal); + Assert.IsTrue(hit); + Assert.IsTrue(fraction == 0.5); + } + + { + var origin = new float3(1, -1, -2); + var direction = new float3(0, 0, 4); + + float fraction = 1; + bool hit = RaycastQueries.RayTriangle(origin, direction, v1, v2, v3, ref fraction, out float3 normal); + Assert.IsTrue(hit); + Assert.IsTrue(fraction == 0.5); + } + + { + var origin = new float3(2, 0, -2); + var direction = new float3(0, 0, 4); + + float fraction = 1; + bool hit = RaycastQueries.RayTriangle(origin, direction, v1, v2, v3, ref fraction, out float3 normal); + Assert.IsFalse(hit); + } + + { + var origin = new float3(2, 0, -2); + var direction = new float3(0, 0, -4); + + float fraction = 1; + bool hit = RaycastQueries.RayTriangle(origin, direction, v1, v2, v3, ref fraction, out float3 normal); + Assert.IsFalse(hit); + Assert.IsTrue(math.all(normal == new float3(0, 0, 0))); + } + + { + v1 = new float3(-4, 0, 0); + v2 = new float3(-5, 0, -1); + v3 = new float3(-4, 0, -1); + + var origin = new float3(-4.497f, 0.325f, -0.613f); + var direction = new float3(0f, -10f, 0f); + + float fraction = 1; + bool hit = RaycastQueries.RayTriangle(origin, direction, v1, v2, v3, ref fraction, out float3 normal); + Assert.IsTrue(hit); + } + } + } +} diff --git a/package/Tests/PlayModeTests/Collision/Queries/RaycastTests.cs.meta b/package/Tests/PlayModeTests/Collision/Queries/RaycastTests.cs.meta new file mode 100755 index 000000000..e8de258d0 --- /dev/null +++ b/package/Tests/PlayModeTests/Collision/Queries/RaycastTests.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 803278d066101a64aac157b7e64493b9 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Tests/PlayModeTests/Collision/RigidBody.meta b/package/Tests/PlayModeTests/Collision/RigidBody.meta new file mode 100755 index 000000000..3b21f5d73 --- /dev/null +++ b/package/Tests/PlayModeTests/Collision/RigidBody.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: 7a7fea9d0d6597040ac872ce3dcea294 +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Tests/PlayModeTests/Collision/RigidBody/RigidBodyTest.cs b/package/Tests/PlayModeTests/Collision/RigidBody/RigidBodyTest.cs new file mode 100755 index 000000000..0f22144de --- /dev/null +++ b/package/Tests/PlayModeTests/Collision/RigidBody/RigidBodyTest.cs @@ -0,0 +1,191 @@ +using NUnit.Framework; +using Unity.Collections; +using Unity.Mathematics; +using Unity.Entities; + +using Assert = UnityEngine.Assertions.Assert; +using float3 = Unity.Mathematics.float3; +using quaternion = Unity.Mathematics.quaternion; + +namespace Unity.Physics.Tests.Collision.RigidBody +{ + public class RigidBodyTest + { + [Test] + public unsafe void RigidBodyCalculateAabb_BoxColliderTest() + { + Physics.RigidBody rigidbodyBox = Unity.Physics.RigidBody.Zero; + const float size = 1.0f; + const float convexRadius = 0.2f; + rigidbodyBox.Collider = (Collider*)BoxCollider.Create(float3.zero, quaternion.identity, new float3(size), convexRadius).GetUnsafePtr(); + + var boxAabb = rigidbodyBox.CalculateAabb(); + var box = (BoxCollider*)BoxCollider.Create(float3.zero, quaternion.identity, new float3(size), convexRadius).GetUnsafePtr(); + Assert.IsTrue(boxAabb.Equals(box->CalculateAabb())); + } + + [Test] + public unsafe void RigidBodyCalculateAabb_SphereColliderTest() + { + Physics.RigidBody rigidbodySphere = Unity.Physics.RigidBody.Zero; + const float convexRadius = 1.0f; + rigidbodySphere.Collider = (Collider*)SphereCollider.Create(float3.zero, convexRadius).GetUnsafePtr(); + + var sphereAabb = rigidbodySphere.CalculateAabb(); + var sphere = (Collider*)SphereCollider.Create(float3.zero, convexRadius).GetUnsafePtr(); + Assert.IsTrue(sphereAabb.Equals(sphere->CalculateAabb())); + } + + [Test] + public unsafe void RigidBodyCastRayTest() + { + Physics.RigidBody rigidbody = Unity.Physics.RigidBody.Zero; + + const float size = 1.0f; + const float convexRadius = 1.0f; + + var rayStartOK = new float3(-10, -10, -10); + var rayEndOK = new float3(10, 10, 10); + + var rayStartFail = new float3(-10, 10, -10); + var rayEndFail = new float3(10, 10, 10); + + rigidbody.Collider = (Collider*)BoxCollider.Create(float3.zero, quaternion.identity, new float3(size), convexRadius).GetUnsafePtr(); + + var raycastInput = new RaycastInput(); + var closestHit = new RaycastHit(); + var allHits = new NativeList(Allocator.Temp); + + // OK case : Ray hits the box collider + float3 rayDir = rayEndOK - rayStartOK; + raycastInput.Ray.Origin = rayStartOK; + raycastInput.Ray.Direction = rayDir; + raycastInput.Filter = CollisionFilter.Default; + + Assert.IsTrue(rigidbody.CastRay(raycastInput)); + Assert.IsTrue(rigidbody.CastRay(raycastInput, out closestHit)); + Assert.IsTrue(rigidbody.CastRay(raycastInput, ref allHits)); + + // Fail Case : wrong direction + rayDir = rayEndFail - rayStartFail; + raycastInput.Ray.Origin = rayStartFail; + raycastInput.Ray.Direction = rayDir; + + Assert.IsFalse(rigidbody.CastRay(raycastInput)); + Assert.IsFalse(rigidbody.CastRay(raycastInput, out closestHit)); + Assert.IsFalse(rigidbody.CastRay(raycastInput, ref allHits)); + } + + [Test] + public unsafe void RigidBodyCastColliderTest() + { + Physics.RigidBody rigidbody = Unity.Physics.RigidBody.Zero; + + const float size = 1.0f; + const float convexRadius = 1.0f; + + var rayStartOK = new float3(-10, -10, -10); + var rayEndOK = new float3(10, 10, 10); + + var rayStartFail = new float3(-10, 10, -10); + var rayEndFail = new float3(10, 10, 10); + + rigidbody.Collider = (Collider*)BoxCollider.Create(float3.zero, quaternion.identity, new float3(size), convexRadius).GetUnsafePtr(); + + var colliderCastInput = new ColliderCastInput(); + var closestHit = new ColliderCastHit(); + var allHits = new NativeList(Allocator.Temp); + + // OK case : Sphere hits the box collider + float3 rayDir = rayEndOK - rayStartOK; + colliderCastInput.Position = rayStartOK; + colliderCastInput.Direction = rayDir; + colliderCastInput.Collider = (Collider*)SphereCollider.Create(float3.zero, convexRadius).GetUnsafePtr(); + + Assert.IsTrue(rigidbody.CastCollider(colliderCastInput)); + Assert.IsTrue(rigidbody.CastCollider(colliderCastInput, out closestHit)); + Assert.IsTrue(rigidbody.CastCollider(colliderCastInput, ref allHits)); + + // Fail case : wrong direction + rayDir = rayEndFail - rayStartFail; + colliderCastInput.Position = rayStartFail; + colliderCastInput.Direction = rayDir; + + Assert.IsFalse(rigidbody.CastCollider(colliderCastInput)); + Assert.IsFalse(rigidbody.CastCollider(colliderCastInput, out closestHit)); + Assert.IsFalse(rigidbody.CastCollider(colliderCastInput, ref allHits)); + } + + [Test] + public unsafe void RigidBodyCalculateDistancePointTest() + { + Physics.RigidBody rigidbody = Unity.Physics.RigidBody.Zero; + + const float size = 1.0f; + const float convexRadius = 1.0f; + + var queryPos = new float3(-10, -10, -10); + + rigidbody.Collider = (Collider*)BoxCollider.Create(float3.zero, quaternion.identity, new float3(size), convexRadius).GetUnsafePtr(); + + var pointDistanceInput = new PointDistanceInput(); + + pointDistanceInput.Position = queryPos; + pointDistanceInput.Filter = CollisionFilter.Default; + + var closestHit = new DistanceHit(); + var allHits = new NativeList(Allocator.Temp); + + // OK case : with enough max distance + pointDistanceInput.MaxDistance = 10000.0f; + Assert.IsTrue(rigidbody.CalculateDistance(pointDistanceInput)); + Assert.IsTrue(rigidbody.CalculateDistance(pointDistanceInput, out closestHit)); + Assert.IsTrue(rigidbody.CalculateDistance(pointDistanceInput, ref allHits)); + + // Fail case : not enough max distance + pointDistanceInput.MaxDistance = 1; + Assert.IsFalse(rigidbody.CalculateDistance(pointDistanceInput)); + Assert.IsFalse(rigidbody.CalculateDistance(pointDistanceInput, out closestHit)); + Assert.IsFalse(rigidbody.CalculateDistance(pointDistanceInput, ref allHits)); + } + + [Test] + public unsafe void RigidBodyCalculateDistanceTest() + { + const float size = 1.0f; + const float convexRadius = 1.0f; + + var queryPos = new float3(-10, -10, -10); + + BlobAssetReference boxCollider = BoxCollider.Create(float3.zero, quaternion.identity, new float3(size), convexRadius); + BlobAssetReference sphereCollider = SphereCollider.Create(float3.zero, convexRadius); + + var rigidBody = new Physics.RigidBody + { + WorldFromBody = RigidTransform.identity, + Collider = (Collider*)boxCollider.GetUnsafePtr() + }; + + var colliderDistanceInput = new ColliderDistanceInput + { + Collider = (Collider*)sphereCollider.GetUnsafePtr(), + Transform = new RigidTransform(quaternion.identity, queryPos) + }; + + var closestHit = new DistanceHit(); + var allHits = new NativeList(Allocator.Temp); + + // OK case : with enough max distance + colliderDistanceInput.MaxDistance = 10000.0f; + Assert.IsTrue(rigidBody.CalculateDistance(colliderDistanceInput)); + Assert.IsTrue(rigidBody.CalculateDistance(colliderDistanceInput, out closestHit)); + Assert.IsTrue(rigidBody.CalculateDistance(colliderDistanceInput, ref allHits)); + + // Fail case : not enough max distance + colliderDistanceInput.MaxDistance = 1; + Assert.IsFalse(rigidBody.CalculateDistance(colliderDistanceInput)); + Assert.IsFalse(rigidBody.CalculateDistance(colliderDistanceInput, out closestHit)); + Assert.IsFalse(rigidBody.CalculateDistance(colliderDistanceInput, ref allHits)); + } + } +} diff --git a/package/Tests/PlayModeTests/Collision/RigidBody/RigidBodyTest.cs.meta b/package/Tests/PlayModeTests/Collision/RigidBody/RigidBodyTest.cs.meta new file mode 100755 index 000000000..7cc31e423 --- /dev/null +++ b/package/Tests/PlayModeTests/Collision/RigidBody/RigidBodyTest.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: cc79a8fcc3c3f6240b5c4fe0831e796f +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Tests/PlayModeTests/Collision/World.meta b/package/Tests/PlayModeTests/Collision/World.meta new file mode 100755 index 000000000..b92355006 --- /dev/null +++ b/package/Tests/PlayModeTests/Collision/World.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: 18365e8e858addb4b9e956c6c3b39957 +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Tests/PlayModeTests/Collision/World/BroadphaseTests.cs b/package/Tests/PlayModeTests/Collision/World/BroadphaseTests.cs new file mode 100755 index 000000000..71120b940 --- /dev/null +++ b/package/Tests/PlayModeTests/Collision/World/BroadphaseTests.cs @@ -0,0 +1,168 @@ +using NUnit.Framework; +using Unity.Entities; +using UnityEngine; +using Assert = UnityEngine.Assertions.Assert; + +namespace Unity.Physics.Tests.Collision.PhysicsWorld +{ + public class BroadPhaseTests + { + /// Util functions + //Creates a world + static public Physics.PhysicsWorld createTestWorld(int staticBodies = 0, int dynamicBodies = 0, int joints = 0) + { + return new Physics.PhysicsWorld(staticBodies, dynamicBodies, joints); + } + + //Adds a static box to the world + static public unsafe void addStaticBoxToWorld(Physics.PhysicsWorld world, int index, Vector3 pos, Quaternion orientation, Vector3 size) + { + Assert.IsTrue(index < world.NumStaticBodies, "Static body index is out of range in addStaticBoxToWorld"); + Unity.Collections.NativeSlice staticBodies = world.StaticBodies; + Physics.RigidBody rb = staticBodies[index]; + BlobAssetReference collider = Unity.Physics.BoxCollider.Create(pos, orientation, size, .01f); + rb.Collider = (Collider*)collider.GetUnsafePtr(); + staticBodies[index] = rb; + } + + //Adds a dynamic box to the world + static public unsafe void addDynamicBoxToWorld(Physics.PhysicsWorld world, int index, Vector3 pos, Quaternion orientation, Vector3 size) + { + Assert.IsTrue(index < world.NumDynamicBodies, "Dynamic body index is out of range in addDynamicBoxToWorld"); + Unity.Collections.NativeSlice dynamicBodies = world.DynamicBodies; + Physics.RigidBody rb = dynamicBodies[index]; + BlobAssetReference collider = Unity.Physics.BoxCollider.Create(pos, orientation, size, .01f); + rb.Collider = (Collider*)collider.GetUnsafePtr(); + dynamicBodies[index] = rb; + } + + /// Tests + //Tests Broadphase Constructor, Init + [Test] + public void InitTest() + { + Broadphase bf = new Broadphase(); + + bf.Init(); + + bf.Dispose(); + } + + //Tests ScheduleBuildJobs with one static box in the world + [Test] + public void ScheduleBuildJobsOneStaticBoxTest() + { + Physics.PhysicsWorld world = createTestWorld(1); + addStaticBoxToWorld(world, 0, new Vector3(0, 0, 0), Quaternion.identity, new Vector3(10, 0.1f, 10)); + Unity.Jobs.JobHandle handle = new Unity.Jobs.JobHandle(); + Unity.Jobs.JobHandle result = world.CollisionWorld.Broadphase.ScheduleBuildJobs(ref world, 1 / 60, 1, true, handle); + result.Complete(); + Assert.IsTrue(result.IsCompleted); + world.Dispose(); + } + + //Tests ScheduleBuildJobs with 10 static boxes + [Test] + public void ScheduleBuildJobsTenStaticBoxesTest() + { + Physics.PhysicsWorld world = createTestWorld(10); + for (int i = 0; i < 10; ++i) + { + addStaticBoxToWorld(world, i, new Vector3(i*11, 0, 0), Quaternion.identity, new Vector3(10, 0.1f, 10)); + } + Unity.Jobs.JobHandle handle = new Unity.Jobs.JobHandle(); + Unity.Jobs.JobHandle result = world.CollisionWorld.Broadphase.ScheduleBuildJobs(ref world, 1 / 60, 1, true, handle); + result.Complete(); + Assert.IsTrue(result.IsCompleted); + world.Dispose(); + } + + //Tests ScheduleBuildJobs with 100 static boxes + [Test] + public void ScheduleBuildJobsOneHundredStaticBoxesTest() + { + Physics.PhysicsWorld world = createTestWorld(100); + for (int i = 0; i < 100; ++i) + { + addStaticBoxToWorld(world, i, new Vector3(i*11, 0, 0), Quaternion.identity, new Vector3(10, 0.1f, 10)); + } + Unity.Jobs.JobHandle handle = new Unity.Jobs.JobHandle(); + Unity.Jobs.JobHandle result = world.CollisionWorld.Broadphase.ScheduleBuildJobs(ref world, 1 / 60, 1, true, handle); + result.Complete(); + Assert.IsTrue(result.IsCompleted); + world.Dispose(); + } + + //Tests ScheduleBuildJobs with one Dynamic box in the world + [Test] + public void ScheduleBuildJobsOneDynamicBoxTest() + { + //Physics.World world = createTestWorld(0,1); + //addDynamicBoxToWorld(world, 0, new Vector3(0, 0, 0), Quaternion.identity, new Vector3(10, 10, 10)); + //Unity.Jobs.JobHandle handle = new Unity.Jobs.JobHandle(); + //Unity.Jobs.JobHandle result = world.CollisionWorld.Broadphase.ScheduleBuildJobs(ref world, 1 / 60, 1, handle); + //result.Complete(); + //Assert.IsTrue(result.IsCompleted); + //world.Dispose(); + } + + //Tests ScheduleBuildJobs with 10 Dynamic boxes + [Test] + public void ScheduleBuildJobsTenDynamicBoxesTest() + { + //Physics.World world = createTestWorld(0,10); + //for (int i = 0; i < 10; ++i) + //{ + // addDynamicBoxToWorld(world, i, new Vector3(i*11, 0, 0), Quaternion.identity, new Vector3(10, 10, 10)); + //} + //Unity.Jobs.JobHandle handle = new Unity.Jobs.JobHandle(); + //Unity.Jobs.JobHandle result = world.CollisionWorld.Broadphase.ScheduleBuildJobs(ref world, 1 / 60, 1, handle); + //result.Complete(); + //Assert.IsTrue(result.IsCompleted); + //world.Dispose(); + } + + //Tests ScheduleBuildJobs with 100 Dynamic boxes + [Test] + public void ScheduleBuildJobsOneHundredDynamicBoxesTest() + { + //Physics.World world = createTestWorld(0,100); + //for (int i = 0; i < 100; ++i) + //{ + // addDynamicBoxToWorld(world, i, new Vector3(i*11, 0, 0), Quaternion.identity, new Vector3(10, 10, 10)); + //} + //Unity.Jobs.JobHandle handle = new Unity.Jobs.JobHandle(); + //Unity.Jobs.JobHandle result = world.CollisionWorld.Broadphase.ScheduleBuildJobs(ref world, 1 / 60, 1, handle); + //result.Complete(); + //Assert.IsTrue(result.IsCompleted); + //world.Dispose(); + } + + [Test] + public void ScheduleBuildJobsStaticAndDynamicBoxesTest() + { + //Physics.World world = createTestWorld(100,100); + //for (int i = 0; i < 100; ++i) + //{ + // addStaticBoxToWorld(world, i, new Vector3(i * 11, 0, 0), Quaternion.identity, new Vector3(10, 0.1f, 10)); + // addDynamicBoxToWorld(world, i, new Vector3(i * 11, 5, 0), Quaternion.identity, new Vector3(1, 1, 1)); + //} + //Unity.Jobs.JobHandle handle = new Unity.Jobs.JobHandle(); + //Unity.Jobs.JobHandle result = world.CollisionWorld.Broadphase.ScheduleBuildJobs(ref world, 1 / 60, 1, handle); + //result.Complete(); + //Assert.IsTrue(result.IsCompleted); + //world.Dispose(); + } + + //Tests that ScheduleBuildJobs on an empty world returns a completable JobHandle + [Test] + public void ScheduleBuildJobsEmptyWorldTest() + { + //Physics.World world = createTestWorld(); + //Unity.Jobs.JobHandle handle = new Unity.Jobs.JobHandle(); + //Unity.Jobs.JobHandle result = world.CollisionWorld.Broadphase.ScheduleBuildJobs(ref world, 1 / 60, 1, handle); + //result.Complete(); + //world.Dispose(); + } + } +} diff --git a/package/Tests/PlayModeTests/Collision/World/BroadphaseTests.cs.meta b/package/Tests/PlayModeTests/Collision/World/BroadphaseTests.cs.meta new file mode 100755 index 000000000..403bbe6e1 --- /dev/null +++ b/package/Tests/PlayModeTests/Collision/World/BroadphaseTests.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 8cfa7fa26c31dc140a924dc000ef62f3 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Tests/PlayModeTests/Collision/World/CollisionWorldTests.cs b/package/Tests/PlayModeTests/Collision/World/CollisionWorldTests.cs new file mode 100755 index 000000000..7ba58633e --- /dev/null +++ b/package/Tests/PlayModeTests/Collision/World/CollisionWorldTests.cs @@ -0,0 +1,139 @@ +using NUnit.Framework; +using Unity.Mathematics; +using UnityEngine; +using Assert = UnityEngine.Assertions.Assert; + +namespace Unity.Physics.Tests.Collision.PhysicsWorld +{ + public class CollisionWorldTests + { + //Tests creating a Zero body world + [Test] + public void ZeroBodyInitTest() + { + CollisionWorld world = new CollisionWorld(0); + Assert.IsTrue(world.NumBodies == 0); + world.Dispose(); + } + + //Tests creating a 10 body world + [Test] + public void TenBodyInitTest() + { + CollisionWorld world = new CollisionWorld(10); + Assert.IsTrue(world.NumBodies == 10); + world.Dispose(); + } + + //Tests updating an empty world + [Test] + public void SheduleUpdateJobsEmptyWorldTest() + { + //Physics.World world = BroadPhaseTests.createTestWorld(); + //Unity.Jobs.JobHandle handle = new Unity.Jobs.JobHandle(); + //Unity.Jobs.JobHandle worldJobHandle = world.CollisionWorld.ScheduleUpdateJobs(ref world, 1 / 60, 1, handle); + //worldJobHandle.Complete(); + //Assert.IsTrue(worldJobHandle.IsCompleted); + //world.Dispose(); + } + + //Tests updating a static box + [Test] + public void SheduleUpdateJobsOneStaticBoxTest() + { + Unity.Physics.PhysicsWorld world = BroadPhaseTests.createTestWorld(1); + BroadPhaseTests.addStaticBoxToWorld(world, 0, Vector3.zero, quaternion.identity, new Vector3(10, .1f, 10)); + Unity.Jobs.JobHandle handle = new Unity.Jobs.JobHandle(); + Unity.Jobs.JobHandle worldJobHandle = world.CollisionWorld.ScheduleUpdateDynamicLayer(ref world, 1 / 60, 1, handle); + worldJobHandle.Complete(); + Assert.IsTrue(worldJobHandle.IsCompleted); + world.Dispose(); + } + + //Tests updating 10 static boxes + [Test] + public void SheduleUpdateJobsTenStaticBoxesTest() + { + Physics.PhysicsWorld world = BroadPhaseTests.createTestWorld(10); + for (int i = 0; i < 10; ++i) + BroadPhaseTests.addStaticBoxToWorld(world, i, new Vector3(11 * i, 0, 0), quaternion.identity, new Vector3(10, .1f, 10)); + Unity.Jobs.JobHandle handle = new Unity.Jobs.JobHandle(); + Unity.Jobs.JobHandle worldJobHandle = world.CollisionWorld.ScheduleUpdateDynamicLayer(ref world, 1 / 60, 1, handle); + worldJobHandle.Complete(); + Assert.IsTrue(worldJobHandle.IsCompleted); + world.Dispose(); + } + + //Tests updating 100 static boxes + [Test] + public void SheduleUpdateJobsOneHundredStaticBoxesTest() + { + Physics.PhysicsWorld world = BroadPhaseTests.createTestWorld(100); + for (int i = 0; i < 100; ++i) + BroadPhaseTests.addStaticBoxToWorld(world, i, new Vector3(11 * i, 0, 0), quaternion.identity, new Vector3(10, .1f, 10)); + Unity.Jobs.JobHandle handle = new Unity.Jobs.JobHandle(); + Unity.Jobs.JobHandle worldJobHandle = world.CollisionWorld.ScheduleUpdateDynamicLayer(ref world, 1 / 60, 1, handle); + worldJobHandle.Complete(); + Assert.IsTrue(worldJobHandle.IsCompleted); + world.Dispose(); + } + + //Tests updating a Dynamic box + [Test] + public void SheduleUpdateJobsOneDynamicBoxTest() + { + //Physics.World world = BroadPhaseTests.createTestWorld(0,1); + //BroadPhaseTests.addDynamicBoxToWorld(world, 0, Vector3.zero, quaternion.identity, new Vector3(10, .1f, 10)); + //Unity.Jobs.JobHandle handle = new Unity.Jobs.JobHandle(); + //Unity.Jobs.JobHandle worldJobHandle = world.CollisionWorld.ScheduleUpdateJobs(ref world, 1 / 60, 1, handle); + //worldJobHandle.Complete(); + //Assert.IsTrue(worldJobHandle.IsCompleted); + //world.Dispose(); + } + + //Tests updating 10 dynamic boxes + [Test] + public void SheduleUpdateJobsTenDynamicBoxesTest() + { + //Physics.World world = BroadPhaseTests.createTestWorld(0,10); + //for (int i = 0; i < 10; ++i) + // BroadPhaseTests.addDynamicBoxToWorld(world, i, new Vector3(11 * i, 0, 0), quaternion.identity, new Vector3(10, .1f, 10)); + //Unity.Jobs.JobHandle handle = new Unity.Jobs.JobHandle(); + //Unity.Jobs.JobHandle worldJobHandle = world.CollisionWorld.ScheduleUpdateJobs(ref world, 1 / 60, 1, handle); + //worldJobHandle.Complete(); + //Assert.IsTrue(worldJobHandle.IsCompleted); + //world.Dispose(); + } + + //Tests updating 100 dynamic boxes + [Test] + public void SheduleUpdateJobsOneHundredDynamicBoxesTest() + { + //Physics.World world = BroadPhaseTests.createTestWorld(0,100); + //for (int i = 0; i < 100; ++i) + // BroadPhaseTests.addDynamicBoxToWorld(world, i, new Vector3(11 * i, 0, 0), quaternion.identity, new Vector3(10, .1f, 10)); + //Unity.Jobs.JobHandle handle = new Unity.Jobs.JobHandle(); + //Unity.Jobs.JobHandle worldJobHandle = world.CollisionWorld.ScheduleUpdateJobs(ref world, 1 / 60, 1, handle); + //worldJobHandle.Complete(); + //Assert.IsTrue(worldJobHandle.IsCompleted); + //world.Dispose(); + } + + //Tests updating 100 static and dynamic boxes + [Test] + public void SheduleUpdateJobsStaticAndDynamicBoxesTest() + { + //Physics.World world = BroadPhaseTests.createTestWorld(100, 100); + //for (int i = 0; i < 100; ++i) + //{ + // BroadPhaseTests.addDynamicBoxToWorld(world, i, new Vector3(11 * i, 0, 0), quaternion.identity, new Vector3(10, .1f, 10)); + // BroadPhaseTests.addStaticBoxToWorld(world, i, new Vector3(11 * i, 0, 0), quaternion.identity, new Vector3(10, .1f, 10)); + //} + //Unity.Jobs.JobHandle handle = new Unity.Jobs.JobHandle(); + //Unity.Jobs.JobHandle worldJobHandle = world.CollisionWorld.ScheduleUpdateDynamicLayer(ref world, 1 / 60, 1, handle); + //worldJobHandle.Complete(); + //Assert.IsTrue(worldJobHandle.IsCompleted); + //world.Dispose(); + } + } +} diff --git a/package/Tests/PlayModeTests/Collision/World/CollisionWorldTests.cs.meta b/package/Tests/PlayModeTests/Collision/World/CollisionWorldTests.cs.meta new file mode 100755 index 000000000..9b2a5eca2 --- /dev/null +++ b/package/Tests/PlayModeTests/Collision/World/CollisionWorldTests.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 3cb9f7119d8af194c96b781c41aa1d77 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Tests/PlayModeTests/Dynamics.meta b/package/Tests/PlayModeTests/Dynamics.meta new file mode 100755 index 000000000..a1414b307 --- /dev/null +++ b/package/Tests/PlayModeTests/Dynamics.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: 7cfc3019a4922544aad58246c4cc77e1 +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Tests/PlayModeTests/Dynamics/Integrator.meta b/package/Tests/PlayModeTests/Dynamics/Integrator.meta new file mode 100755 index 000000000..d149293d2 --- /dev/null +++ b/package/Tests/PlayModeTests/Dynamics/Integrator.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: 62fbaafdfe7d70c41b086ebf33defdf0 +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Tests/PlayModeTests/Dynamics/Integrator/IntegratorTests.cs b/package/Tests/PlayModeTests/Dynamics/Integrator/IntegratorTests.cs new file mode 100755 index 000000000..04a170423 --- /dev/null +++ b/package/Tests/PlayModeTests/Dynamics/Integrator/IntegratorTests.cs @@ -0,0 +1,33 @@ +using NUnit.Framework; +using Unity.Mathematics; +using UnityEngine; +using Assert = UnityEngine.Assertions.Assert; + +namespace Unity.Physics.Tests.Dynamics.Integrator +{ + public class IntegratorTests + { + [Test] + public void IntegrateOrientationTest() + { + var orientation = quaternion.identity; + var angularVelocity = new float3(0.0f, 0.0f, 0.0f); + float timestep = 1.0f; + + Physics.Integrator.IntegrateOrientation(ref orientation, angularVelocity, timestep); + + Assert.AreEqual(new quaternion(0.0f, 0.0f, 0.0f, 1.0f), orientation); + } + + [Test] + public void IntegrateAngularVelocityTest() + { + var angularVelocity = new float3(1.0f, 2.0f, 3.0f); + float timestep = 4.0f; + + var orientation = Unity.Physics.Integrator.IntegrateAngularVelocity(angularVelocity, timestep); + + Assert.AreEqual(new quaternion(2.0f, 4.0f, 6.0f, 1.0f), orientation); + } + } +} diff --git a/package/Tests/PlayModeTests/Dynamics/Integrator/IntegratorTests.cs.meta b/package/Tests/PlayModeTests/Dynamics/Integrator/IntegratorTests.cs.meta new file mode 100755 index 000000000..46971c639 --- /dev/null +++ b/package/Tests/PlayModeTests/Dynamics/Integrator/IntegratorTests.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: f53a3e884e719db43a6851cfbb47bdef +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Tests/PlayModeTests/Dynamics/Jacobians.meta b/package/Tests/PlayModeTests/Dynamics/Jacobians.meta new file mode 100755 index 000000000..ddb0f2a86 --- /dev/null +++ b/package/Tests/PlayModeTests/Dynamics/Jacobians.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: d54fa250079bbd64b870c12e6e7465f5 +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Tests/PlayModeTests/Dynamics/Jacobians/AngularLimit1DJacobianTests.cs b/package/Tests/PlayModeTests/Dynamics/Jacobians/AngularLimit1DJacobianTests.cs new file mode 100755 index 000000000..f8ee267f1 --- /dev/null +++ b/package/Tests/PlayModeTests/Dynamics/Jacobians/AngularLimit1DJacobianTests.cs @@ -0,0 +1,39 @@ +using NUnit.Framework; +using Unity.Mathematics; +using static Unity.Physics.Math; +using Assert = UnityEngine.Assertions.Assert; + +namespace Unity.Physics.Tests.Dynamics.Jacobians +{ + public class AngularLimit1DJacobianTests + { + [Test] + public void BuildTest() + { + var jacobian = new AngularLimit1DJacobian(); + + var aFromConstraint = MTransform.Identity; + var bFromConstraint = MTransform.Identity; + var velocityA = MotionVelocity.Zero; + var velocityB = MotionVelocity.Zero; + var motionA = MotionData.Zero; + var motionB = MotionData.Zero; + var constraint = new Constraint() { ConstrainedAxes = new bool3(true, false, false) }; + var tau = 1.0f; + var damping = 1.0f; + + jacobian.Build(aFromConstraint, bFromConstraint, velocityA, velocityB, motionA, motionB, constraint, tau, damping); + + Assert.AreEqual(new float3(1.0f, 0.0f, 0.0f), jacobian.AxisInMotionA); + Assert.AreEqual(0.0f, jacobian.MinAngle); + Assert.AreEqual(0.0f, jacobian.MaxAngle); + Assert.AreEqual(1.0f, jacobian.Tau); + Assert.AreEqual(1.0f, jacobian.Damping); + Assert.AreEqual(quaternion.identity, jacobian.MotionBFromA); + Assert.AreEqual(quaternion.identity, jacobian.MotionAFromJoint); + Assert.AreEqual(quaternion.identity, jacobian.MotionBFromJoint); + + Assert.AreEqual(0.0f, jacobian.InitialError); + } + } +} diff --git a/package/Tests/PlayModeTests/Dynamics/Jacobians/AngularLimit1DJacobianTests.cs.meta b/package/Tests/PlayModeTests/Dynamics/Jacobians/AngularLimit1DJacobianTests.cs.meta new file mode 100755 index 000000000..1a10705ba --- /dev/null +++ b/package/Tests/PlayModeTests/Dynamics/Jacobians/AngularLimit1DJacobianTests.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: ebebc96dd461a5a44b0835b49546db76 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Tests/PlayModeTests/Dynamics/Jacobians/AngularLimit2DJacobianTests.cs b/package/Tests/PlayModeTests/Dynamics/Jacobians/AngularLimit2DJacobianTests.cs new file mode 100755 index 000000000..13b3dbe7f --- /dev/null +++ b/package/Tests/PlayModeTests/Dynamics/Jacobians/AngularLimit2DJacobianTests.cs @@ -0,0 +1,53 @@ +using NUnit.Framework; +using Unity.Mathematics; +using static Unity.Physics.Math; +using Assert = UnityEngine.Assertions.Assert; + +namespace Unity.Physics.Tests.Dynamics.Jacobians +{ + public class AngularLimit2DJacobianTests + { + [Test] + public void BuildTest() + { + var jacobian = new AngularLimit2DJacobian(); + + var aFromConstraint = MTransform.Identity; + var bFromConstraint = MTransform.Identity; + var velocityA = MotionVelocity.Zero; + var velocityB = MotionVelocity.Zero; + var motionA = MotionData.Zero; + var motionB = MotionData.Zero; + var constraint = new Constraint() { ConstrainedAxes = new bool3(true, true, false) }; + var tau = 1.0f; + var damping = 1.0f; + + jacobian.Build(aFromConstraint, bFromConstraint, velocityA, velocityB, motionA, motionB, constraint, tau, damping); + + Assert.AreEqual(new float3(0.0f, 0.0f, 1.0f), jacobian.AxisAinA); + Assert.AreEqual(new float3(0.0f, 0.0f, 1.0f), jacobian.AxisBinB); + Assert.AreEqual(0.0f, jacobian.MinAngle); + Assert.AreEqual(0.0f, jacobian.MaxAngle); + Assert.AreEqual(1.0f, jacobian.Tau); + Assert.AreEqual(1.0f, jacobian.Damping); + Assert.AreEqual(quaternion.identity, jacobian.BFromA); + + Assert.AreEqual(0.0f, jacobian.InitialError); + } + + [Test] + public void SolveTest() + { + var jacobian = new AngularLimit2DJacobian() { BFromA = quaternion.identity }; + + var velocityA = MotionVelocity.Zero; + var velocityB = MotionVelocity.Zero; + var timestep = 1.0f; + + jacobian.Solve(ref velocityA, ref velocityB, timestep); + + Assert.AreEqual(MotionVelocity.Zero, velocityA); + Assert.AreEqual(MotionVelocity.Zero, velocityB); + } + } +} diff --git a/package/Tests/PlayModeTests/Dynamics/Jacobians/AngularLimit2DJacobianTests.cs.meta b/package/Tests/PlayModeTests/Dynamics/Jacobians/AngularLimit2DJacobianTests.cs.meta new file mode 100755 index 000000000..03233f04a --- /dev/null +++ b/package/Tests/PlayModeTests/Dynamics/Jacobians/AngularLimit2DJacobianTests.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: dfb19d570b45e8e44958ac212d683c3c +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Tests/PlayModeTests/Dynamics/Jacobians/AngularLimit3DJacobianTests.cs b/package/Tests/PlayModeTests/Dynamics/Jacobians/AngularLimit3DJacobianTests.cs new file mode 100755 index 000000000..8347a7a65 --- /dev/null +++ b/package/Tests/PlayModeTests/Dynamics/Jacobians/AngularLimit3DJacobianTests.cs @@ -0,0 +1,51 @@ +using NUnit.Framework; +using Unity.Mathematics; +using static Unity.Physics.Math; +using Assert = UnityEngine.Assertions.Assert; + +namespace Unity.Physics.Tests.Dynamics.Jacobians +{ + public class AngularLimit3DJacobianTests + { + [Test] + public void BuildTest() + { + var jacobian = new AngularLimit3DJacobian(); + + var aFromConstraint = MTransform.Identity; + var bFromConstraint = MTransform.Identity; + var velocityA = MotionVelocity.Zero; + var velocityB = MotionVelocity.Zero; + var motionA = MotionData.Zero; + var motionB = MotionData.Zero; + var constraint = new Constraint() { ConstrainedAxes = new bool3(true, true, true) }; + var tau = 1.0f; + var damping = 1.0f; + + jacobian.Build(aFromConstraint, bFromConstraint, velocityA, velocityB, motionA, motionB, constraint, tau, damping); + + Assert.AreEqual(0.0f, jacobian.MinAngle); + Assert.AreEqual(0.0f, jacobian.MaxAngle); + } + + [Test] + public void SolveTest() + { + var jacobian = new AngularLimit3DJacobian() + { + BFromA = quaternion.identity, + MotionAFromJoint = quaternion.identity, + MotionBFromJoint = quaternion.identity + }; + + var velocityA = MotionVelocity.Zero; + var velocityB = MotionVelocity.Zero; + var timestep = 1.0f; + + jacobian.Solve(ref velocityA, ref velocityB, timestep); + + Assert.AreEqual(MotionVelocity.Zero, velocityA); + Assert.AreEqual(MotionVelocity.Zero, velocityB); + } + } +} diff --git a/package/Tests/PlayModeTests/Dynamics/Jacobians/AngularLimit3DJacobianTests.cs.meta b/package/Tests/PlayModeTests/Dynamics/Jacobians/AngularLimit3DJacobianTests.cs.meta new file mode 100755 index 000000000..642430bcf --- /dev/null +++ b/package/Tests/PlayModeTests/Dynamics/Jacobians/AngularLimit3DJacobianTests.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 029cbad49b2c73e40aab0d42b5a0d40a +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Tests/PlayModeTests/Dynamics/Jacobians/ContactJacobianTests.cs b/package/Tests/PlayModeTests/Dynamics/Jacobians/ContactJacobianTests.cs new file mode 100755 index 000000000..9e2170382 --- /dev/null +++ b/package/Tests/PlayModeTests/Dynamics/Jacobians/ContactJacobianTests.cs @@ -0,0 +1,30 @@ +using NUnit.Framework; +using Unity.Collections; +using Unity.Mathematics; +using static Unity.Physics.Math; +using Assert = UnityEngine.Assertions.Assert; + +namespace Unity.Physics.Tests.Dynamics.Jacobians +{ + public class ContactJacobiansTests + { + [Test] + public void SolveTest() + { + var jacobian = new ContactJacobian(); + + var jacHeader = new JacobianHeader(); + var velocityA = MotionVelocity.Zero; + var velocityB = MotionVelocity.Zero; + var stepInput = new Solver.StepInput(); + var collisionEventsWriter = new BlockStream.Writer(); + + jacobian.Solve(ref jacHeader, ref velocityA, ref velocityB, stepInput, ref collisionEventsWriter); + + Assert.AreEqual(new JacobianHeader(), jacHeader); + Assert.AreEqual(MotionVelocity.Zero, velocityA); + Assert.AreEqual(MotionVelocity.Zero, velocityB); + Assert.AreEqual(new BlockStream.Writer(), collisionEventsWriter); + } + } +} diff --git a/package/Tests/PlayModeTests/Dynamics/Jacobians/ContactJacobianTests.cs.meta b/package/Tests/PlayModeTests/Dynamics/Jacobians/ContactJacobianTests.cs.meta new file mode 100755 index 000000000..f334ea860 --- /dev/null +++ b/package/Tests/PlayModeTests/Dynamics/Jacobians/ContactJacobianTests.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 0918a9edc8ff48b4dbd5f755bfb9ef87 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Tests/PlayModeTests/Dynamics/Jacobians/JacobianTests.cs b/package/Tests/PlayModeTests/Dynamics/Jacobians/JacobianTests.cs new file mode 100755 index 000000000..b9427de61 --- /dev/null +++ b/package/Tests/PlayModeTests/Dynamics/Jacobians/JacobianTests.cs @@ -0,0 +1,85 @@ +using NUnit.Framework; +using Unity.Collections; +using Unity.Mathematics; +using static Unity.Physics.Math; +using Assert = UnityEngine.Assertions.Assert; + +namespace Unity.Physics.Tests.Dynamics.Jacobians +{ + public class JacobiansTests + { + [Test] + public void JacobianUtilitiesCalculateTauAndDampingTest() + { + float springFrequency = 1.0f; + float springDampingRatio = 1.0f; + float timestep = 1.0f; + int iterations = 4; + + JacobianUtilities.CalculateTauAndDamping(springFrequency, springDampingRatio, timestep, iterations, out float tau, out float damping); + + Assert.AreApproximatelyEqual(0.4774722f, tau); + Assert.AreApproximatelyEqual(0.6294564f, damping); + } + + [Test] + public void JacobianUtilitiesCalculateTauAndDampingFromConstraintTest() + { + var constraint = new Constraint { SpringFrequency = 1.0f, SpringDamping = 1.0f }; + float timestep = 1.0f; + int iterations = 4; + + float tau; + float damping; + JacobianUtilities.CalculateTauAndDamping(constraint, timestep, iterations, out tau, out damping); + + Assert.AreApproximatelyEqual(0.4774722f, tau); + Assert.AreApproximatelyEqual(0.6294564f, damping); + } + + [Test] + public void JacobianUtilitiesCalculateErrorTest() + { + float x = 5.0f; + float min = 0.0f; + float max = 10.0f; + + Assert.AreApproximatelyEqual(0.0f, JacobianUtilities.CalculateError(x, min, max)); + } + + [Test] + public void JacobianUtilitiesCalculateCorrectionTest() + { + float predictedError = 0.2f; + float initialError = 0.1f; + float tau = 0.6f; + float damping = 1.0f; + + Assert.AreApproximatelyEqual(0.16f, JacobianUtilities.CalculateCorrection(predictedError, initialError, tau, damping)); + } + + [Test] + public void JacobianUtilitiesIntegrateOrientationBFromATest() + { + var bFromA = quaternion.identity; + var angularVelocityA = float3.zero; + var angularVelocityB = float3.zero; + var timestep = 1.0f; + + Assert.AreEqual(quaternion.identity, JacobianUtilities.IntegrateOrientationBFromA(bFromA, angularVelocityA, angularVelocityB, timestep)); + } + + [Test] + public void JacobianIteratorHasJacobiansLeftTest() + { + var jacobianStream = new BlockStream(1, 0x01234567); + BlockStream.Reader jacobianStreamReader = jacobianStream; + int workItemIndex = 0; + var jacIterator = new JacobianIterator(jacobianStreamReader, workItemIndex); + + Assert.IsFalse(jacIterator.HasJacobiansLeft()); + + jacobianStream.Dispose(); + } + } +} diff --git a/package/Tests/PlayModeTests/Dynamics/Jacobians/JacobianTests.cs.meta b/package/Tests/PlayModeTests/Dynamics/Jacobians/JacobianTests.cs.meta new file mode 100755 index 000000000..76b6497b3 --- /dev/null +++ b/package/Tests/PlayModeTests/Dynamics/Jacobians/JacobianTests.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: b5a0a85cdaa715a469eeaaff80ca00e1 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Tests/PlayModeTests/Dynamics/Jacobians/LinearLimitJacobianTests.cs b/package/Tests/PlayModeTests/Dynamics/Jacobians/LinearLimitJacobianTests.cs new file mode 100755 index 000000000..bdd6857a1 --- /dev/null +++ b/package/Tests/PlayModeTests/Dynamics/Jacobians/LinearLimitJacobianTests.cs @@ -0,0 +1,56 @@ +using NUnit.Framework; +using Unity.Mathematics; +using static Unity.Physics.Math; +using Assert = UnityEngine.Assertions.Assert; + +namespace Unity.Physics.Tests.Dynamics.Jacobians +{ + public class LinearLimitJacobianTests + { + [Test] + public void BuildTest() + { + var jacobian = new LinearLimitJacobian(); + + var aFromConstraint = MTransform.Identity; + var bFromConstraint = MTransform.Identity; + var velocityA = MotionVelocity.Zero; + var velocityB = MotionVelocity.Zero; + var motionA = MotionData.Zero; + var motionB = MotionData.Zero; + var constraint = new Constraint() { ConstrainedAxes = new bool3(true, true, true) }; + var tau = 1.0f; + var damping = 1.0f; + + jacobian.Build(aFromConstraint, bFromConstraint, velocityA, velocityB, motionA, motionB, constraint, tau, damping); + + Assert.AreEqual(RigidTransform.identity, jacobian.WorldFromA); + Assert.AreEqual(RigidTransform.identity, jacobian.WorldFromB); + Assert.AreEqual(float3.zero, jacobian.PivotAinA); + Assert.AreEqual(float3.zero, jacobian.PivotBinB); + Assert.AreEqual(float3.zero, jacobian.AxisInB); + Assert.IsFalse(jacobian.Is1D); + Assert.AreEqual(0.0f, jacobian.MinDistance); + Assert.AreEqual(0.0f, jacobian.MaxDistance); + Assert.AreEqual(1.0f, jacobian.Tau); + Assert.AreEqual(1.0f, jacobian.Damping); + + Assert.AreEqual(0.0f, jacobian.InitialError); + } + + [Test] + public void SolveTest() + { + var jacobian = new LinearLimitJacobian() { WorldFromA = RigidTransform.identity, WorldFromB = RigidTransform.identity }; + + var velocityA = MotionVelocity.Zero; + var velocityB = MotionVelocity.Zero; + var timestep = 1.0f; + + jacobian.Solve(ref velocityA, ref velocityB, timestep); + + Assert.AreEqual(MotionVelocity.Zero, velocityA); + Assert.AreEqual(MotionVelocity.Zero, velocityB); + } + } +} diff --git a/package/Tests/PlayModeTests/Dynamics/Jacobians/LinearLimitJacobianTests.cs.meta b/package/Tests/PlayModeTests/Dynamics/Jacobians/LinearLimitJacobianTests.cs.meta new file mode 100755 index 000000000..0e6fa85a4 --- /dev/null +++ b/package/Tests/PlayModeTests/Dynamics/Jacobians/LinearLimitJacobianTests.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 6320481d4720cee44a69e1a222a39a34 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Tests/PlayModeTests/Dynamics/Joint.meta b/package/Tests/PlayModeTests/Dynamics/Joint.meta new file mode 100755 index 000000000..8c54adb12 --- /dev/null +++ b/package/Tests/PlayModeTests/Dynamics/Joint.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: f3d9ea0a7995b77438f80ea5e7eb6cd4 +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Tests/PlayModeTests/Dynamics/Joint/JointTests.cs b/package/Tests/PlayModeTests/Dynamics/Joint/JointTests.cs new file mode 100755 index 000000000..130074572 --- /dev/null +++ b/package/Tests/PlayModeTests/Dynamics/Joint/JointTests.cs @@ -0,0 +1,261 @@ +using System.Collections; +using System.Collections.Generic; +using NUnit.Framework; +using Unity.Mathematics; +using Unity.Entities; +using UnityEngine; +using UnityEngine.TestTools; +using static Unity.Physics.Math; + +namespace Unity.Physics.Tests.Dynamics.Joint +{ + public class JointTests + { + [Test] + public void JointDataCreateTest() + { + var aFromJoint = MTransform.Identity; + var bFromJoint = MTransform.Identity; + var constraints = new Constraint[0]; + + var jointDataRef = JointData.Create(aFromJoint, bFromJoint, constraints); + + var jointData = jointDataRef.Value; + + Assert.AreEqual(MTransform.Identity, jointData.AFromJoint); + Assert.AreEqual(MTransform.Identity, jointData.BFromJoint); + Assert.AreEqual(1, jointData.Version); + } + + [Test] + public void JointDataCreateBallAndSocketTest() + { + var positionAinA = float3.zero; + var positionBinB = float3.zero; + + var jointDataRef = JointData.CreateBallAndSocket(positionAinA, positionBinB); + + var jointData = jointDataRef.Value; + Assert.AreEqual(MTransform.Identity, jointData.AFromJoint); + Assert.AreEqual(MTransform.Identity, jointData.BFromJoint); + Assert.AreEqual(1, jointData.Version); + Assert.AreEqual(1, jointData.NumConstraints); + + var constraint = jointDataRef.Value.Constraints[0]; + Assert.AreEqual(new bool3(true), constraint.ConstrainedAxes); + Assert.AreEqual(ConstraintType.Linear, constraint.Type); + Assert.AreEqual(0.0f, constraint.Min); + Assert.AreEqual(0.0f, constraint.Max); + Assert.AreEqual(Constraint.DefaultSpringFrequency, constraint.SpringFrequency); + Assert.AreEqual(Constraint.DefaultSpringDamping, constraint.SpringDamping); + } + + [Test] + public void JointDataCreateStiffSpringTest() + { + var positionAinA = new float3(0.0f, 1.0f, 2.0f); + var positionBinB = new float3(1.0f, 0.0f, 3.0f); + var minDistance = 1.0f; + var maxDistance = 10.0f; + + var jointDataRef = JointData.CreateStiffSpring(positionAinA, positionBinB, minDistance, maxDistance); + + var jointData = jointDataRef.Value; + Assert.AreEqual(positionAinA, jointData.AFromJoint.Translation); + Assert.AreEqual(positionBinB, jointData.BFromJoint.Translation); + Assert.AreEqual(1, jointData.Version); + Assert.AreEqual(1, jointData.NumConstraints); + + var constraint = jointDataRef.Value.Constraints[0]; + Assert.AreEqual(new bool3(true), constraint.ConstrainedAxes); + Assert.AreEqual(ConstraintType.Linear, constraint.Type); + Assert.AreEqual(1.0f, constraint.Min); + Assert.AreEqual(10.0f, constraint.Max); + Assert.AreEqual(Constraint.DefaultSpringFrequency, constraint.SpringFrequency); + Assert.AreEqual(Constraint.DefaultSpringDamping, constraint.SpringDamping); + } + + [Test] + public void JointDataCreatePrismaticTest() + { + var positionAinA = new float3(0.0f, 1.0f, 2.0f); + var positionBinB = new float3(1.0f, 0.0f, 3.0f); + var axisInB = float3.zero; + + var minDistanceOnAxis = 1.0f; + var maxDistanceOnAxis = 10.0f; + var minDistanceFromAxis = 2.0f; + var maxDistanceFromAxis = 20.0f; + + var jointDataRef = JointData.CreatePrismatic(positionAinA, positionBinB, axisInB, minDistanceOnAxis, maxDistanceOnAxis, minDistanceFromAxis, maxDistanceFromAxis); + + var jointData = jointDataRef.Value; + Assert.AreEqual(positionAinA, jointData.AFromJoint.Translation); + Assert.AreEqual(positionBinB, jointData.BFromJoint.Translation); + Assert.AreEqual(1, jointData.Version); + Assert.AreEqual(2, jointData.NumConstraints); + + var planarConstraint = jointDataRef.Value.Constraints[0]; + Assert.AreEqual(new bool3(true, false, false), planarConstraint.ConstrainedAxes); + Assert.AreEqual(ConstraintType.Linear, planarConstraint.Type); + Assert.AreEqual(1.0f, planarConstraint.Min); + Assert.AreEqual(10.0f, planarConstraint.Max); + Assert.AreEqual(Constraint.DefaultSpringFrequency, planarConstraint.SpringFrequency); + Assert.AreEqual(Constraint.DefaultSpringDamping, planarConstraint.SpringDamping); + + var cylindricalConstraint = jointDataRef.Value.Constraints[1]; + Assert.AreEqual(new bool3(false, true, true), cylindricalConstraint.ConstrainedAxes); + Assert.AreEqual(ConstraintType.Linear, cylindricalConstraint.Type); + Assert.AreEqual(2.0f, cylindricalConstraint.Min); + Assert.AreEqual(20.0f, cylindricalConstraint.Max); + Assert.AreEqual(Constraint.DefaultSpringFrequency, cylindricalConstraint.SpringFrequency); + Assert.AreEqual(Constraint.DefaultSpringDamping, cylindricalConstraint.SpringDamping); + } + + [Test] + public void JointDataCreateHingeTest() + { + var positionAinA = new float3(0.0f, 1.0f, 2.0f); + var positionBinB = new float3(1.0f, 0.0f, 3.0f); + var axisInA = float3.zero; + var axisInB = float3.zero; + + var jointDataRef = JointData.CreateHinge(positionAinA, positionBinB, axisInA, axisInB); + + var jointData = jointDataRef.Value; + Assert.AreEqual(positionAinA, jointData.AFromJoint.Translation); + Assert.AreEqual(positionBinB, jointData.BFromJoint.Translation); + Assert.AreEqual(1, jointData.Version); + Assert.AreEqual(2, jointData.NumConstraints); + + var hingeConstraint = jointDataRef.Value.Constraints[0]; + Assert.AreEqual(new bool3(false, true, true), hingeConstraint.ConstrainedAxes); + Assert.AreEqual(ConstraintType.Angular, hingeConstraint.Type); + Assert.AreEqual(0.0f, hingeConstraint.Min); + Assert.AreEqual(0.0f, hingeConstraint.Max); + Assert.AreEqual(Constraint.DefaultSpringFrequency, hingeConstraint.SpringFrequency); + Assert.AreEqual(Constraint.DefaultSpringDamping, hingeConstraint.SpringDamping); + + var ballAndSocketConstraint = jointDataRef.Value.Constraints[1]; + Assert.AreEqual(new bool3(true, true, true), ballAndSocketConstraint.ConstrainedAxes); + Assert.AreEqual(ConstraintType.Linear, ballAndSocketConstraint.Type); + Assert.AreEqual(0.0f, ballAndSocketConstraint.Min); + Assert.AreEqual(0.0f, ballAndSocketConstraint.Max); + Assert.AreEqual(Constraint.DefaultSpringFrequency, ballAndSocketConstraint.SpringFrequency); + Assert.AreEqual(Constraint.DefaultSpringDamping, ballAndSocketConstraint.SpringDamping); + } + + [Test] + public void JointDataCreateLimitedHingeTest() + { + var positionAinA = new float3(0.0f, 1.0f, 2.0f); + var positionBinB = new float3(1.0f, 0.0f, 3.0f); + var axisInA = new float3(1.0f, 0.0f, 0.0f); + var axisInB = new float3(1.0f, 0.0f, 0.0f); + var perpendicularInA = new float3(0.0f, 1.0f, 0.0f); + var perpendicularInB = new float3(0.0f, 1.0f, 0.0f); + var minAngle = -0.5f; + var maxAngle = 0.5f; + + var jointDataRef = JointData.CreateLimitedHinge(positionAinA, positionBinB, axisInA, axisInB, perpendicularInA, perpendicularInB, minAngle, maxAngle); + + var jointData = jointDataRef.Value; + Assert.AreEqual(positionAinA, jointData.AFromJoint.Translation); + Assert.AreEqual(positionBinB, jointData.BFromJoint.Translation); + Assert.AreEqual(1, jointData.Version); + Assert.AreEqual(3, jointData.NumConstraints); + + var twistConstraint = jointDataRef.Value.Constraints[0]; + Assert.AreEqual(new bool3(true, false, false), twistConstraint.ConstrainedAxes); + Assert.AreEqual(ConstraintType.Angular, twistConstraint.Type); + Assert.AreEqual(-0.5f, twistConstraint.Min); + Assert.AreEqual(0.5f, twistConstraint.Max); + Assert.AreEqual(Constraint.DefaultSpringFrequency, twistConstraint.SpringFrequency); + Assert.AreEqual(Constraint.DefaultSpringDamping, twistConstraint.SpringDamping); + + var hingeConstraint = jointDataRef.Value.Constraints[1]; + Assert.AreEqual(new bool3(false, true, true), hingeConstraint.ConstrainedAxes); + Assert.AreEqual(ConstraintType.Angular, hingeConstraint.Type); + Assert.AreEqual(0.0f, hingeConstraint.Min); + Assert.AreEqual(0.0f, hingeConstraint.Max); + Assert.AreEqual(Constraint.DefaultSpringFrequency, hingeConstraint.SpringFrequency); + Assert.AreEqual(Constraint.DefaultSpringDamping, hingeConstraint.SpringDamping); + + var ballAndSocketConstraint = jointDataRef.Value.Constraints[2]; + Assert.AreEqual(new bool3(true, true, true), ballAndSocketConstraint.ConstrainedAxes); + Assert.AreEqual(ConstraintType.Linear, ballAndSocketConstraint.Type); + Assert.AreEqual(0.0f, ballAndSocketConstraint.Min); + Assert.AreEqual(0.0f, ballAndSocketConstraint.Max); + Assert.AreEqual(Constraint.DefaultSpringFrequency, ballAndSocketConstraint.SpringFrequency); + Assert.AreEqual(Constraint.DefaultSpringDamping, ballAndSocketConstraint.SpringDamping); + } + + [Test] + public void JointDataCreateRagdollTest() + { + var positionAinA = new float3(0.0f, 1.0f, 2.0f); + var positionBinB = new float3(1.0f, 0.0f, 3.0f); + var twistAxisInA = new float3(1.0f, 0.0f, 0.0f); + var twistAxisInB = new float3(1.0f, 0.0f, 0.0f); + var perpendicularInA = new float3(0.0f, 1.0f, 0.0f); + var perpendicularInB = new float3(0.0f, 1.0f, 0.0f); + var maxConeAngle = 0.8f; + var minPerpendicularAngle = 0.1f; + var maxPerpendicularAngle = 1.1f; + var minTwistAngle = 0.2f; + var maxTwistAngle = 1.2f; + + BlobAssetReference jointData0; + BlobAssetReference jointData1; + + JointData.CreateRagdoll(positionAinA, positionBinB, twistAxisInA, twistAxisInB, perpendicularInA, perpendicularInB, + maxConeAngle, minPerpendicularAngle, maxPerpendicularAngle, minTwistAngle, maxTwistAngle, + out jointData0, out jointData1); + + var joint0 = jointData0.Value; + Assert.AreEqual(positionAinA, joint0.AFromJoint.Translation); + Assert.AreEqual(positionBinB, joint0.BFromJoint.Translation); + Assert.AreEqual(1, joint0.Version); + Assert.AreEqual(2, joint0.NumConstraints); + + var twistConstraint = jointData0.Value.Constraints[0]; + Assert.AreEqual(new bool3(true, false, false), twistConstraint.ConstrainedAxes); + Assert.AreEqual(ConstraintType.Angular, twistConstraint.Type); + Assert.AreEqual(0.2f, twistConstraint.Min); + Assert.AreEqual(1.2f, twistConstraint.Max); + Assert.AreEqual(Constraint.DefaultSpringFrequency, twistConstraint.SpringFrequency); + Assert.AreEqual(Constraint.DefaultSpringDamping, twistConstraint.SpringDamping); + + var coneConstraint0 = jointData0.Value.Constraints[1]; + Assert.AreEqual(new bool3(false, true, true), coneConstraint0.ConstrainedAxes); + Assert.AreEqual(ConstraintType.Angular, coneConstraint0.Type); + Assert.AreEqual(0.0f, coneConstraint0.Min); + Assert.AreEqual(0.8f, coneConstraint0.Max); + Assert.AreEqual(Constraint.DefaultSpringFrequency, coneConstraint0.SpringFrequency); + Assert.AreEqual(Constraint.DefaultSpringDamping, coneConstraint0.SpringDamping); + + var joint1 = jointData1.Value; + Assert.AreEqual(positionAinA, joint1.AFromJoint.Translation); + Assert.AreEqual(positionBinB, joint1.BFromJoint.Translation); + Assert.AreEqual(1, joint1.Version); + Assert.AreEqual(2, joint1.NumConstraints); + + var coneConstraint1 = jointData0.Value.Constraints[0]; + Assert.AreEqual(new bool3(true, false, false), coneConstraint1.ConstrainedAxes); + Assert.AreEqual(ConstraintType.Angular, coneConstraint1.Type); + Assert.AreEqual(0.2f, coneConstraint1.Min); + Assert.AreEqual(1.2f, coneConstraint1.Max); + Assert.AreEqual(Constraint.DefaultSpringFrequency, coneConstraint1.SpringFrequency); + Assert.AreEqual(Constraint.DefaultSpringDamping, coneConstraint1.SpringDamping); + + var ballAndSocketConstraint = jointData0.Value.Constraints[1]; + Assert.AreEqual(new bool3(false, true, true), ballAndSocketConstraint.ConstrainedAxes); + Assert.AreEqual(ConstraintType.Angular, ballAndSocketConstraint.Type); + Assert.AreEqual(0.0f, ballAndSocketConstraint.Min); + Assert.AreEqual(0.8f, ballAndSocketConstraint.Max); + Assert.AreEqual(Constraint.DefaultSpringFrequency, ballAndSocketConstraint.SpringFrequency); + Assert.AreEqual(Constraint.DefaultSpringDamping, ballAndSocketConstraint.SpringDamping); + } + + } +} diff --git a/package/Tests/PlayModeTests/Dynamics/Joint/JointTests.cs.meta b/package/Tests/PlayModeTests/Dynamics/Joint/JointTests.cs.meta new file mode 100755 index 000000000..ffd0045d4 --- /dev/null +++ b/package/Tests/PlayModeTests/Dynamics/Joint/JointTests.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 15b80a0a28f80f24e84e3e5c73ff1261 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Tests/PlayModeTests/Dynamics/Material.meta b/package/Tests/PlayModeTests/Dynamics/Material.meta new file mode 100755 index 000000000..71d3b28d3 --- /dev/null +++ b/package/Tests/PlayModeTests/Dynamics/Material.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: 00efdf243d4ae79408b791754d730da6 +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Tests/PlayModeTests/Dynamics/Material/MaterialTests.cs b/package/Tests/PlayModeTests/Dynamics/Material/MaterialTests.cs new file mode 100755 index 000000000..10d92d794 --- /dev/null +++ b/package/Tests/PlayModeTests/Dynamics/Material/MaterialTests.cs @@ -0,0 +1,296 @@ +using System.Collections; +using System.Collections.Generic; +using NUnit.Framework; +using UnityEngine; +using UnityEngine.TestTools; + +namespace Unity.Physics.Tests.Dynamics.Materials +{ + public class MaterialTests + { + [Test] + public void FrictionCombinePolicyTest() + { + Material mat1 = new Material(); + Material mat2 = new Material(); + float combinedFriction = 0; + + // GeometricMean Tests + mat1.FrictionCombinePolicy = Material.CombinePolicy.GeometricMean; + mat2.FrictionCombinePolicy = Material.CombinePolicy.GeometricMean; + + mat1.Friction = 1.0f; + mat2.Friction = 0.0f; + combinedFriction = Material.GetCombinedFriction(mat1, mat2); + Assert.IsTrue(combinedFriction == 0.0f); + + mat1.Friction = 0.5f; + mat2.Friction = 0.5f; + combinedFriction = Material.GetCombinedFriction(mat1, mat2); + Assert.IsTrue(combinedFriction == 0.5f); + + mat1.Friction = 1.0f; + mat2.Friction = 0.25f; + combinedFriction = Material.GetCombinedFriction(mat1, mat2); + Assert.IsTrue(combinedFriction == 0.5f); + + // Minimum Tests + mat1.FrictionCombinePolicy = Material.CombinePolicy.Minimum; + mat2.FrictionCombinePolicy = Material.CombinePolicy.Minimum; + mat1.Friction = 1.0f; + mat2.Friction = 0.0f; + combinedFriction = Material.GetCombinedFriction(mat1, mat2); + Assert.IsTrue(combinedFriction == 0.0f); + + mat1.Friction = 0.5f; + mat2.Friction = 0.5f; + combinedFriction = Material.GetCombinedFriction(mat1, mat2); + Assert.IsTrue(combinedFriction == 0.5f); + + mat1.Friction = 1.0f; + mat2.Friction = 0.5f; + combinedFriction = Material.GetCombinedFriction(mat1, mat2); + Assert.IsTrue(combinedFriction == 0.5f); + + // Maximum Tests + mat1.FrictionCombinePolicy = Material.CombinePolicy.Maximum; + mat2.FrictionCombinePolicy = Material.CombinePolicy.Maximum; + mat1.Friction = 1.0f; + mat2.Friction = 0.0f; + combinedFriction = Material.GetCombinedFriction(mat1, mat2); + Assert.IsTrue(combinedFriction == 1.0f); + + mat1.Friction = 0.5f; + mat2.Friction = 0.5f; + combinedFriction = Material.GetCombinedFriction(mat1, mat2); + Assert.IsTrue(combinedFriction == 0.5f); + + mat1.Friction = 2.0f; + mat2.Friction = 0.5f; + combinedFriction = Material.GetCombinedFriction(mat1, mat2); + Assert.IsTrue(combinedFriction == 2.0f); + + // ArithmeticMean Tests + mat1.FrictionCombinePolicy = Material.CombinePolicy.ArithmeticMean; + mat2.FrictionCombinePolicy = Material.CombinePolicy.ArithmeticMean; + mat1.Friction = 1.0f; + mat2.Friction = 0.0f; + combinedFriction = Material.GetCombinedFriction(mat1, mat2); + Assert.IsTrue(combinedFriction == 0.5f); + + mat1.Friction = 0.25f; + mat2.Friction = 0.75f; + combinedFriction = Material.GetCombinedFriction(mat1, mat2); + Assert.IsTrue(combinedFriction == 0.5f); + + mat1.Friction = 2.0f; + mat2.Friction = 0.5f; + combinedFriction = Material.GetCombinedFriction(mat1, mat2); + Assert.IsTrue(combinedFriction == 1.25f); + + // Mixed CombinePolicy Tests - Note that max(CombinePolicy of both materials) is used + mat1.FrictionCombinePolicy = Material.CombinePolicy.GeometricMean; + mat2.FrictionCombinePolicy = Material.CombinePolicy.ArithmeticMean; // this policy should be used + mat1.Friction = 2.0f; + mat2.Friction = 0.5f; + combinedFriction = Material.GetCombinedFriction(mat1, mat2); + Assert.IsTrue(combinedFriction == 1.25f); + //switch order + combinedFriction = Material.GetCombinedFriction(mat2, mat1); + Assert.IsTrue(combinedFriction == 1.25f); + + mat1.FrictionCombinePolicy = Material.CombinePolicy.GeometricMean; + mat2.FrictionCombinePolicy = Material.CombinePolicy.Maximum; // this policy should be used + mat1.Friction = 2.0f; + mat2.Friction = 0.5f; + combinedFriction = Material.GetCombinedFriction(mat1, mat2); + Assert.IsTrue(combinedFriction == 2.0f); + //switch order + combinedFriction = Material.GetCombinedFriction(mat2, mat1); + Assert.IsTrue(combinedFriction == 2.0f); + + mat1.FrictionCombinePolicy = Material.CombinePolicy.GeometricMean; + mat2.FrictionCombinePolicy = Material.CombinePolicy.Minimum; // this policy should be used + mat1.Friction = 2.0f; + mat2.Friction = 0.5f; + combinedFriction = Material.GetCombinedFriction(mat1, mat2); + Assert.IsTrue(combinedFriction == 0.5f); + //switch order + combinedFriction = Material.GetCombinedFriction(mat2, mat1); + Assert.IsTrue(combinedFriction == 0.5f); + + mat1.FrictionCombinePolicy = Material.CombinePolicy.Minimum; + mat2.FrictionCombinePolicy = Material.CombinePolicy.Maximum; // this policy should be used + mat1.Friction = 2.0f; + mat2.Friction = 0.5f; + combinedFriction = Material.GetCombinedFriction(mat1, mat2); + Assert.IsTrue(combinedFriction == 2.0f); + //switch order + combinedFriction = Material.GetCombinedFriction(mat2, mat1); + Assert.IsTrue(combinedFriction == 2.0f); + + mat1.FrictionCombinePolicy = Material.CombinePolicy.Minimum; + mat2.FrictionCombinePolicy = Material.CombinePolicy.ArithmeticMean; // this policy should be used + mat1.Friction = 2.0f; + mat2.Friction = 0.5f; + combinedFriction = Material.GetCombinedFriction(mat1, mat2); + Assert.IsTrue(combinedFriction == 1.25f); + //switch order + combinedFriction = Material.GetCombinedFriction(mat2, mat1); + Assert.IsTrue(combinedFriction == 1.25f); + + mat1.FrictionCombinePolicy = Material.CombinePolicy.Maximum; + mat2.FrictionCombinePolicy = Material.CombinePolicy.ArithmeticMean; // this policy should be used + mat1.Friction = 2.0f; + mat2.Friction = 0.5f; + combinedFriction = Material.GetCombinedFriction(mat1, mat2); + Assert.IsTrue(combinedFriction == 1.25f); + //switch order + combinedFriction = Material.GetCombinedFriction(mat2, mat1); + Assert.IsTrue(combinedFriction == 1.25f); + } + + + [Test] + public void RestitutionCombinePolicyTest() + { + Material mat1 = new Material(); + Material mat2 = new Material(); + float combinedRestitution = 0; + + // GeometricMean Tests + mat1.RestitutionCombinePolicy = Material.CombinePolicy.GeometricMean; + mat2.RestitutionCombinePolicy = Material.CombinePolicy.GeometricMean; + + mat1.Restitution = 1.0f; + mat2.Restitution = 0.0f; + combinedRestitution = Material.GetCombinedRestitution(mat1, mat2); + Assert.IsTrue(combinedRestitution == 0.0f); + + mat1.Restitution = 0.5f; + mat2.Restitution = 0.5f; + combinedRestitution = Material.GetCombinedRestitution(mat1, mat2); + Assert.IsTrue(combinedRestitution == 0.5f); + + mat1.Restitution = 1.0f; + mat2.Restitution = 0.25f; + combinedRestitution = Material.GetCombinedRestitution(mat1, mat2); + Assert.IsTrue(combinedRestitution == 0.5f); + + // Minimum Tests + mat1.RestitutionCombinePolicy = Material.CombinePolicy.Minimum; + mat2.RestitutionCombinePolicy = Material.CombinePolicy.Minimum; + mat1.Restitution = 1.0f; + mat2.Restitution = 0.0f; + combinedRestitution = Material.GetCombinedRestitution(mat1, mat2); + Assert.IsTrue(combinedRestitution == 0.0f); + + mat1.Restitution = 0.5f; + mat2.Restitution = 0.5f; + combinedRestitution = Material.GetCombinedRestitution(mat1, mat2); + Assert.IsTrue(combinedRestitution == 0.5f); + + mat1.Restitution = 1.0f; + mat2.Restitution = 0.5f; + combinedRestitution = Material.GetCombinedRestitution(mat1, mat2); + Assert.IsTrue(combinedRestitution == 0.5f); + + // Maximum Tests + mat1.RestitutionCombinePolicy = Material.CombinePolicy.Maximum; + mat2.RestitutionCombinePolicy = Material.CombinePolicy.Maximum; + mat1.Restitution = 1.0f; + mat2.Restitution = 0.0f; + combinedRestitution = Material.GetCombinedRestitution(mat1, mat2); + Assert.IsTrue(combinedRestitution == 1.0f); + + mat1.Restitution = 0.5f; + mat2.Restitution = 0.5f; + combinedRestitution = Material.GetCombinedRestitution(mat1, mat2); + Assert.IsTrue(combinedRestitution == 0.5f); + + mat1.Restitution = 2.0f; + mat2.Restitution = 0.5f; + combinedRestitution = Material.GetCombinedRestitution(mat1, mat2); + Assert.IsTrue(combinedRestitution == 2.0f); + + // ArithmeticMean Tests + mat1.RestitutionCombinePolicy = Material.CombinePolicy.ArithmeticMean; + mat2.RestitutionCombinePolicy = Material.CombinePolicy.ArithmeticMean; + mat1.Restitution = 1.0f; + mat2.Restitution = 0.0f; + combinedRestitution = Material.GetCombinedRestitution(mat1, mat2); + Assert.IsTrue(combinedRestitution == 0.5f); + + mat1.Restitution = 0.25f; + mat2.Restitution = 0.75f; + combinedRestitution = Material.GetCombinedRestitution(mat1, mat2); + Assert.IsTrue(combinedRestitution == 0.5f); + + mat1.Restitution = 2.0f; + mat2.Restitution = 0.5f; + combinedRestitution = Material.GetCombinedRestitution(mat1, mat2); + Assert.IsTrue(combinedRestitution == 1.25f); + + // Mixed CombinePolicy Tests - Note that max(CombinePolicy of both materials) is used + mat1.RestitutionCombinePolicy = Material.CombinePolicy.GeometricMean; + mat2.RestitutionCombinePolicy = Material.CombinePolicy.ArithmeticMean; // this policy should be used + mat1.Restitution = 2.0f; + mat2.Restitution = 0.5f; + combinedRestitution = Material.GetCombinedRestitution(mat1, mat2); + Assert.IsTrue(combinedRestitution == 1.25f); + //switch order + combinedRestitution = Material.GetCombinedRestitution(mat2, mat1); + Assert.IsTrue(combinedRestitution == 1.25f); + + mat1.RestitutionCombinePolicy = Material.CombinePolicy.GeometricMean; + mat2.RestitutionCombinePolicy = Material.CombinePolicy.Maximum; // this policy should be used + mat1.Restitution = 2.0f; + mat2.Restitution = 0.5f; + combinedRestitution = Material.GetCombinedRestitution(mat1, mat2); + Assert.IsTrue(combinedRestitution == 2.0f); + //switch order + combinedRestitution = Material.GetCombinedRestitution(mat2, mat1); + Assert.IsTrue(combinedRestitution == 2.0f); + + mat1.RestitutionCombinePolicy = Material.CombinePolicy.GeometricMean; + mat2.RestitutionCombinePolicy = Material.CombinePolicy.Minimum; // this policy should be used + mat1.Restitution = 2.0f; + mat2.Restitution = 0.5f; + combinedRestitution = Material.GetCombinedRestitution(mat1, mat2); + Assert.IsTrue(combinedRestitution == 0.5f); + //switch order + combinedRestitution = Material.GetCombinedRestitution(mat2, mat1); + Assert.IsTrue(combinedRestitution == 0.5f); + + mat1.RestitutionCombinePolicy = Material.CombinePolicy.Minimum; + mat2.RestitutionCombinePolicy = Material.CombinePolicy.Maximum; // this policy should be used + mat1.Restitution = 2.0f; + mat2.Restitution = 0.5f; + combinedRestitution = Material.GetCombinedRestitution(mat1, mat2); + Assert.IsTrue(combinedRestitution == 2.0f); + //switch order + combinedRestitution = Material.GetCombinedRestitution(mat2, mat1); + Assert.IsTrue(combinedRestitution == 2.0f); + + mat1.RestitutionCombinePolicy = Material.CombinePolicy.Minimum; + mat2.RestitutionCombinePolicy = Material.CombinePolicy.ArithmeticMean; // this policy should be used + mat1.Restitution = 2.0f; + mat2.Restitution = 0.5f; + combinedRestitution = Material.GetCombinedRestitution(mat1, mat2); + Assert.IsTrue(combinedRestitution == 1.25f); + //switch order + combinedRestitution = Material.GetCombinedRestitution(mat2, mat1); + Assert.IsTrue(combinedRestitution == 1.25f); + + mat1.RestitutionCombinePolicy = Material.CombinePolicy.Maximum; + mat2.RestitutionCombinePolicy = Material.CombinePolicy.ArithmeticMean; // this policy should be used + mat1.Restitution = 2.0f; + mat2.Restitution = 0.5f; + combinedRestitution = Material.GetCombinedRestitution(mat1, mat2); + Assert.IsTrue(combinedRestitution == 1.25f); + //switch order + combinedRestitution = Material.GetCombinedRestitution(mat2, mat1); + Assert.IsTrue(combinedRestitution == 1.25f); + } + } +} diff --git a/package/Tests/PlayModeTests/Dynamics/Material/MaterialTests.cs.meta b/package/Tests/PlayModeTests/Dynamics/Material/MaterialTests.cs.meta new file mode 100755 index 000000000..e04621438 --- /dev/null +++ b/package/Tests/PlayModeTests/Dynamics/Material/MaterialTests.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 3dea23326f560c14e8f4ef81e1f5db40 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Tests/PlayModeTests/Dynamics/Motion.meta b/package/Tests/PlayModeTests/Dynamics/Motion.meta new file mode 100755 index 000000000..b3335ced1 --- /dev/null +++ b/package/Tests/PlayModeTests/Dynamics/Motion.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: 9ba01e76c3917ad438bfc6eaee73a633 +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Tests/PlayModeTests/Dynamics/Motion/MotionTests.cs b/package/Tests/PlayModeTests/Dynamics/Motion/MotionTests.cs new file mode 100755 index 000000000..c26c69666 --- /dev/null +++ b/package/Tests/PlayModeTests/Dynamics/Motion/MotionTests.cs @@ -0,0 +1,88 @@ +using NUnit.Framework; +using Unity.Mathematics; +using Assert = UnityEngine.Assertions.Assert; + +namespace Unity.Physics.Tests.Dynamics.Motion +{ + public class MotionTests + { + [Test] + public void MassPropertiesUnitSphereTest() + { + var unitSphere = MassProperties.UnitSphere; + + Assert.AreEqual(float3.zero, unitSphere.MassDistribution.Transform.pos); + Assert.AreEqual(quaternion.identity, unitSphere.MassDistribution.Transform.rot); + Assert.AreEqual(new float3(0.4f), unitSphere.MassDistribution.InertiaTensor); + Assert.AreEqual(0.0f, unitSphere.AngularExpansionFactor); + } + + [Test] + public void MotionVelocityApplyLinearImpulseTest() + { + var motionVelocity = new MotionVelocity() + { + LinearVelocity = new float3(3.0f, 4.0f, 5.0f), + InverseInertiaAndMass = new float4(0.0f, 0.0f, 0.0f, 2.0f) + }; + motionVelocity.ApplyLinearImpulse(new float3(1.0f, 2.0f, 3.0f)); + + Assert.AreEqual(new float3(5.0f, 8.0f, 11.0f), motionVelocity.LinearVelocity); + } + + [Test] + public void MotionVelocityApplyAngularImpulseTest() + { + var motionVelocity = new MotionVelocity() + { + AngularVelocity = new float3(3.0f, 4.0f, 5.0f), + InverseInertiaAndMass = new float4(2.0f, 3.0f, 4.0f, 2.0f) + }; + motionVelocity.ApplyAngularImpulse(new float3(1.0f, 2.0f, 3.0f)); + + Assert.AreEqual(new float3(5.0f, 10.0f, 17.0f), motionVelocity.AngularVelocity); + } + + [Test] + public void MotionVelocityCalculateExpansionTest() + { + var motionVelocity = new MotionVelocity() + { + LinearVelocity = new float3(2.0f, 1.0f, 5.0f), + AngularVelocity = new float3(3.0f, 4.0f, 5.0f), + InverseInertiaAndMass = new float4(2.0f, 3.0f, 4.0f, 2.0f), + AngularExpansionFactor = 1.2f + }; + var motionExpansion = motionVelocity.CalculateExpansion(1.0f / 60.0f); + + Assert.AreEqual(new float3(1.0f / 30.0f, 1.0f / 60.0f, 1.0f / 12.0f), motionExpansion.Linear); + Assert.AreApproximatelyEqual((float)math.SQRT2 / 10.0f, motionExpansion.Uniform); + } + + [Test] + public void MotionExpansionMaxDistanceTest() + { + var motionExpansion = new MotionExpansion() + { + Linear = new float3(2.0f, 3.0f, 4.0f), + Uniform = 5.0f + }; + + Assert.AreEqual(math.sqrt(29.0f) + 5.0f, motionExpansion.MaxDistance); + } + + [Test] + public void MotionExpansionSweepAabbTest() + { + var motionExpansion = new MotionExpansion() + { + Linear = new float3(2.0f, 3.0f, 4.0f), + Uniform = 5.0f + }; + var aabb = motionExpansion.ExpandAabb(new Aabb() { Min = new float3(-10.0f, -10.0f, -10.0f), Max = new float3(10.0f, 10.0f, 10.0f) }); + + Assert.AreEqual(new float3(-15.0f, -15.0f, -15.0f), aabb.Min); + Assert.AreEqual(new float3(17.0f, 18.0f, 19.0f), aabb.Max); + } + } +} diff --git a/package/Tests/PlayModeTests/Dynamics/Motion/MotionTests.cs.meta b/package/Tests/PlayModeTests/Dynamics/Motion/MotionTests.cs.meta new file mode 100755 index 000000000..652de20df --- /dev/null +++ b/package/Tests/PlayModeTests/Dynamics/Motion/MotionTests.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: d7768d4f82d9b654291c604ae29a7d5d +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Tests/PlayModeTests/Dynamics/World.meta b/package/Tests/PlayModeTests/Dynamics/World.meta new file mode 100755 index 000000000..0c166f534 --- /dev/null +++ b/package/Tests/PlayModeTests/Dynamics/World.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: 86faf1eee34fa2240aa69e2e3de8be9a +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Tests/PlayModeTests/Dynamics/World/PhysicsWorldTests.cs b/package/Tests/PlayModeTests/Dynamics/World/PhysicsWorldTests.cs new file mode 100755 index 000000000..0d727a29e --- /dev/null +++ b/package/Tests/PlayModeTests/Dynamics/World/PhysicsWorldTests.cs @@ -0,0 +1,32 @@ +using NUnit.Framework; + +namespace Unity.Physics.Tests.Dynamics.PhysicsWorld +{ + public class PhysicsWorldTests + { + [Test] + public void WorldTest() + { + var world = new Physics.PhysicsWorld(10, 5, 10); + Assert.IsTrue((world.NumDynamicBodies == 5) && (world.NumStaticBodies == 10) && (world.NumBodies == 15) && (world.NumJoints == 10)); + + world.Reset(0, 0, 0); + Assert.IsTrue((world.NumDynamicBodies == 0) && (world.NumStaticBodies == 0) && (world.NumBodies == 0) && (world.NumJoints == 0)); + + world.Reset(5, 1, 7); + Assert.IsTrue((world.NumDynamicBodies == 1) && (world.NumStaticBodies == 5) && (world.NumBodies == 6) && (world.NumJoints == 7)); + + // clone world + var worldClone = new Physics.PhysicsWorld(0, 0, 0); + Assert.IsTrue((worldClone.NumDynamicBodies == 0) && (worldClone.NumStaticBodies == 0) && (worldClone.NumBodies == 0) && (worldClone.NumJoints == 0)); + + worldClone.Dispose(); + worldClone = (Physics.PhysicsWorld)world.Clone(); + Assert.IsTrue((worldClone.NumDynamicBodies == 1) && (worldClone.NumStaticBodies == 5) && (worldClone.NumBodies == 6) && (worldClone.NumJoints == 7)); + + // dispose cloned world + worldClone.Dispose(); + world.Dispose(); + } + } +} diff --git a/package/Tests/PlayModeTests/Dynamics/World/PhysicsWorldTests.cs.meta b/package/Tests/PlayModeTests/Dynamics/World/PhysicsWorldTests.cs.meta new file mode 100755 index 000000000..7326a3fa4 --- /dev/null +++ b/package/Tests/PlayModeTests/Dynamics/World/PhysicsWorldTests.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 63ed89eb54fa89444bf3c6fdd8d204da +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Tests/PlayModeTests/PerformanceTests.meta b/package/Tests/PlayModeTests/PerformanceTests.meta new file mode 100755 index 000000000..a0fa73786 --- /dev/null +++ b/package/Tests/PlayModeTests/PerformanceTests.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: 0db5c26545e917f4c8a48d003a22f805 +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Tests/PlayModeTests/PerformanceTests/RadixSortTests.cs b/package/Tests/PlayModeTests/PerformanceTests/RadixSortTests.cs new file mode 100755 index 000000000..e7e6e7029 --- /dev/null +++ b/package/Tests/PlayModeTests/PerformanceTests/RadixSortTests.cs @@ -0,0 +1,148 @@ +using NUnit.Framework; +using Unity.Collections; +using Unity.Jobs; +using Unity.Mathematics; +using Unity.PerformanceTesting; +using Assert = UnityEngine.Assertions.Assert; +using Random = UnityEngine.Random; + +namespace Unity.Physics.Tests.PerformanceTests +{ + public class RadixSortTests + { + public static void InitPairs(int minIndex, int maxIndex, int count, NativeArray pairs) + { + Random.InitState(1234); + + for (var i = 0; i < pairs.Length; ++i) + { + ulong indexA = (ulong)Random.Range(minIndex, maxIndex); + ulong indexB = (ulong)Random.Range(minIndex, maxIndex); + + if (indexB == indexA) + { + if (indexB < (ulong)maxIndex) + { + indexB++; + } + } + + pairs[i] = indexB << 40 | indexA << 16; + } + } + +#if UNITY_2019_2_OR_NEWER + [Test, Performance] +#else + [PerformanceTest] +#endif + [TestCase(1, TestName = "PerfRadixPassOnBodyA 1")] + [TestCase(10, TestName = "PerfRadixPassOnBodyA 10")] + [TestCase(100, TestName = "PerfRadixPassOnBodyA 100")] + [TestCase(1000, TestName = "PerfRadixPassOnBodyA 1000")] + [TestCase(10000, TestName = "PerfRadixPassOnBodyA 10 000")] + [TestCase(100000, TestName = "PerfRadixPassOnBodyA 100 000")] + public void PerfRadixPassOnBodyA(int count) + { + int maxBodyIndex = (int)math.pow(count, 0.7f); + int numDigits = 0; + int val = maxBodyIndex; + while (val > 0) + { + val >>= 1; + numDigits++; + } + + var pairs = new NativeArray(count, Allocator.TempJob); + var sortedPairs = new NativeArray(count, Allocator.TempJob); + var tempCount = new NativeArray(maxBodyIndex + 1, Allocator.TempJob); + + InitPairs(1, maxBodyIndex, count, pairs); + + var job = new Scheduler.RadixSortPerBodyAJob + { + InputArray = pairs, + OutputArray = sortedPairs, + DigitCount = tempCount, + MaxDigits = numDigits, + MaxIndex = maxBodyIndex + }; + + Measure.Method(() => + { + job.Run(); + }) + .Definition(sampleUnit: SampleUnit.Microsecond) + .MeasurementCount(1) + .Run(); + + for (int i = 0; i < count - 1; i++) + { + Assert.IsTrue((sortedPairs[i] & 0xffffff0000000000) <= (sortedPairs[i + 1] & 0xffffff0000000000), + $"Not sorted for index {i}, sortedPairs[i]= {sortedPairs[i]}, sortedPairs[i+1]= {sortedPairs[i + 1]}"); + } + + // Dispose all allocated data. + pairs.Dispose(); + sortedPairs.Dispose(); + tempCount.Dispose(); + } + +#if UNITY_2019_2_OR_NEWER + [Test, Performance] +#else + [PerformanceTest] +#endif + [TestCase(1, TestName = "PerfDefaultSortOnSubarrays 1")] + [TestCase(10, TestName = "PerfDefaultSortOnSubarrays 10")] + [TestCase(100, TestName = "PerfDefaultSortOnSubarrays 100")] + [TestCase(1000, TestName = "PerfDefaultSortOnSubarrays 1000")] + [TestCase(10000, TestName = "PerfDefaultSortOnSubarrays 10 000")] + [TestCase(100000, TestName = "PerfDefaultSortOnSubarrays 100 000")] + public void PerfDefaultSortOnSubarrays(int count) + { + int maxBodyIndex = (int)math.pow(count, 0.7f); + int numDigits = 0; + int val = maxBodyIndex; + while (val > 0) + { + val >>= 1; + numDigits++; + } + + var pairs = new NativeArray(count, Allocator.Temp); + var sortedPairs = new NativeArray(count, Allocator.Temp); + + InitPairs(1, maxBodyIndex, count, pairs); + + // Do a single pass of radix sort on bodyA only. + var tempCount = new NativeArray(maxBodyIndex + 1, Allocator.TempJob); + Scheduler.RadixSortPerBodyAJob.RadixSortPerBodyA(pairs, sortedPairs, tempCount, numDigits, maxBodyIndex, 16); + + var job = new Scheduler.SortSubArraysJob + { + InOutArray = sortedPairs, + NextElementIndex = tempCount + }; + + Measure.Method(() => + { + job.Run(tempCount.Length); + }) + .Definition(sampleUnit: SampleUnit.Microsecond) + .MeasurementCount(1) + .Run(); + + for (int i = 0; i < count - 1; i++) + { + Assert.IsTrue((sortedPairs[i] & 0x00000000ffffffff) < (sortedPairs[i + 1] & 0x00000000ffffffff) || + (sortedPairs[i] <= sortedPairs[i + 1]), + $"Not sorted for index {i}, sortedPairs[i] = {sortedPairs[i]}, sortedPairs[i+1] = {sortedPairs[i + 1]}"); + } + + // Dispose all allocated data. + pairs.Dispose(); + sortedPairs.Dispose(); + } + } +} diff --git a/package/Tests/PlayModeTests/PerformanceTests/RadixSortTests.cs.meta b/package/Tests/PlayModeTests/PerformanceTests/RadixSortTests.cs.meta new file mode 100755 index 000000000..37f71da5e --- /dev/null +++ b/package/Tests/PlayModeTests/PerformanceTests/RadixSortTests.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 76c603b26a6c0ca49a77826c6a7d475e +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Tests/PlayModeTests/Unity.Physics.PlayModeTests.asmdef b/package/Tests/PlayModeTests/Unity.Physics.PlayModeTests.asmdef new file mode 100755 index 000000000..4b9b60a9b --- /dev/null +++ b/package/Tests/PlayModeTests/Unity.Physics.PlayModeTests.asmdef @@ -0,0 +1,27 @@ +{ + "name": "Unity.Physics.PlayModeTests", + "references": [ + "Unity.Burst", + "Unity.Collections", + "Unity.Entities", + "Unity.Entities.Hybrid", + "Unity.Entities.Tests", + "Unity.Mathematics", + "Unity.PerformanceTesting", + "Unity.Physics", + "Unity.Physics.Authoring", + "Unity.Physics.Editor", + "Unity.Transforms" + ], + "optionalUnityReferences": [ + "TestAssemblies" + ], + "includePlatforms": [], + "excludePlatforms": [], + "allowUnsafeCode": true, + "overrideReferences": false, + "precompiledReferences": [], + "autoReferenced": true, + "defineConstraints": [], + "versionDefines": [] +} \ No newline at end of file diff --git a/package/Tests/PlayModeTests/Unity.Physics.PlayModeTests.asmdef.meta b/package/Tests/PlayModeTests/Unity.Physics.PlayModeTests.asmdef.meta new file mode 100755 index 000000000..2fff636aa --- /dev/null +++ b/package/Tests/PlayModeTests/Unity.Physics.PlayModeTests.asmdef.meta @@ -0,0 +1,7 @@ +fileFormatVersion: 2 +guid: 39ef29dfff7f0f4448aed3dda2a3f978 +AssemblyDefinitionImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Tests/PlayModeTests/Utils.meta b/package/Tests/PlayModeTests/Utils.meta new file mode 100755 index 000000000..8446e4bc8 --- /dev/null +++ b/package/Tests/PlayModeTests/Utils.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: b031ff7a95d6d9141880b30c8fcee20d +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Tests/PlayModeTests/Utils/TestUtils.cs b/package/Tests/PlayModeTests/Utils/TestUtils.cs new file mode 100755 index 000000000..3f3c2a606 --- /dev/null +++ b/package/Tests/PlayModeTests/Utils/TestUtils.cs @@ -0,0 +1,983 @@ +using Unity.Mathematics; +using Assert = UnityEngine.Assertions.Assert; +using Unity.Entities; +using Unity.Collections; +using Unity.Jobs; + +namespace Unity.Physics.Tests.Utils +{ + class TestUtils + { + public static void AreEqual(bool a, bool b) + { + Assert.AreEqual(a, b); + } + + public static void AreEqual(int a, int b) + { + Assert.AreEqual(a, b); + } + + public static void AreEqual(uint a, uint b) + { + Assert.AreEqual(a, b); + } + + public static void AreEqual(long a, long b) + { + Assert.AreEqual(a, b); + } + + public static void AreEqual(ulong a, ulong b) + { + Assert.AreEqual(a, b); + } + + public static void AreEqual(float a, float b, float delta = 0.0f) + { + Assert.AreApproximatelyEqual(a, b, delta); + } + + public static void AreEqual(float a, float b, int maxUlp, bool signedZeroEqual) + { + if (signedZeroEqual && a == b) + return; + + if (math.isfinite(a) && math.isfinite(b)) + { + int ia = math.asint(a); + int ib = math.asint(b); + if ((ia ^ ib) < 0) + Assert.AreEqual(true, false); + int ulps = math.abs(ia - ib); + Assert.AreEqual(true, ulps <= maxUlp); + } + else + { + if (a != b && (!math.isnan(a) || !math.isnan(b))) + Assert.AreEqual(true, false); + } + } + + public static void AreEqual(double a, double b, double delta = 0.0) + { + Assert.IsTrue(math.abs(a - b) < delta); + } + + public static void AreEqual(double a, double b, int maxUlp, bool signedZeroEqual) + { + if (signedZeroEqual && a == b) + return; + + if (math.isfinite(a) && math.isfinite(b)) + { + long la = math.aslong(a); + long lb = math.aslong(b); + if ((la ^ lb) < 0) + Assert.AreEqual(true, false); + long ulps = la > lb ? la - lb : lb - la; + Assert.AreEqual(true, ulps <= maxUlp); + } + else + { + if (a != b && (!math.isnan(a) || !math.isnan(b))) + Assert.AreEqual(true, false); + } + } + + + // bool + public static void AreEqual(bool2 a, bool2 b) + { + AreEqual(a.x, b.x); + AreEqual(a.y, b.y); + } + + public static void AreEqual(bool3 a, bool3 b) + { + AreEqual(a.x, b.x); + AreEqual(a.y, b.y); + AreEqual(a.z, b.z); + } + + public static void AreEqual(bool4 a, bool4 b) + { + AreEqual(a.x, b.x); + AreEqual(a.y, b.y); + AreEqual(a.z, b.z); + AreEqual(a.w, b.w); + } + + + public static void AreEqual(bool2x2 a, bool2x2 b) + { + AreEqual(a.c0, b.c0); + AreEqual(a.c1, b.c1); + } + + public static void AreEqual(bool3x2 a, bool3x2 b) + { + AreEqual(a.c0, b.c0); + AreEqual(a.c1, b.c1); + } + + public static void AreEqual(bool4x2 a, bool4x2 b) + { + AreEqual(a.c0, b.c0); + AreEqual(a.c1, b.c1); + } + + + public static void AreEqual(bool2x3 a, bool2x3 b) + { + AreEqual(a.c0, b.c0); + AreEqual(a.c1, b.c1); + AreEqual(a.c2, b.c2); + } + + public static void AreEqual(bool3x3 a, bool3x3 b) + { + AreEqual(a.c0, b.c0); + AreEqual(a.c1, b.c1); + AreEqual(a.c2, b.c2); + } + + public static void AreEqual(bool4x3 a, bool4x3 b) + { + AreEqual(a.c0, b.c0); + AreEqual(a.c1, b.c1); + AreEqual(a.c2, b.c2); + } + + + public static void AreEqual(bool2x4 a, bool2x4 b) + { + AreEqual(a.c0, b.c0); + AreEqual(a.c1, b.c1); + AreEqual(a.c2, b.c2); + AreEqual(a.c3, b.c3); + } + + public static void AreEqual(bool3x4 a, bool3x4 b) + { + AreEqual(a.c0, b.c0); + AreEqual(a.c1, b.c1); + AreEqual(a.c2, b.c2); + AreEqual(a.c3, b.c3); + } + + public static void AreEqual(bool4x4 a, bool4x4 b) + { + AreEqual(a.c0, b.c0); + AreEqual(a.c1, b.c1); + AreEqual(a.c2, b.c2); + AreEqual(a.c3, b.c3); + } + + // int + public static void AreEqual(int2 a, int2 b) + { + AreEqual(a.x, b.x); + AreEqual(a.y, b.y); + } + + public static void AreEqual(int3 a, int3 b) + { + AreEqual(a.x, b.x); + AreEqual(a.y, b.y); + AreEqual(a.z, b.z); + } + + public static void AreEqual(int4 a, int4 b) + { + AreEqual(a.x, b.x); + AreEqual(a.y, b.y); + AreEqual(a.z, b.z); + AreEqual(a.w, b.w); + } + + + public static void AreEqual(int2x2 a, int2x2 b) + { + AreEqual(a.c0, b.c0); + AreEqual(a.c1, b.c1); + } + + public static void AreEqual(int3x2 a, int3x2 b) + { + AreEqual(a.c0, b.c0); + AreEqual(a.c1, b.c1); + } + + public static void AreEqual(int4x2 a, int4x2 b) + { + AreEqual(a.c0, b.c0); + AreEqual(a.c1, b.c1); + } + + + public static void AreEqual(int2x3 a, int2x3 b) + { + AreEqual(a.c0, b.c0); + AreEqual(a.c1, b.c1); + AreEqual(a.c2, b.c2); + } + + public static void AreEqual(int3x3 a, int3x3 b) + { + AreEqual(a.c0, b.c0); + AreEqual(a.c1, b.c1); + AreEqual(a.c2, b.c2); + } + + public static void AreEqual(int4x3 a, int4x3 b) + { + AreEqual(a.c0, b.c0); + AreEqual(a.c1, b.c1); + AreEqual(a.c2, b.c2); + } + + + + public static void AreEqual(int2x4 a, int2x4 b) + { + AreEqual(a.c0, b.c0); + AreEqual(a.c1, b.c1); + AreEqual(a.c2, b.c2); + AreEqual(a.c3, b.c3); + } + + public static void AreEqual(int3x4 a, int3x4 b) + { + AreEqual(a.c0, b.c0); + AreEqual(a.c1, b.c1); + AreEqual(a.c2, b.c2); + AreEqual(a.c3, b.c3); + } + + public static void AreEqual(int4x4 a, int4x4 b) + { + AreEqual(a.c0, b.c0); + AreEqual(a.c1, b.c1); + AreEqual(a.c2, b.c2); + AreEqual(a.c3, b.c3); + } + + + // uint + public static void AreEqual(uint2 a, uint2 b) + { + AreEqual(a.x, b.x); + AreEqual(a.y, b.y); + } + + public static void AreEqual(uint3 a, uint3 b) + { + AreEqual(a.x, b.x); + AreEqual(a.y, b.y); + AreEqual(a.z, b.z); + } + + public static void AreEqual(uint4 a, uint4 b) + { + AreEqual(a.x, b.x); + AreEqual(a.y, b.y); + AreEqual(a.z, b.z); + AreEqual(a.w, b.w); + } + + + public static void AreEqual(uint2x2 a, uint2x2 b) + { + AreEqual(a.c0, b.c0); + AreEqual(a.c1, b.c1); + } + + public static void AreEqual(uint3x2 a, uint3x2 b) + { + AreEqual(a.c0, b.c0); + AreEqual(a.c1, b.c1); + } + + public static void AreEqual(uint4x2 a, uint4x2 b) + { + AreEqual(a.c0, b.c0); + AreEqual(a.c1, b.c1); + } + + + public static void AreEqual(uint2x3 a, uint2x3 b) + { + AreEqual(a.c0, b.c0); + AreEqual(a.c1, b.c1); + AreEqual(a.c2, b.c2); + } + + public static void AreEqual(uint3x3 a, uint3x3 b) + { + AreEqual(a.c0, b.c0); + AreEqual(a.c1, b.c1); + AreEqual(a.c2, b.c2); + } + + public static void AreEqual(uint4x3 a, uint4x3 b) + { + AreEqual(a.c0, b.c0); + AreEqual(a.c1, b.c1); + AreEqual(a.c2, b.c2); + } + + + public static void AreEqual(uint2x4 a, uint2x4 b) + { + AreEqual(a.c0, b.c0); + AreEqual(a.c1, b.c1); + AreEqual(a.c2, b.c2); + AreEqual(a.c3, b.c3); + } + + public static void AreEqual(uint3x4 a, uint3x4 b) + { + AreEqual(a.c0, b.c0); + AreEqual(a.c1, b.c1); + AreEqual(a.c2, b.c2); + AreEqual(a.c3, b.c3); + } + + public static void AreEqual(uint4x4 a, uint4x4 b) + { + AreEqual(a.c0, b.c0); + AreEqual(a.c1, b.c1); + AreEqual(a.c2, b.c2); + AreEqual(a.c3, b.c3); + } + + // float + public static void AreEqual(float2 a, float2 b, float delta = 0.0f) + { + AreEqual(a.x, b.x, delta); + AreEqual(a.y, b.y, delta); + } + + public static void AreEqual(float2 a, float2 b, int maxUlp, bool signedZeroEqual) + { + AreEqual(a.x, b.x, maxUlp, signedZeroEqual); + AreEqual(a.y, b.y, maxUlp, signedZeroEqual); + } + + public static void AreEqual(float3 a, float3 b, float delta = 0.0f) + { + AreEqual(a.x, b.x, delta); + AreEqual(a.y, b.y, delta); + AreEqual(a.z, b.z, delta); + } + + public static void AreEqual(float3 a, float3 b, int maxUlp, bool signedZeroEqual) + { + AreEqual(a.x, b.x, maxUlp, signedZeroEqual); + AreEqual(a.y, b.y, maxUlp, signedZeroEqual); + AreEqual(a.z, b.z, maxUlp, signedZeroEqual); + } + + public static void AreEqual(float4 a, float4 b, float delta = 0.0f) + { + AreEqual(a.x, b.x, delta); + AreEqual(a.y, b.y, delta); + AreEqual(a.z, b.z, delta); + AreEqual(a.w, b.w, delta); + } + + public static void AreEqual(float4 a, float4 b, int maxUlp, bool signedZeroEqual) + { + AreEqual(a.x, b.x, maxUlp, signedZeroEqual); + AreEqual(a.y, b.y, maxUlp, signedZeroEqual); + AreEqual(a.z, b.z, maxUlp, signedZeroEqual); + AreEqual(a.w, b.w, maxUlp, signedZeroEqual); + } + + + public static void AreEqual(float2x2 a, float2x2 b, float delta = 0.0f) + { + AreEqual(a.c0, b.c0, delta); + AreEqual(a.c1, b.c1, delta); + } + + public static void AreEqual(float2x2 a, float2x2 b, int maxUlp, bool signedZeroEqual) + { + AreEqual(a.c0, b.c0, maxUlp, signedZeroEqual); + AreEqual(a.c1, b.c1, maxUlp, signedZeroEqual); + } + + public static void AreEqual(float3x2 a, float3x2 b, float delta = 0.0f) + { + AreEqual(a.c0, b.c0, delta); + AreEqual(a.c1, b.c1, delta); + } + + public static void AreEqual(float3x2 a, float3x2 b, int maxUlp, bool signedZeroEqual) + { + AreEqual(a.c0, b.c0, maxUlp, signedZeroEqual); + AreEqual(a.c1, b.c1, maxUlp, signedZeroEqual); + } + + public static void AreEqual(float4x2 a, float4x2 b, float delta = 0.0f) + { + AreEqual(a.c0, b.c0, delta); + AreEqual(a.c1, b.c1, delta); + } + + public static void AreEqual(float4x2 a, float4x2 b, int maxUlp, bool signedZeroEqual) + { + AreEqual(a.c0, b.c0, maxUlp, signedZeroEqual); + AreEqual(a.c1, b.c1, maxUlp, signedZeroEqual); + } + + + public static void AreEqual(float2x3 a, float2x3 b, float delta = 0.0f) + { + AreEqual(a.c0, b.c0, delta); + AreEqual(a.c1, b.c1, delta); + AreEqual(a.c2, b.c2, delta); + } + + public static void AreEqual(float2x3 a, float2x3 b, int maxUlp, bool signedZeroEqual) + { + AreEqual(a.c0, b.c0, maxUlp, signedZeroEqual); + AreEqual(a.c1, b.c1, maxUlp, signedZeroEqual); + AreEqual(a.c2, b.c2, maxUlp, signedZeroEqual); + } + + public static void AreEqual(float3x3 a, float3x3 b, float delta = 0.0f) + { + AreEqual(a.c0, b.c0, delta); + AreEqual(a.c1, b.c1, delta); + AreEqual(a.c2, b.c2, delta); + } + + public static void AreEqual(float3x3 a, float3x3 b, int maxUlp, bool signedZeroEqual) + { + AreEqual(a.c0, b.c0, maxUlp, signedZeroEqual); + AreEqual(a.c1, b.c1, maxUlp, signedZeroEqual); + AreEqual(a.c2, b.c2, maxUlp, signedZeroEqual); + } + + public static void AreEqual(float4x3 a, float4x3 b, float delta = 0.0f) + { + AreEqual(a.c0, b.c0, delta); + AreEqual(a.c1, b.c1, delta); + AreEqual(a.c2, b.c2, delta); + } + + public static void AreEqual(float4x3 a, float4x3 b, int maxUlp, bool signedZeroEqual) + { + AreEqual(a.c0, b.c0, maxUlp, signedZeroEqual); + AreEqual(a.c1, b.c1, maxUlp, signedZeroEqual); + AreEqual(a.c2, b.c2, maxUlp, signedZeroEqual); + } + + + public static void AreEqual(float2x4 a, float2x4 b, float delta = 0.0f) + { + AreEqual(a.c0, b.c0, delta); + AreEqual(a.c1, b.c1, delta); + AreEqual(a.c2, b.c2, delta); + AreEqual(a.c3, b.c3, delta); + } + + public static void AreEqual(float2x4 a, float2x4 b, int maxUlp, bool signedZeroEqual) + { + AreEqual(a.c0, b.c0, maxUlp, signedZeroEqual); + AreEqual(a.c1, b.c1, maxUlp, signedZeroEqual); + AreEqual(a.c2, b.c2, maxUlp, signedZeroEqual); + AreEqual(a.c3, b.c3, maxUlp, signedZeroEqual); + } + + public static void AreEqual(float3x4 a, float3x4 b, float delta = 0.0f) + { + AreEqual(a.c0, b.c0, delta); + AreEqual(a.c1, b.c1, delta); + AreEqual(a.c2, b.c2, delta); + AreEqual(a.c3, b.c3, delta); + } + + public static void AreEqual(float3x4 a, float3x4 b, int maxUlp, bool signedZeroEqual) + { + AreEqual(a.c0, b.c0, maxUlp, signedZeroEqual); + AreEqual(a.c1, b.c1, maxUlp, signedZeroEqual); + AreEqual(a.c2, b.c2, maxUlp, signedZeroEqual); + AreEqual(a.c3, b.c3, maxUlp, signedZeroEqual); + } + + public static void AreEqual(float4x4 a, float4x4 b, float delta = 0.0f) + { + AreEqual(a.c0, b.c0, delta); + AreEqual(a.c1, b.c1, delta); + AreEqual(a.c2, b.c2, delta); + AreEqual(a.c3, b.c3, delta); + } + + public static void AreEqual(float4x4 a, float4x4 b, int maxUlp, bool signedZeroEqual) + { + AreEqual(a.c0, b.c0, maxUlp, signedZeroEqual); + AreEqual(a.c1, b.c1, maxUlp, signedZeroEqual); + AreEqual(a.c2, b.c2, maxUlp, signedZeroEqual); + AreEqual(a.c3, b.c3, maxUlp, signedZeroEqual); + } + + // double + public static void AreEqual(double2 a, double2 b, double delta = 0.0) + { + AreEqual(a.x, b.x, delta); + AreEqual(a.y, b.y, delta); + } + + public static void AreEqual(double2 a, double2 b, int maxUlp, bool signedZeroEqual) + { + AreEqual(a.x, b.x, maxUlp, signedZeroEqual); + AreEqual(a.y, b.y, maxUlp, signedZeroEqual); + } + + public static void AreEqual(double3 a, double3 b, double delta = 0.0) + { + AreEqual(a.x, b.x, delta); + AreEqual(a.y, b.y, delta); + AreEqual(a.z, b.z, delta); + } + + public static void AreEqual(double3 a, double3 b, int maxUlp, bool signedZeroEqual) + { + AreEqual(a.x, b.x, maxUlp, signedZeroEqual); + AreEqual(a.y, b.y, maxUlp, signedZeroEqual); + AreEqual(a.z, b.z, maxUlp, signedZeroEqual); + } + + public static void AreEqual(double4 a, double4 b, double delta = 0.0) + { + AreEqual(a.x, b.x, delta); + AreEqual(a.y, b.y, delta); + AreEqual(a.z, b.z, delta); + AreEqual(a.w, b.w, delta); + } + + public static void AreEqual(double4 a, double4 b, int maxUlp, bool signedZeroEqual) + { + AreEqual(a.x, b.x, maxUlp, signedZeroEqual); + AreEqual(a.y, b.y, maxUlp, signedZeroEqual); + AreEqual(a.z, b.z, maxUlp, signedZeroEqual); + AreEqual(a.w, b.w, maxUlp, signedZeroEqual); + } + + + public static void AreEqual(double2x2 a, double2x2 b, double delta = 0.0) + { + AreEqual(a.c0, b.c0, delta); + AreEqual(a.c1, b.c1, delta); + } + + public static void AreEqual(double2x2 a, double2x2 b, int maxUlp, bool signedZeroEqual) + { + AreEqual(a.c0, b.c0, maxUlp, signedZeroEqual); + AreEqual(a.c1, b.c1, maxUlp, signedZeroEqual); + } + + public static void AreEqual(double3x2 a, double3x2 b, double delta = 0.0) + { + AreEqual(a.c0, b.c0, delta); + AreEqual(a.c1, b.c1, delta); + } + + public static void AreEqual(double3x2 a, double3x2 b, int maxUlp, bool signedZeroEqual) + { + AreEqual(a.c0, b.c0, maxUlp, signedZeroEqual); + AreEqual(a.c1, b.c1, maxUlp, signedZeroEqual); + } + + public static void AreEqual(double4x2 a, double4x2 b, double delta = 0.0) + { + AreEqual(a.c0, b.c0, delta); + AreEqual(a.c1, b.c1, delta); + } + + public static void AreEqual(double4x2 a, double4x2 b, int maxUlp, bool signedZeroEqual) + { + AreEqual(a.c0, b.c0, maxUlp, signedZeroEqual); + AreEqual(a.c1, b.c1, maxUlp, signedZeroEqual); + } + + public static void AreEqual(double2x3 a, double2x3 b, double delta = 0.0) + { + AreEqual(a.c0, b.c0, delta); + AreEqual(a.c1, b.c1, delta); + AreEqual(a.c2, b.c2, delta); + } + + public static void AreEqual(double2x3 a, double2x3 b, int maxUlp, bool signedZeroEqual) + { + AreEqual(a.c0, b.c0, maxUlp, signedZeroEqual); + AreEqual(a.c1, b.c1, maxUlp, signedZeroEqual); + AreEqual(a.c2, b.c2, maxUlp, signedZeroEqual); + } + + public static void AreEqual(double3x3 a, double3x3 b, double delta = 0.0) + { + AreEqual(a.c0, b.c0, delta); + AreEqual(a.c1, b.c1, delta); + AreEqual(a.c2, b.c2, delta); + } + + public static void AreEqual(double3x3 a, double3x3 b, int maxUlp, bool signedZeroEqual) + { + AreEqual(a.c0, b.c0, maxUlp, signedZeroEqual); + AreEqual(a.c1, b.c1, maxUlp, signedZeroEqual); + AreEqual(a.c2, b.c2, maxUlp, signedZeroEqual); + } + + public static void AreEqual(double4x3 a, double4x3 b, double delta = 0.0) + { + AreEqual(a.c0, b.c0, delta); + AreEqual(a.c1, b.c1, delta); + AreEqual(a.c2, b.c2, delta); + } + + public static void AreEqual(double4x3 a, double4x3 b, int maxUlp, bool signedZeroEqual) + { + AreEqual(a.c0, b.c0, maxUlp, signedZeroEqual); + AreEqual(a.c1, b.c1, maxUlp, signedZeroEqual); + AreEqual(a.c2, b.c2, maxUlp, signedZeroEqual); + } + + + public static void AreEqual(double2x4 a, double2x4 b, double delta = 0.0) + { + AreEqual(a.c0, b.c0, delta); + AreEqual(a.c1, b.c1, delta); + AreEqual(a.c2, b.c2, delta); + AreEqual(a.c3, b.c3, delta); + } + + public static void AreEqual(double2x4 a, double2x4 b, int maxUlp, bool signedZeroEqual) + { + AreEqual(a.c0, b.c0, maxUlp, signedZeroEqual); + AreEqual(a.c1, b.c1, maxUlp, signedZeroEqual); + AreEqual(a.c2, b.c2, maxUlp, signedZeroEqual); + AreEqual(a.c3, b.c3, maxUlp, signedZeroEqual); + } + + public static void AreEqual(double3x4 a, double3x4 b, double delta = 0.0) + { + AreEqual(a.c0, b.c0, delta); + AreEqual(a.c1, b.c1, delta); + AreEqual(a.c2, b.c2, delta); + AreEqual(a.c3, b.c3, delta); + } + + public static void AreEqual(double3x4 a, double3x4 b, int maxUlp, bool signedZeroEqual) + { + AreEqual(a.c0, b.c0, maxUlp, signedZeroEqual); + AreEqual(a.c1, b.c1, maxUlp, signedZeroEqual); + AreEqual(a.c2, b.c2, maxUlp, signedZeroEqual); + AreEqual(a.c3, b.c3, maxUlp, signedZeroEqual); + } + + public static void AreEqual(double4x4 a, double4x4 b, double delta = 0.0) + { + AreEqual(a.c0, b.c0, delta); + AreEqual(a.c1, b.c1, delta); + AreEqual(a.c2, b.c2, delta); + AreEqual(a.c3, b.c3, delta); + } + + public static void AreEqual(double4x4 a, double4x4 b, int maxUlp, bool signedZeroEqual) + { + AreEqual(a.c0, b.c0, maxUlp, signedZeroEqual); + AreEqual(a.c1, b.c1, maxUlp, signedZeroEqual); + AreEqual(a.c2, b.c2, maxUlp, signedZeroEqual); + AreEqual(a.c3, b.c3, maxUlp, signedZeroEqual); + } + + public static void AreEqual(quaternion a, quaternion b, float delta = 0.0f) + { + AreEqual(a.value, b.value, delta); + } + + public static void AreEqual(RigidTransform a, RigidTransform b, float delta = 0.0f) + { + AreEqual(a.rot, b.rot, delta); + AreEqual(a.pos, b.pos, delta); + } + + public delegate void FuncDelegate(); + public static void ThrowsException(FuncDelegate func) where T : System.Exception + { + try + { + func(); + } + catch (T) + { + return; + } + Assert.IsTrue(false, "Expected exception to be thrown"); + } + + // + // Random generation + // + + public static unsafe BlobAssetReference GenerateRandomMesh(ref Random rnd) + { + int numTriangles = rnd.NextInt(1, 250); + float3[] vertices = new float3[numTriangles * 3]; + int[] indices = new int[numTriangles * 3]; + int nextIndex = 0; + + while (numTriangles > 0) + { + int featureTriangles = math.min(rnd.NextInt(1, 100), numTriangles); + + int featureType = rnd.NextInt(0, 3); + switch (featureType) + { + case 0: + { + // Soup + for (int i = 0; i < featureTriangles; i++) + { + float size = rnd.NextFloat(0.1f, 2.5f); + size *= size; + + float3 center = rnd.NextFloat3(-10.0f, 10.0f); + for (int j = 0; j < 3; j++) + { + vertices[nextIndex++] = center + rnd.NextFloat3(-size, size); + } + } + break; + } + + case 1: + { + // Fan + float3 center = rnd.NextFloat3(-10.0f, 10.0f); + float3 arm = rnd.NextFloat3Direction() * rnd.NextFloat(0.1f, 1.0f); + float3 axis; + { + float3 unused; + Math.CalculatePerpendicularNormalized(math.normalize(arm), out axis, out unused); + } + float arc = rnd.NextFloat(0.1f, 2.0f * (float)math.PI); + arc = math.min(arc, featureTriangles * (float)math.PI / 2.0f); // avoid degenerate triangles + featureTriangles = math.min(featureTriangles, (int)(arc / 0.025f)); // avoid degenerate triangles + quaternion q = Unity.Mathematics.quaternion.AxisAngle(axis, arc / numTriangles); + for (int i = 0; i < featureTriangles; i++) + { + vertices[nextIndex++] = center; + vertices[nextIndex++] = center + arm; + arm = math.mul(q, arm); + vertices[nextIndex++] = center + arm; + } + break; + } + + case 2: + { + // Strip + float3 v0 = rnd.NextFloat3(-10.0f, 10.0f); + float3 v1 = v0 + rnd.NextFloat3(-0.5f, 0.5f); + float3 dir; + { + float3 unused; + Math.CalculatePerpendicularNormalized(math.normalize(v1 - v0), out dir, out unused); + } + for (int i = 0; i < featureTriangles; i++) + { + float3 v2 = v0 + rnd.NextFloat(0.25f, 0.5f) * dir; + dir = math.mul(Unity.Mathematics.quaternion.AxisAngle(rnd.NextFloat3Direction(), rnd.NextFloat(0.0f, 0.3f)), dir); + + vertices[nextIndex++] = v0; + vertices[nextIndex++] = v1; + vertices[nextIndex++] = v2; + + v0 = v1; + v1 = v2; + } + break; + } + + case 3: + { + // Grid + int quads = featureTriangles / 2; + if (quads == 0) + { + featureTriangles = 0; // Too small, try again for a different feature + break; + } + + int rows = rnd.NextInt(1, (int)math.sqrt(quads)); + int cols = quads / rows; + quads = rows * cols; + featureTriangles = quads * 2; + + float3 origin = rnd.NextFloat3(-10.0f, 10.0f); + float3 x = rnd.NextFloat3(-0.5f, 0.5f); + float3 y = rnd.NextFloat3(-0.5f, 0.5f); + for (int i = 0; i < rows; i++) + { + for (int j = 0; j < cols; j++) + { + vertices[nextIndex++] = origin + x * (i + 0) + y * (j + 0); + vertices[nextIndex++] = origin + x * (i + 0) + y * (j + 1); + vertices[nextIndex++] = origin + x * (i + 1) + y * (j + 1); + + vertices[nextIndex++] = origin + x * (i + 0) + y * (j + 0); + vertices[nextIndex++] = origin + x * (i + 1) + y * (j + 1); + vertices[nextIndex++] = origin + x * (i + 0) + y * (j + 1); + } + } + + break; + } + } + + numTriangles -= featureTriangles; + } + + for (int i = 0; i < indices.Length; i++) + { + indices[i] = i; + } + + return MeshCollider.Create(vertices, indices); + } + + public static unsafe BlobAssetReference GenerateRandomCompound(ref Random rnd) + { + int numChildren = rnd.NextInt(1, 10); + var children = new NativeArray(numChildren, Allocator.Temp); + for (int i = 0; i < numChildren; i++) + { + children[i] = new CompoundCollider.ColliderBlobInstance + { + CompoundFromChild = new RigidTransform + { + pos = (rnd.NextInt(10) > 0) ? rnd.NextFloat3(-5.0f, 5.0f) : float3.zero, + rot = (rnd.NextInt(10) > 0) ? rnd.NextQuaternionRotation() : quaternion.identity + }, + Collider = GenerateRandomCollider(ref rnd) + }; + } + + return CompoundCollider.Create(children); + } + + public static unsafe BlobAssetReference GenerateRandomConvex(ref Random rnd) + { + ColliderType colliderType = (ColliderType)rnd.NextInt((int)ColliderType.Box); + float radius = (rnd.NextInt(4) > 0) ? rnd.NextFloat(0.5f) : 0.0f; + switch (colliderType) + { + case ColliderType.Convex: + { + int numPoints = rnd.NextInt(1, 16); + if (numPoints == 3) // TODO - hull builder doesn't build faces for flat shapes, work around it for now to run the test + { + numPoints++; + } + var points = new NativeArray(numPoints, Allocator.Temp); + for (int i = 0; i < numPoints; i++) + { + points[i] = rnd.NextFloat3(-1.0f, 1.0f); + } + var collider = ConvexCollider.Create(points, radius); + points.Dispose(); + return collider; + } + + case ColliderType.Sphere: + { + float3 center = (rnd.NextInt(4) > 0) ? float3.zero : rnd.NextFloat3(-0.5f, 0.5f); + return SphereCollider.Create(center, rnd.NextFloat(0.01f, 0.5f)); + } + + case ColliderType.Capsule: + { + float3 point0 = rnd.NextFloat3(0.0f, 1.0f); + float3 point1 = (rnd.NextInt(4) > 0) ? -point0 : rnd.NextFloat3(-1.0f, 1.0f); + return CapsuleCollider.Create(point0, point1, rnd.NextFloat(0.01f, 0.5f)); + } + + case ColliderType.Triangle: + { + return PolygonCollider.CreateTriangle(rnd.NextFloat3(-1.0f, 1.0f), rnd.NextFloat3(-1.0f, 1.0f), rnd.NextFloat3(-1.0f, 1.0f)); + } + + case ColliderType.Quad: + { + // Pick 3 totally random points, then choose a fourth that makes a flat and convex quad + float3 point0 = rnd.NextFloat3(-1.0f, 1.0f); + float3 point1 = rnd.NextFloat3(-1.0f, 1.0f); + float3 point3 = rnd.NextFloat3(-1.0f, 1.0f); + float t0 = rnd.NextFloat(0.0f, 1.0f); + float t1 = rnd.NextFloat(0.0f, 1.0f); + float3 e = point1 + point1 - point0; + float3 a = math.lerp(point1, e, t0); + float3 b = math.lerp(point3, point3 + point3 - point0, t0); + float3 point2 = math.lerp(a, b, t1); + + return PolygonCollider.CreateQuad(point0, point1, point2, point3); + } + + case ColliderType.Box: + { + float3 center = (rnd.NextInt(4) > 0) ? float3.zero : rnd.NextFloat3(-0.5f, 0.5f); + quaternion orientation = (rnd.NextInt(4) > 0) ? quaternion.identity : rnd.NextQuaternionRotation(); + return BoxCollider.Create(center, orientation, rnd.NextFloat3(0.01f, 1.0f), radius); + } + + default: throw new System.NotImplementedException(); + } + } + + public static unsafe BlobAssetReference GenerateRandomCollider(ref Random rnd) + { + if (rnd.NextInt(10) > 0) + { + return GenerateRandomConvex(ref rnd); + } + else if (rnd.NextInt(4) > 0) + { + return GenerateRandomMesh(ref rnd); + } + return GenerateRandomCompound(ref rnd); + } + + public static unsafe Physics.PhysicsWorld GenerateRandomWorld(ref Random rnd, int numBodies, float size) + { + // Create the world + PhysicsWorld world = new PhysicsWorld(numBodies, 0, 0); + + // Create bodies + NativeSlice bodies = world.StaticBodies; + for (int i = 0; i < numBodies; i++) + { + bodies[i] = new RigidBody + { + WorldFromBody = new RigidTransform + { + pos = rnd.NextFloat3(-size, size), + rot = (rnd.NextInt(10) > 0) ? rnd.NextQuaternionRotation() : quaternion.identity + }, + Collider = (Collider*)GenerateRandomCollider(ref rnd).GetUnsafePtr(), + Entity = Entity.Null, + CustomData = 0 + }; + } + + // Build the broadphase + world.CollisionWorld.Broadphase.ScheduleBuildJobs(ref world, timeStep: 1.0f, numThreadsHint: 1, haveStaticBodiesChanged: true, inputDeps: new JobHandle()).Complete(); + + return world; + } + } +} diff --git a/package/Tests/PlayModeTests/Utils/TestUtils.cs.meta b/package/Tests/PlayModeTests/Utils/TestUtils.cs.meta new file mode 100755 index 000000000..fc23e9fa2 --- /dev/null +++ b/package/Tests/PlayModeTests/Utils/TestUtils.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 7eeef8159f2598640ae9d41d1488c113 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Unity.Physics.Authoring.meta b/package/Unity.Physics.Authoring.meta new file mode 100755 index 000000000..9a21f6512 --- /dev/null +++ b/package/Unity.Physics.Authoring.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: b0ac81b0cc8fb4bcaade4b098324ddd7 +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Unity.Physics.Authoring/AssemblyInfo.cs b/package/Unity.Physics.Authoring/AssemblyInfo.cs new file mode 100755 index 000000000..188360047 --- /dev/null +++ b/package/Unity.Physics.Authoring/AssemblyInfo.cs @@ -0,0 +1,4 @@ +using System.Runtime.CompilerServices; + +[assembly: InternalsVisibleTo("Unity.Physics.Editor")] +[assembly: InternalsVisibleTo("Unity.Physics.PlayModeTests")] \ No newline at end of file diff --git a/package/Unity.Physics.Authoring/AssemblyInfo.cs.meta b/package/Unity.Physics.Authoring/AssemblyInfo.cs.meta new file mode 100755 index 000000000..e3954a595 --- /dev/null +++ b/package/Unity.Physics.Authoring/AssemblyInfo.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 03c38d1c6355841b1aeed1a2b3cb81dd +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Unity.Physics.Authoring/Assets.meta b/package/Unity.Physics.Authoring/Assets.meta new file mode 100755 index 000000000..11108df91 --- /dev/null +++ b/package/Unity.Physics.Authoring/Assets.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: 536d9b526ab0f4734b47b8c78c155daf +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Unity.Physics.Authoring/Assets/CustomFlagNames.cs b/package/Unity.Physics.Authoring/Assets/CustomFlagNames.cs new file mode 100755 index 000000000..8126d88f7 --- /dev/null +++ b/package/Unity.Physics.Authoring/Assets/CustomFlagNames.cs @@ -0,0 +1,28 @@ +using System; +using System.Collections.Generic; +using System.Linq; +using UnityEngine; + +namespace Unity.Physics.Authoring +{ + interface IFlagNames + { + IReadOnlyList FlagNames { get; } + } + + [CreateAssetMenu(menuName = "DOTS Physics/Custom Flag Names", fileName = "Custom Flag Names")] + public class CustomFlagNames : ScriptableObject, IFlagNames + { + public IReadOnlyList FlagNames => m_FlagNames; + [SerializeField] + string[] m_FlagNames = Enumerable.Range(0, 8).Select(i => string.Empty).ToArray(); + + void OnValidate() + { + if (m_FlagNames.Length != 8) + { + Array.Resize(ref m_FlagNames, 8); + } + } + } +} diff --git a/package/Unity.Physics.Authoring/Assets/CustomFlagNames.cs.meta b/package/Unity.Physics.Authoring/Assets/CustomFlagNames.cs.meta new file mode 100755 index 000000000..4c5aa63d4 --- /dev/null +++ b/package/Unity.Physics.Authoring/Assets/CustomFlagNames.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 7c8956dd10f66427faa4a4067c5dd97d +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {fileID: 2800000, guid: 7c7a0159cb0d5433b90c970978f6cf5c, type: 3} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Unity.Physics.Authoring/Assets/PhysicsCategoryNames.cs b/package/Unity.Physics.Authoring/Assets/PhysicsCategoryNames.cs new file mode 100755 index 000000000..f5c5f40ce --- /dev/null +++ b/package/Unity.Physics.Authoring/Assets/PhysicsCategoryNames.cs @@ -0,0 +1,24 @@ +using System; +using System.Collections.Generic; +using System.Linq; +using UnityEngine; + +namespace Unity.Physics.Authoring +{ + [CreateAssetMenu(menuName = "DOTS Physics/Physics Category Names", fileName = "Physics Category Names")] + public sealed class PhysicsCategoryNames : ScriptableObject, IFlagNames + { + IReadOnlyList IFlagNames.FlagNames => CategoryNames; + public IReadOnlyList CategoryNames => m_CategoryNames; + [SerializeField] + string[] m_CategoryNames = Enumerable.Range(0, 32).Select(i => string.Empty).ToArray(); + + void OnValidate() + { + if (m_CategoryNames.Length != 32) + { + Array.Resize(ref m_CategoryNames, 32); + } + } + } +} \ No newline at end of file diff --git a/package/Unity.Physics.Authoring/Assets/PhysicsCategoryNames.cs.meta b/package/Unity.Physics.Authoring/Assets/PhysicsCategoryNames.cs.meta new file mode 100755 index 000000000..94da74c23 --- /dev/null +++ b/package/Unity.Physics.Authoring/Assets/PhysicsCategoryNames.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: cea4b49c4d6784a3291bea84d8086c8e +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {fileID: 2800000, guid: 269c31d3570d84742a0d8739b06d5a18, type: 3} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Unity.Physics.Authoring/Assets/PhysicsMaterialTemplate.cs b/package/Unity.Physics.Authoring/Assets/PhysicsMaterialTemplate.cs new file mode 100755 index 000000000..ca0496193 --- /dev/null +++ b/package/Unity.Physics.Authoring/Assets/PhysicsMaterialTemplate.cs @@ -0,0 +1,39 @@ +using UnityEngine; + +namespace Unity.Physics.Authoring +{ + [CreateAssetMenu(menuName = "DOTS Physics/Physics Material Template")] + public class PhysicsMaterialTemplate : ScriptableObject, IPhysicsMaterialProperties + { + public bool IsTrigger { get => m_Value.IsTrigger; set => m_Value.IsTrigger = value; } + + public PhysicsMaterialCoefficient Friction { get => m_Value.Friction; set => m_Value.Friction = value; } + public PhysicsMaterialCoefficient Restitution { get => m_Value.Restitution; set => m_Value.Restitution = value; } + + public int BelongsTo { get => m_Value.BelongsTo; set => m_Value.BelongsTo = value; } + public bool GetBelongsTo(int categoryIndex) => m_Value.GetBelongsTo(categoryIndex); + public void SetBelongsTo(int categoryIndex, bool value) => m_Value.SetBelongsTo(categoryIndex, value); + + public int CollidesWith { get => m_Value.CollidesWith; set => m_Value.CollidesWith = value; } + public bool GetCollidesWith(int categoryIndex) => m_Value.GetCollidesWith(categoryIndex); + public void SetCollidesWith(int categoryIndex, bool value) => m_Value.SetCollidesWith(categoryIndex, value); + + public bool RaisesCollisionEvents + { + get => m_Value.RaisesCollisionEvents; + set => m_Value.RaisesCollisionEvents = value; + } + + public byte CustomFlags { get => m_Value.CustomFlags; set => m_Value.CustomFlags = value; } + public bool GetCustomFlag(int customFlagIndex) => m_Value.GetCustomFlag(customFlagIndex); + public void SetCustomFlag(int customFlagIndex, bool value) => m_Value.SetCustomFlag(customFlagIndex, value); + + [SerializeField] + PhysicsMaterialProperties m_Value = new PhysicsMaterialProperties(false); + + void OnValidate() + { + PhysicsMaterialProperties.OnValidate(ref m_Value, false); + } + } +} diff --git a/package/Unity.Physics.Authoring/Assets/PhysicsMaterialTemplate.cs.meta b/package/Unity.Physics.Authoring/Assets/PhysicsMaterialTemplate.cs.meta new file mode 100755 index 000000000..292673a20 --- /dev/null +++ b/package/Unity.Physics.Authoring/Assets/PhysicsMaterialTemplate.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 79f877434772c463192af154345455c9 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {fileID: 2800000, guid: dea88969a14a54552b290b80692e0785, type: 3} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Unity.Physics.Authoring/Components.meta b/package/Unity.Physics.Authoring/Components.meta new file mode 100755 index 000000000..ee166ff74 --- /dev/null +++ b/package/Unity.Physics.Authoring/Components.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: 0d77a02ce9849480b9c0ecbc455073d2 +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Unity.Physics.Authoring/Components/PhysicsBody.cs b/package/Unity.Physics.Authoring/Components/PhysicsBody.cs new file mode 100755 index 000000000..1c004df41 --- /dev/null +++ b/package/Unity.Physics.Authoring/Components/PhysicsBody.cs @@ -0,0 +1,112 @@ +using System; +using Unity.Entities; +using Unity.Mathematics; +using UnityEngine; + +namespace Unity.Physics.Authoring +{ + public enum BodyMotionType + { + Dynamic, + Kinematic, + Static + } + + [AddComponentMenu("DOTS Physics/Physics Body")] + [DisallowMultipleComponent] + [RequiresEntityConversion] + public sealed class PhysicsBody : MonoBehaviour + { + public BodyMotionType MotionType { get => m_MotionType; set => m_MotionType = value; } + [SerializeField] + [Tooltip("Specifies whether the body should be fully physically simulated, moved directly, or fixed in place.")] + BodyMotionType m_MotionType; + + const float k_MinimumMass = 0.001f; + + public float Mass + { + get => m_MotionType == BodyMotionType.Dynamic ? m_Mass : float.PositiveInfinity; + set => m_Mass = math.max(k_MinimumMass, value); + } + [SerializeField] + float m_Mass = 1.0f; + + public float LinearDamping { get => m_LinearDamping; set => m_LinearDamping = value; } + [SerializeField] + [Tooltip("This is applied to a body's linear velocity reducing it over time.")] + float m_LinearDamping = 0.01f; + + public float AngularDamping { get => m_AngularDamping; set => m_AngularDamping = value; } + [SerializeField] + [Tooltip("This is applied to a body's angular velocity reducing it over time.")] + float m_AngularDamping = 0.05f; + + public float3 InitialLinearVelocity { get => m_InitialLinearVelocity; set => m_InitialLinearVelocity = value; } + [SerializeField] + [Tooltip("The initial linear velocity of the body in world space")] + float3 m_InitialLinearVelocity = float3.zero; + + public float3 InitialAngularVelocity { get => m_InitialAngularVelocity; set => m_InitialAngularVelocity = value; } + [SerializeField] + [Tooltip("This represents the initial rotation speed around each axis in the local motion space of the body i.e. around the center of mass")] + float3 m_InitialAngularVelocity = float3.zero; + + public float GravityFactor + { + get => m_MotionType == BodyMotionType.Dynamic ? m_GravityFactor : 0f; + set => m_GravityFactor = value; + } + [SerializeField] + [Tooltip("Scales the amount of gravity to apply to this body.")] + float m_GravityFactor = 1f; + + public bool OverrideDefaultMassDistribution + { + get => m_OverrideDefaultMassDistribution; + set => m_OverrideDefaultMassDistribution = value; + } + [SerializeField] + [Tooltip("Default mass distribution is based on the shapes associated with this body.")] + public bool m_OverrideDefaultMassDistribution; + + public MassDistribution CustomMassDistribution + { + get => new MassDistribution + { + Transform = new RigidTransform(m_Orientation, m_CenterOfMass), + InertiaTensor = + m_MotionType == BodyMotionType.Dynamic ? m_InertiaTensor : new float3(float.PositiveInfinity) + }; + set + { + m_CenterOfMass = value.Transform.pos; + m_Orientation.SetValue(value.Transform.rot); + m_InertiaTensor = value.InertiaTensor; + m_OverrideDefaultMassDistribution = true; + } + } + + [SerializeField] + float3 m_CenterOfMass; + + [SerializeField] + EulerAngles m_Orientation = EulerAngles.Default; + + [SerializeField] + // Default value to solid unit sphere : https://en.wikipedia.org/wiki/List_of_moments_of_inertia + float3 m_InertiaTensor = new float3(2f/5f); + + void OnEnable() + { + // included so tick box appears in Editor + } + + void OnValidate() + { + m_Mass = math.max(k_MinimumMass, m_Mass); + m_LinearDamping = math.max(m_LinearDamping, 0f); + m_AngularDamping = math.max(m_AngularDamping, 0f); + } + } +} diff --git a/package/Unity.Physics.Authoring/Components/PhysicsBody.cs.meta b/package/Unity.Physics.Authoring/Components/PhysicsBody.cs.meta new file mode 100755 index 000000000..57fdef823 --- /dev/null +++ b/package/Unity.Physics.Authoring/Components/PhysicsBody.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: ccea9ea98e38942e0b0938c27ed1903e +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Unity.Physics.Authoring/Components/PhysicsDebugDisplay.cs b/package/Unity.Physics.Authoring/Components/PhysicsDebugDisplay.cs new file mode 100755 index 000000000..c10e73ffc --- /dev/null +++ b/package/Unity.Physics.Authoring/Components/PhysicsDebugDisplay.cs @@ -0,0 +1,78 @@ +using Unity.Entities; +using UnityEngine; + +namespace Unity.Physics.Authoring +{ + public struct PhysicsDebugDisplayData : IComponentData + { + public int DrawColliders; + public int DrawMeshEdges; + public int DrawColliderAabbs; + public int DrawBroadphase; + public int DrawMassProperties; + public int DrawContacts; + public int DrawCollisionEvents; + public int DrawTriggerEvents; + public int DrawJoints; + } + + [AddComponentMenu("DOTS Physics/Physics Debug Display")] + [DisallowMultipleComponent] + [RequiresEntityConversion] + public class PhysicsDebugDisplay : MonoBehaviour, IConvertGameObjectToEntity + { + public bool DrawColliders = false; + public bool DrawMeshEdges = false; + public bool DrawColliderAabbs = false; + public bool DrawBroadphase = false; + public bool DrawMassProperties = false; + public bool DrawContacts = false; + public bool DrawCollisionEvents = false; + public bool DrawTriggerEvents = false; + public bool DrawJoints = false; + + private Entity convertedEntity = Entity.Null; + + void IConvertGameObjectToEntity.Convert(Entity entity, EntityManager dstManager, GameObjectConversionSystem conversionSystem) + { + var componentData = new PhysicsDebugDisplayData + { + DrawColliders = DrawColliders ? 1 : 0, + DrawMeshEdges = DrawMeshEdges ? 1 : 0, + DrawColliderAabbs = DrawColliderAabbs ? 1 : 0, + DrawBroadphase = DrawBroadphase ? 1 : 0, + DrawMassProperties = DrawMassProperties ? 1 : 0, + DrawContacts = DrawContacts ? 1 : 0, + DrawCollisionEvents = DrawCollisionEvents ? 1 : 0, + DrawTriggerEvents = DrawTriggerEvents ? 1 : 0, + DrawJoints = DrawJoints ? 1 : 0 + }; + dstManager.AddComponentData(entity, componentData); + + convertedEntity = entity; + } + + void OnValidate() + { + if (!isActiveAndEnabled) return; + if (convertedEntity == Entity.Null) return; + + // This requires Entity Conversion mode to be 'Convert And Inject Game Object' + var entityManager = World.Active.EntityManager; + if (entityManager.HasComponent(convertedEntity)) + { + var component = entityManager.GetComponentData(convertedEntity); + component.DrawColliders = DrawColliders ? 1 : 0; + component.DrawMeshEdges = DrawMeshEdges ? 1 : 0; + component.DrawColliderAabbs = DrawColliderAabbs ? 1 : 0; + component.DrawBroadphase = DrawBroadphase ? 1 : 0; + component.DrawMassProperties = DrawMassProperties ? 1 : 0; + component.DrawContacts = DrawContacts ? 1 : 0; + component.DrawCollisionEvents = DrawCollisionEvents ? 1 : 0; + component.DrawTriggerEvents = DrawTriggerEvents ? 1 : 0; + component.DrawJoints = DrawJoints ? 1 : 0; + entityManager.SetComponentData(convertedEntity, component); + } + } + } +} diff --git a/package/Unity.Physics.Authoring/Components/PhysicsDebugDisplay.cs.meta b/package/Unity.Physics.Authoring/Components/PhysicsDebugDisplay.cs.meta new file mode 100755 index 000000000..4ec9c7aaa --- /dev/null +++ b/package/Unity.Physics.Authoring/Components/PhysicsDebugDisplay.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: bb54ea4f67dc17342bcd49954c53502d +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Unity.Physics.Authoring/Components/PhysicsShape.cs b/package/Unity.Physics.Authoring/Components/PhysicsShape.cs new file mode 100755 index 000000000..f5200c345 --- /dev/null +++ b/package/Unity.Physics.Authoring/Components/PhysicsShape.cs @@ -0,0 +1,616 @@ +using System; +using System.Collections.Generic; +using System.ComponentModel; +using Unity.Collections; +using Unity.Entities; +using Unity.Mathematics; +using UnityEngine; +using UnityMesh = UnityEngine.Mesh; + +namespace Unity.Physics.Authoring +{ + public enum ShapeType + { + Box = 0, + Capsule = 1, + Sphere = 2, + [Description("Cylinder (Convex Hull)")] + Cylinder = 3, + Plane = 4, + // extra space to accommodate other possible primitives in the future + ConvexHull = 30, + Mesh = 31 + } + + [Serializable] + struct EulerAngles + { + public float3 Value; + [HideInInspector] + public math.RotationOrder RotationOrder; + + public void SetValue(quaternion value) + { + if (RotationOrder != math.RotationOrder.ZXY) + throw new NotSupportedException(); + // TODO: add generic Euler decomposition to math.quaternion + Value = ((Quaternion)value).eulerAngles; + } + + public static implicit operator quaternion(EulerAngles euler) => + quaternion.Euler(math.radians(euler.Value), euler.RotationOrder); + + public static EulerAngles Default => new EulerAngles { RotationOrder = math.RotationOrder.ZXY }; + } + + [AddComponentMenu("DOTS Physics/Physics Shape")] + [DisallowMultipleComponent] + [RequiresEntityConversion] + public sealed class PhysicsShape : MonoBehaviour, IInheritPhysicsMaterialProperties + { + [Serializable] + struct CylindricalProperties + { + public float Height; + public float Radius; + [HideInInspector] + public int Axis; + } + + static readonly int[] k_NextAxis = { 1, 2, 0 }; + internal const float k_DefaultConvexRadius = 0.05f; + + public ShapeType ShapeType => m_ShapeType; + [SerializeField] + ShapeType m_ShapeType = ShapeType.Box; + + [SerializeField] + float3 m_PrimitiveCenter; + + [SerializeField] + float3 m_PrimitiveSize = new float3(1f, 1f, 1f); + + [SerializeField] + EulerAngles m_PrimitiveOrientation = EulerAngles.Default; + + [SerializeField] + [ExpandChildren] + CylindricalProperties m_Capsule = new CylindricalProperties { Height = 1f, Radius = 0.5f, Axis = 2 }; + + [SerializeField] + [ExpandChildren] + CylindricalProperties m_Cylinder = new CylindricalProperties { Height = 1f, Radius = 0.5f, Axis = 2 }; + + [SerializeField] + float m_SphereRadius = 0.5f; + + public float ConvexRadius + { + get + { + switch (m_ShapeType) + { + case ShapeType.Box: + case ShapeType.Cylinder: + case ShapeType.ConvexHull: + return m_ConvexRadius; + case ShapeType.Capsule: + return m_Capsule.Radius; + case ShapeType.Sphere: + return 0.5f * math.cmax(m_PrimitiveSize); + default: + return 0f; + } + } + set + { + var maxRadius = float.MaxValue; + switch (m_ShapeType) + { + case ShapeType.Box: + maxRadius = 0.5f * math.cmin(m_PrimitiveSize); + break; + case ShapeType.Cylinder: + GetCylinderProperties( + out var center, out var height, out var radius, out EulerAngles orientation + ); + maxRadius = math.min(0.5f * height, radius); + break; + case ShapeType.ConvexHull: + case ShapeType.Mesh: + // TODO : any benefit in clamping this e.g. maxRadius = k_DefaultConvexRadius; + break; + case ShapeType.Plane: + maxRadius = 0f; + break; + } + m_ConvexRadius = math.clamp(m_ConvexRadius, 0f, maxRadius); + } + } + [SerializeField] + [Tooltip("Determines how rounded the corners of the convex shape will be. A value greater than 0 results in more optimized collision, at the cost of some shape detail.")] + float m_ConvexRadius = k_DefaultConvexRadius; + + // TODO: remove this accessor in favor of GetRawVertices() when blob data is serializable + internal UnityMesh CustomMesh => m_CustomMesh; + [SerializeField] + [Tooltip("If no custom mesh is specified, then one will be generated using this body's rendered meshes.")] + UnityMesh m_CustomMesh; + + public PhysicsMaterialTemplate MaterialTemplate { get => m_Material.Template; set => m_Material.Template = value; } + PhysicsMaterialTemplate IInheritPhysicsMaterialProperties.Template + { + get => m_Material.Template; + set => m_Material.Template = value; + } + + public bool OverrideIsTrigger { get => m_Material.OverrideIsTrigger; set => m_Material.OverrideIsTrigger = value; } + public bool IsTrigger { get => m_Material.IsTrigger; set => m_Material.IsTrigger = value; } + + public bool OverrideFriction { get => m_Material.OverrideFriction; set => m_Material.OverrideFriction = value; } + public PhysicsMaterialCoefficient Friction { get => m_Material.Friction; set => m_Material.Friction = value; } + + public bool OverrideRestitution + { + get => m_Material.OverrideRestitution; + set => m_Material.OverrideRestitution = value; + } + public PhysicsMaterialCoefficient Restitution + { + get => m_Material.Restitution; + set => m_Material.Restitution = value; + } + + public bool OverrideBelongsTo + { + get => m_Material.OverrideBelongsTo; + set => m_Material.OverrideBelongsTo = value; + } + public int BelongsTo { get => m_Material.BelongsTo; set => m_Material.BelongsTo = value; } + public bool GetBelongsTo(int categoryIndex) => m_Material.GetBelongsTo(categoryIndex); + public void SetBelongsTo(int categoryIndex, bool value) => m_Material.SetBelongsTo(categoryIndex, value); + + public bool OverrideCollidesWith + { + get => m_Material.OverrideCollidesWith; + set => m_Material.OverrideCollidesWith = value; + } + public int CollidesWith { get => m_Material.CollidesWith; set => m_Material.CollidesWith = value; } + public bool GetCollidesWith(int categoryIndex) => m_Material.GetCollidesWith(categoryIndex); + public void SetCollidesWith(int categoryIndex, bool value) => m_Material.SetCollidesWith(categoryIndex, value); + + public bool OverrideRaisesCollisionEvents + { + get => m_Material.OverrideRaisesCollisionEvents; + set => m_Material.OverrideRaisesCollisionEvents = value; + } + public bool RaisesCollisionEvents + { + get => m_Material.RaisesCollisionEvents; + set => m_Material.RaisesCollisionEvents = value; + } + + public bool OverrideCustomFlags + { + get => m_Material.OverrideCustomFlags; + set => m_Material.OverrideCustomFlags = value; + } + public byte CustomFlags { get => m_Material.CustomFlags; set => m_Material.CustomFlags = value; } + public bool GetCustomFlag(int customFlagIndex) => m_Material.GetCustomFlag(customFlagIndex); + public void SetCustomFlag(int customFlagIndex, bool value) => m_Material.SetCustomFlag(customFlagIndex, value); + + [SerializeField] + PhysicsMaterialProperties m_Material = new PhysicsMaterialProperties(true); + + public void GetBoxProperties(out float3 center, out float3 size, out quaternion orientation) + { + GetBoxProperties(out center, out size, out EulerAngles euler); + orientation = euler; + } + + internal void GetBoxProperties(out float3 center, out float3 size, out EulerAngles orientation) + { + center = m_PrimitiveCenter; + size = m_PrimitiveSize; + orientation = m_PrimitiveOrientation; + // prefer identity and shuffle size if aligned with basis vectors + const float tolerance = 0.00001f; + var fwd = new float3 { z = 1f }; + var dotFwd = math.abs(math.dot(math.mul(orientation, fwd), fwd)); + var up = new float3 { y = 1f }; + var dotUp = math.abs(math.dot(math.mul(orientation, up), up)); + if ( + math.abs(math.dot(orientation, quaternion.identity)) > tolerance + && (dotFwd < tolerance || 1f - dotFwd < tolerance) + && (dotUp < tolerance || 1f - dotUp < tolerance) + ) + { + size = math.abs(math.mul(orientation, size)); // TODO: handle floating point error + orientation.SetValue(quaternion.identity); + } + } + + void GetCylindricalProperties( + CylindricalProperties props, + out float3 center, out float height, out float radius, out EulerAngles orientation, + bool rebuildOrientation + ) + { + center = m_PrimitiveCenter; + var lookVector = math.mul(m_PrimitiveOrientation, new float3 { [props.Axis] = 1f }); + // use previous axis so forward will prefer up + var upVector = math.mul(m_PrimitiveOrientation, new float3 { [k_NextAxis[k_NextAxis[props.Axis]]] = 1f }); + orientation = m_PrimitiveOrientation; + if (rebuildOrientation && props.Axis != 2) + orientation.SetValue(quaternion.LookRotation(lookVector, upVector)); + radius = props.Radius; + height = props.Height; + } + + public void GetCapsuleProperties( + out float3 center, out float height, out float radius, out quaternion orientation + ) + { + GetCapsuleProperties(out center, out height, out radius, out EulerAngles euler); + orientation = euler; + } + + internal void GetCapsuleProperties( + out float3 center, out float height, out float radius, out EulerAngles orientation + ) + { + GetCylindricalProperties( + m_Capsule, out center, out height, out radius, out orientation, m_ShapeType != ShapeType.Capsule + ); + } + + public void GetCapsuleProperties(out float3 vertex0, out float3 vertex1, out float radius) + { + UpdateCapsuleAxis(); + var axis = math.mul(m_PrimitiveOrientation, new float3 { z = 1f }); + radius = m_Capsule.Radius; + var endPoint = axis * (0.5f * m_Capsule.Height - radius); + vertex0 = m_PrimitiveCenter + endPoint; + vertex1 = m_PrimitiveCenter - endPoint; + } + + public void GetCylinderProperties( + out float3 center, out float height, out float radius, out quaternion orientation + ) + { + GetCylinderProperties(out center, out height, out radius, out EulerAngles euler); + orientation = euler; + } + + internal void GetCylinderProperties( + out float3 center, out float height, out float radius, out EulerAngles orientation + ) + { + GetCylindricalProperties( + m_Cylinder, out center, out height, out radius, out orientation, m_ShapeType != ShapeType.Cylinder + ); + } + + public void GetSphereProperties(out float3 center, out float radius, out quaternion orientation) + { + GetSphereProperties(out center, out radius, out EulerAngles euler); + orientation = euler; + } + + internal void GetSphereProperties(out float3 center, out float radius, out EulerAngles orientation) + { + center = m_PrimitiveCenter; + radius = m_SphereRadius; + orientation = m_PrimitiveOrientation; + } + + public void GetPlaneProperties(out float3 center, out float2 size, out quaternion orientation) + { + GetPlaneProperties(out center, out size, out EulerAngles euler); + orientation = euler; + } + + internal void GetPlaneProperties(out float3 center, out float2 size, out EulerAngles orientation) + { + center = m_PrimitiveCenter; + orientation = m_PrimitiveOrientation; + + if (m_ShapeType == ShapeType.Plane) + { + size = m_PrimitiveSize.xz; + return; + } + + UpdateCapsuleAxis(); + var look = m_Capsule.Axis; + var nextAx = k_NextAxis[look]; + var prevAx = k_NextAxis[k_NextAxis[look]]; + var ax2 = m_PrimitiveSize[nextAx] > m_PrimitiveSize[prevAx] ? nextAx : prevAx; + size = new float2(m_PrimitiveSize[ax2], m_PrimitiveSize[look]); + + var up = k_NextAxis[ax2] == look ? k_NextAxis[look] : k_NextAxis[ax2]; + var offset = quaternion.LookRotation(new float3 { [look] = 1f }, new float3 { [up] = 1f }); + + orientation.SetValue(math.mul(m_PrimitiveOrientation, offset)); + } + + public void GetPlaneProperties(out float3 vertex0, out float3 vertex1, out float3 vertex2, out float3 vertex3) + { + float3 center; + float2 size; + quaternion orientation; + GetPlaneProperties(out center, out size, out orientation); + float3 sizeYUp = math.float3(size.x, 0, size.y); + + vertex0 = center + math.mul(orientation, sizeYUp * math.float3(-0.5f, 0, 0.5f)); + vertex1 = center + math.mul(orientation, sizeYUp * math.float3( 0.5f, 0, 0.5f)); + vertex2 = center + math.mul(orientation, sizeYUp * math.float3( 0.5f, 0, -0.5f)); + vertex3 = center + math.mul(orientation, sizeYUp * math.float3(-0.5f, 0, -0.5f)); + } + + static readonly List s_Vertices = new List(65535); + + public void GetConvexHullProperties(NativeList pointCloud) + { + // TODO: currently only handling a single mesh + var mesh = GetMesh(); + if (mesh == null) + return; + mesh.GetVertices(s_Vertices); + foreach (var v in s_Vertices) + pointCloud.Add(v); + } + + public UnityMesh GetMesh() + { + if (m_CustomMesh != null) + return m_CustomMesh; + var meshFilter = gameObject.GetComponent(); + return meshFilter == null ? null : meshFilter.sharedMesh; + } + + void UpdateCapsuleAxis() + { + var cmax = math.cmax(m_PrimitiveSize); + var cmin = math.cmin(m_PrimitiveSize); + if (cmin == cmax) + return; + m_Capsule.Axis = m_PrimitiveSize.GetMaxAxis(); + } + + void UpdateCylinderAxis() => m_Cylinder.Axis = m_PrimitiveSize.GetDeviantAxis(); + + void Sync(ref CylindricalProperties props) + { + props.Height = m_PrimitiveSize[props.Axis]; + props.Radius = 0.5f * math.max( + m_PrimitiveSize[k_NextAxis[props.Axis]], + m_PrimitiveSize[k_NextAxis[k_NextAxis[props.Axis]]] + ); + } + + void SyncCapsuleProperties() + { + UpdateCapsuleAxis(); + Sync(ref m_Capsule); + } + + void SyncCylinderProperties() + { + UpdateCylinderAxis(); + Sync(ref m_Cylinder); + } + + void SyncSphereProperties() + { + m_SphereRadius = 0.5f * math.cmax(m_PrimitiveSize); + } + + public void SetBox(float3 center, float3 size, quaternion orientation) + { + var euler = m_PrimitiveOrientation; + euler.SetValue(orientation); + SetBox(center, size, euler); + } + + internal void SetBox(float3 center, float3 size, EulerAngles orientation) + { + m_ShapeType = ShapeType.Box; + m_PrimitiveCenter = center; + m_PrimitiveSize = math.max(size, new float3()); + m_PrimitiveOrientation = orientation; + + SyncCapsuleProperties(); + SyncCylinderProperties(); + SyncSphereProperties(); + } + + public void SetCapsule(float3 center, float height, float radius, quaternion orientation) + { + var euler = m_PrimitiveOrientation; + euler.SetValue(orientation); + SetCapsule(center, height, radius, euler); + } + + internal void SetCapsule(float3 center, float height, float radius, EulerAngles orientation) + { + m_ShapeType = ShapeType.Capsule; + m_PrimitiveCenter = center; + m_PrimitiveOrientation = orientation; + + radius = math.max(0f, radius); + height = math.max(height, radius * 2f); + m_PrimitiveSize = new float3(radius * 2f, radius * 2f, height); + + SyncCapsuleProperties(); + SyncCylinderProperties(); + SyncSphereProperties(); + } + + public void SetCylinder(float3 center, float height, float radius, quaternion orientation) + { + var euler = m_PrimitiveOrientation; + euler.SetValue(orientation); + SetCylinder(center, height, radius, euler); + } + + internal void SetCylinder(float3 center, float height, float radius, EulerAngles orientation) + { + m_ShapeType = ShapeType.Cylinder; + m_PrimitiveCenter = center; + m_PrimitiveOrientation = orientation; + + radius = math.max(0f, radius); + height = math.max(0f, height); + m_PrimitiveSize = new float3(radius * 2f, radius * 2f, height); + + SyncCapsuleProperties(); + SyncCylinderProperties(); + SyncSphereProperties(); + } + + public void SetSphere(float3 center, float radius, quaternion orientation) + { + var euler = m_PrimitiveOrientation; + euler.SetValue(orientation); + SetSphere(center, radius, euler); + } + + internal void SetSphere(float3 center, float radius, EulerAngles orientation) + { + m_ShapeType = ShapeType.Sphere; + m_PrimitiveCenter = center; + + radius = math.max(0f, radius); + m_PrimitiveSize = new float3(2f * radius, 2f * radius, 2f * radius); + + m_PrimitiveOrientation = orientation; + + SyncCapsuleProperties(); + SyncCylinderProperties(); + SyncSphereProperties(); + } + + public void SetPlane(float3 center, float2 size, quaternion orientation) + { + var euler = m_PrimitiveOrientation; + euler.SetValue(orientation); + SetPlane(center, size, euler); + } + + internal void SetPlane(float3 center, float2 size, EulerAngles orientation) + { + m_ShapeType = ShapeType.Plane; + m_PrimitiveCenter = center; + m_PrimitiveOrientation = orientation; + m_PrimitiveSize = new float3(size.x, 0f, size.y); + + SyncCapsuleProperties(); + SyncCylinderProperties(); + SyncSphereProperties(); + } + + public void SetConvexHull(UnityMesh convexHull = null) + { + m_ShapeType = ShapeType.ConvexHull; + m_CustomMesh = convexHull; + } + + public void SetMesh(UnityMesh mesh = null) + { + m_ShapeType = ShapeType.Mesh; + m_CustomMesh = mesh; + } + + void OnEnable() + { + // included so tick box appears in Editor + } + + static void Validate(ref CylindricalProperties props) + { + props.Height = math.max(0f, props.Height); + props.Radius = math.max(0f, props.Radius); + } + + void OnValidate() + { + m_PrimitiveSize = math.max(m_PrimitiveSize, new float3()); + Validate(ref m_Capsule); + Validate(ref m_Cylinder); + switch (m_ShapeType) + { + case ShapeType.Box: + GetBoxProperties(out var center, out var size, out EulerAngles orientation); + SetBox(center, size, orientation); + break; + case ShapeType.Capsule: + GetCapsuleProperties(out center, out var height, out var radius, out orientation); + SetCapsule(center, height, radius, orientation); + break; + case ShapeType.Cylinder: + GetCylinderProperties(out center, out height, out radius, out orientation); + SetCylinder(center, height, radius, orientation); + break; + case ShapeType.Sphere: + GetSphereProperties(out center, out radius, out orientation); + SetSphere(center, radius, orientation); + break; + case ShapeType.Plane: + GetPlaneProperties(out center, out var size2D, out orientation); + SetPlane(center, size2D, orientation); + break; + } + SyncCapsuleProperties(); + SyncCylinderProperties(); + SyncSphereProperties(); + ConvexRadius = m_ConvexRadius; + PhysicsMaterialProperties.OnValidate(ref m_Material, true); + } + + public void FitToGeometry() + { + // TODO: replace this naive generation of shape information based on bounds extents + UnityMesh mesh = GetMesh(); + if (mesh == null) + return; + + Bounds bounds = mesh.bounds; + ShapeType shapeType = m_ShapeType; + SetBox(bounds.center, bounds.size, quaternion.identity); + m_ShapeType = shapeType; + m_ConvexRadius = math.min(math.cmin(bounds.size) * 0.1f, k_DefaultConvexRadius); + + switch (m_ShapeType) + { + case ShapeType.Capsule: + GetCapsuleProperties(out var center, out var height, out var radius, out EulerAngles orientation); + SetCapsule(center, height, radius, orientation); + break; + case ShapeType.Cylinder: + GetCylinderProperties(out center, out height, out radius, out orientation); + SetCylinder(center, height, radius, orientation); + break; + case ShapeType.Sphere: + GetSphereProperties(out center, out radius, out orientation); + SetSphere(center, radius, orientation); + break; + case ShapeType.Plane: + // force recalculation of plane orientation by making it think shape type is out of date + m_ShapeType = ShapeType.Box; + GetPlaneProperties(out center, out var size2D, out orientation); + SetPlane(center, size2D, orientation); + break; + } + SyncCapsuleProperties(); + SyncCylinderProperties(); + SyncSphereProperties(); + } + + void Reset() + { + FitToGeometry(); + // TODO: also pick best primitive shape + } + } +} diff --git a/package/Unity.Physics.Authoring/Components/PhysicsShape.cs.meta b/package/Unity.Physics.Authoring/Components/PhysicsShape.cs.meta new file mode 100755 index 000000000..a78fe5868 --- /dev/null +++ b/package/Unity.Physics.Authoring/Components/PhysicsShape.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: b275e5f92732148048d7b77e264ac30e +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Unity.Physics.Authoring/Components/PhysicsStep.cs b/package/Unity.Physics.Authoring/Components/PhysicsStep.cs new file mode 100755 index 000000000..197bc677e --- /dev/null +++ b/package/Unity.Physics.Authoring/Components/PhysicsStep.cs @@ -0,0 +1,55 @@ +using UnityEngine; +using Unity.Entities; +using Unity.Mathematics; +using static Unity.Physics.PhysicsStep; + +namespace Unity.Physics.Authoring +{ + [AddComponentMenu("DOTS Physics/Physics Step")] + [DisallowMultipleComponent] + [RequiresEntityConversion] + public class PhysicsStep : MonoBehaviour, IConvertGameObjectToEntity + { + public SimulationType SimulationType = Default.SimulationType; + public float3 Gravity = Default.Gravity; + public int SolverIterationCount = Default.SolverIterationCount; + public int ThreadCountHint = Default.ThreadCountHint; + + private Entity convertedEntity = Entity.Null; + + void IConvertGameObjectToEntity.Convert(Entity entity, EntityManager dstManager, GameObjectConversionSystem conversionSystem) + { + var componentData = new Physics.PhysicsStep + { + SimulationType = SimulationType, + Gravity = Gravity, + SolverIterationCount = SolverIterationCount, + ThreadCountHint = ThreadCountHint + }; + dstManager.AddComponentData(entity, componentData); + + convertedEntity = entity; + } + + void OnValidate() + { + SolverIterationCount = math.max(1, SolverIterationCount); + ThreadCountHint = math.max(1, ThreadCountHint); + + if (!isActiveAndEnabled) return; + if (convertedEntity == Entity.Null) return; + + // This requires Entity Conversion mode to be 'Convert And Inject Game Object' + var entityManager = World.Active.EntityManager; + if (entityManager.HasComponent(convertedEntity)) + { + var component = entityManager.GetComponentData(convertedEntity); + component.SimulationType = SimulationType; + component.Gravity = Gravity; + component.SolverIterationCount = SolverIterationCount; + component.ThreadCountHint = ThreadCountHint; + entityManager.SetComponentData(convertedEntity, component); + } + } + } +} diff --git a/package/Unity.Physics.Authoring/Components/PhysicsStep.cs.meta b/package/Unity.Physics.Authoring/Components/PhysicsStep.cs.meta new file mode 100755 index 000000000..ead71d3df --- /dev/null +++ b/package/Unity.Physics.Authoring/Components/PhysicsStep.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 4d30974bf68d61043b1572197db94cdc +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Unity.Physics.Authoring/Conversion.meta b/package/Unity.Physics.Authoring/Conversion.meta new file mode 100755 index 000000000..6fcae0637 --- /dev/null +++ b/package/Unity.Physics.Authoring/Conversion.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: dca549f9b288d45d392f556b8bd2ac01 +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Unity.Physics.Authoring/Conversion/BaseShapeConversionSystem.cs b/package/Unity.Physics.Authoring/Conversion/BaseShapeConversionSystem.cs new file mode 100755 index 000000000..46009769e --- /dev/null +++ b/package/Unity.Physics.Authoring/Conversion/BaseShapeConversionSystem.cs @@ -0,0 +1,134 @@ +using System; +using Unity.Collections; +using Unity.Entities; +using Unity.Mathematics; +using UnityEngine; + +namespace Unity.Physics.Authoring +{ + public abstract class BaseShapeConversionSystem : GameObjectConversionSystem where T : Component + { + struct ComparableEntity : IEquatable, IComparable + { + public Entity Entity; + + public bool Equals(ComparableEntity other) => Entity.Equals(other.Entity); + + public int CompareTo(ComparableEntity other) => Entity.Index - other.Entity.Index; + } + + struct LeafShapeData + { + public CompoundCollider.ColliderBlobInstance ColliderBlobInstance; + public byte CustomFlags; + } + + protected abstract bool ShouldConvertShape(T shape); + protected abstract GameObject GetPrimaryBody(T shape); + protected abstract BlobAssetReference ProduceColliderBlob(T shape); + protected abstract byte GetCustomFlags(T shape); + + void ConvertShape(T shape) + { + if (!ShouldConvertShape(shape)) + return; + + var body = GetPrimaryBody(shape); + var entity = GetPrimaryEntity(body); + + var colliderBlob = ProduceColliderBlob(shape); + + if (!DstEntityManager.HasComponent(entity)) + DstEntityManager.AddComponentData(entity, new PhysicsCollider()); + + var collider = DstEntityManager.GetComponentData(entity); + var customFlags = GetCustomFlags(shape); + if (!collider.IsValid && body == shape.gameObject) + { + // Shape is on same object as body, therefore has no relative transform. + // Set it directly into the entity component. + collider.Value = colliderBlob; + DstEntityManager.SetComponentData(entity, collider); + if (customFlags != 0) + DstEntityManager.AddOrSetComponent(entity, new PhysicsCustomData { Value = customFlags }); + } + else + { + // Body has multiple shapes or may have a relative transform. + // Store it for building compound colliders in the second pass. + // Note: Not including relative scale, since that is baked into the colliders + var worldFromBody = new RigidTransform(body.transform.rotation, body.transform.position); + var worldFromShape = new RigidTransform(shape.transform.rotation, shape.transform.position); + var compoundFromChild = math.mul(math.inverse(worldFromBody), worldFromShape); + m_ExtraColliders.Add( + new ComparableEntity { Entity = entity }, + new LeafShapeData + { + ColliderBlobInstance = new CompoundCollider.ColliderBlobInstance + { + CompoundFromChild = compoundFromChild, + Collider = colliderBlob + }, + CustomFlags = customFlags + } + ); + } + } + + NativeMultiHashMap m_ExtraColliders; + + protected override void OnUpdate() + { + // A map from entities to arrays of colliders that were not applied to the body during the first pass. + m_ExtraColliders = new NativeMultiHashMap(64, Allocator.Temp); // TODO - add custom data to Physics.Collider? + + // First pass. + // Convert all editor shape components into colliders, and either apply them to their parent rigid body + // or store them for building compound colliders in the second pass. + Entities.ForEach(ConvertShape); + + // Second pass. + // Merge any leftover colliders into their parent rigid bodies, as compound colliders. + if (m_ExtraColliders.Length > 0) + { + var keys = m_ExtraColliders.GetUniqueKeyArray(Allocator.Temp); + using (keys.Item1) + { + for (var k = 0; k < keys.Item2; ++k) + { + ComparableEntity entity = keys.Item1[k]; + var collider = DstEntityManager.GetComponentData(entity.Entity); + var children = new NativeList(16, Allocator.Temp); + + if (collider.IsValid) + { + // Include the already assigned collider as a child + children.Add(new CompoundCollider.ColliderBlobInstance { Collider = collider.Value, CompoundFromChild = RigidTransform.identity }); + } + + byte customData = 0; + if (m_ExtraColliders.TryGetFirstValue(entity, out var child, out var iterator)) + { + do + { + children.Add(child.ColliderBlobInstance); + customData &= child.CustomFlags; // TODO: Should be |= ?? + } while (m_ExtraColliders.TryGetNextValue(out child, ref iterator)); + } + + collider.Value = CompoundCollider.Create(children); + + children.Dispose(); + + DstEntityManager.SetComponentData(entity.Entity, collider); + + if (customData != 0) + DstEntityManager.AddOrSetComponent(entity.Entity, new PhysicsCustomData { Value = customData }); + } + } + } + m_ExtraColliders.Dispose(); + } + } +} + diff --git a/package/Unity.Physics.Authoring/Conversion/BaseShapeConversionSystem.cs.meta b/package/Unity.Physics.Authoring/Conversion/BaseShapeConversionSystem.cs.meta new file mode 100755 index 000000000..414761b3f --- /dev/null +++ b/package/Unity.Physics.Authoring/Conversion/BaseShapeConversionSystem.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 4a56c3660f8ff41798691d34fb7630f3 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Unity.Physics.Authoring/Conversion/ConversionExtensions.cs b/package/Unity.Physics.Authoring/Conversion/ConversionExtensions.cs new file mode 100755 index 000000000..56ca4ee3d --- /dev/null +++ b/package/Unity.Physics.Authoring/Conversion/ConversionExtensions.cs @@ -0,0 +1,28 @@ +using Unity.Entities; +using Unity.Mathematics; +using UnityEngine; + +namespace Unity.Physics.Authoring +{ + static class ConversionExtensions + { + internal static void AddOrSetComponent(this EntityManager manager, Entity entity, T value) + where T : struct, IComponentData + { + if (!manager.HasComponent(entity)) + manager.AddComponentData(entity, value); + else if (!TypeManager.IsZeroSized(TypeManager.GetTypeIndex())) + manager.SetComponentData(entity, value); + } + + internal static float3[] GetScaledVertices(this UnityEngine.Mesh mesh, float3 scale) + { + var source = mesh.vertices; + var vertices = new float3[mesh.vertices.Length]; + for (int i = 0, count = vertices.Length; i < count; ++i) + vertices[i] = source[i] * scale; + return vertices; + } + } +} + diff --git a/package/Unity.Physics.Authoring/Conversion/ConversionExtensions.cs.meta b/package/Unity.Physics.Authoring/Conversion/ConversionExtensions.cs.meta new file mode 100755 index 000000000..9ba8f35fe --- /dev/null +++ b/package/Unity.Physics.Authoring/Conversion/ConversionExtensions.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 5e1cb9623bd494b5bb4bc812da88c7a7 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Unity.Physics.Authoring/Conversion/FirstPassLegacyRigidbodyConversionSystem.cs b/package/Unity.Physics.Authoring/Conversion/FirstPassLegacyRigidbodyConversionSystem.cs new file mode 100755 index 000000000..24d44c035 --- /dev/null +++ b/package/Unity.Physics.Authoring/Conversion/FirstPassLegacyRigidbodyConversionSystem.cs @@ -0,0 +1,18 @@ +using UnityEngine; + +namespace Unity.Physics.Authoring +{ + public class FirstPassLegacyRigidbodyConversionSystem : GameObjectConversionSystem + { + protected override void OnUpdate() + { + Entities.ForEach( + (Rigidbody body) => + { + var entity = GetPrimaryEntity(body.gameObject); + DstEntityManager.AddOrSetComponent(entity, new PhysicsCollider()); + } + ); + } + } +} diff --git a/package/Unity.Physics.Authoring/Conversion/FirstPassLegacyRigidbodyConversionSystem.cs.meta b/package/Unity.Physics.Authoring/Conversion/FirstPassLegacyRigidbodyConversionSystem.cs.meta new file mode 100755 index 000000000..742ef6ade --- /dev/null +++ b/package/Unity.Physics.Authoring/Conversion/FirstPassLegacyRigidbodyConversionSystem.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 92dc0b2b752cf4fa3a34e44a9546231e +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Unity.Physics.Authoring/Conversion/FirstPassPhysicsBodyConversionSystem.cs b/package/Unity.Physics.Authoring/Conversion/FirstPassPhysicsBodyConversionSystem.cs new file mode 100755 index 000000000..9cc14bfba --- /dev/null +++ b/package/Unity.Physics.Authoring/Conversion/FirstPassPhysicsBodyConversionSystem.cs @@ -0,0 +1,19 @@ +using UnityEngine; + +namespace Unity.Physics.Authoring +{ + public class FirstPassPhysicsBodyConversionSystem : GameObjectConversionSystem + { + protected override void OnUpdate() + { + Entities.ForEach( + (PhysicsBody body) => + { + if (!body.enabled) return; + var entity = GetPrimaryEntity(body.gameObject); + DstEntityManager.AddOrSetComponent(entity, new PhysicsCollider()); + } + ); + } + } +} diff --git a/package/Unity.Physics.Authoring/Conversion/FirstPassPhysicsBodyConversionSystem.cs.meta b/package/Unity.Physics.Authoring/Conversion/FirstPassPhysicsBodyConversionSystem.cs.meta new file mode 100755 index 000000000..2c0271ef5 --- /dev/null +++ b/package/Unity.Physics.Authoring/Conversion/FirstPassPhysicsBodyConversionSystem.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 6dc5d811e03b74f33b22f0075da9004b +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Unity.Physics.Authoring/Conversion/LegacyColliderConversionSystem.cs b/package/Unity.Physics.Authoring/Conversion/LegacyColliderConversionSystem.cs new file mode 100755 index 000000000..2755c6fa9 --- /dev/null +++ b/package/Unity.Physics.Authoring/Conversion/LegacyColliderConversionSystem.cs @@ -0,0 +1,184 @@ +using System; +using System.Collections.Generic; +using Unity.Entities; +using Unity.Mathematics; +using UnityEngine; +using LegacyPhysics = UnityEngine.Physics; +using LegacyCollider = UnityEngine.Collider; +using LegacyBox = UnityEngine.BoxCollider; +using LegacyCapsule = UnityEngine.CapsuleCollider; +using LegacyMesh = UnityEngine.MeshCollider; +using LegacySphere = UnityEngine.SphereCollider; +using Unity.Collections; + +namespace Unity.Physics.Authoring +{ + public abstract class BaseLegacyColliderConversionSystem : BaseShapeConversionSystem where T : LegacyCollider + { + static readonly IReadOnlyDictionary k_MaterialCombineLookup = + new Dictionary + { + { PhysicMaterialCombine.Average, Material.CombinePolicy.ArithmeticMean }, + { PhysicMaterialCombine.Maximum, Material.CombinePolicy.Maximum }, + { PhysicMaterialCombine.Minimum, Material.CombinePolicy.Minimum } + }; + + protected static Material ProduceMaterial(LegacyCollider collider) + { + var material = new Material + { + // n.b. need to manually opt in to collision events with legacy colliders if desired + Flags = collider.isTrigger + ? Material.MaterialFlags.IsTrigger + : new Material.MaterialFlags() + }; + + var legacyMaterial = collider.material; + if (legacyMaterial != null) + { + material.Friction = legacyMaterial.dynamicFriction; + if (k_MaterialCombineLookup.TryGetValue(legacyMaterial.frictionCombine, out var combine)) + material.FrictionCombinePolicy = combine; + else + Debug.LogWarning( + $"{collider.name} uses {legacyMaterial.name}, which specifies non-convertible mode {legacyMaterial.frictionCombine} for {nameof(PhysicMaterial.frictionCombine)}.", + collider + ); + + material.Restitution = legacyMaterial.bounciness; + if (k_MaterialCombineLookup.TryGetValue(legacyMaterial.bounceCombine, out combine)) + material.RestitutionCombinePolicy = combine; + else + Debug.LogWarning( + $"{collider.name} uses {legacyMaterial.name}, which specifies non-convertible mode {legacyMaterial.bounceCombine} for {nameof(PhysicMaterial.bounceCombine)}.", + collider + ); + } + + return material; + } + + protected static CollisionFilter ProduceCollisionFilter(LegacyCollider collider) + { + var layer = collider.gameObject.layer; + var filter = new CollisionFilter { CategoryBits = (uint)(1 << collider.gameObject.layer) }; + for (var i = 0; i < 32; ++i) + filter.MaskBits |= (uint)(LegacyPhysics.GetIgnoreLayerCollision(layer, i) ? 0 : 1 << i); + return filter; + } + + protected override bool ShouldConvertShape(T shape) => true; + protected override GameObject GetPrimaryBody(T shape) => shape.GetPrimaryBody(); + protected override byte GetCustomFlags(T shape) => 0; + } + + [UpdateAfter(typeof(FirstPassPhysicsBodyConversionSystem))] + [UpdateAfter(typeof(FirstPassLegacyRigidbodyConversionSystem))] + public class LegacyBoxColliderConversionSystem : BaseLegacyColliderConversionSystem + { + protected override BlobAssetReference ProduceColliderBlob(LegacyBox shape) + { + var worldCenter = math.mul(shape.transform.localToWorldMatrix, new float4(shape.center, 1f)); + var shapeFromWorld = math.inverse( + new float4x4(new RigidTransform(shape.transform.rotation, shape.transform.position)) + ); + var center = math.mul(shapeFromWorld, worldCenter).xyz; + + var linearScale = (float3)shape.transform.lossyScale; + var size = math.abs(shape.size * linearScale); + + return BoxCollider.Create( + center, + quaternion.identity, + size, + PhysicsShape.k_DefaultConvexRadius, + ProduceCollisionFilter(shape), + ProduceMaterial(shape) + ); + } + } + + [UpdateAfter(typeof(FirstPassPhysicsBodyConversionSystem))] + [UpdateAfter(typeof(FirstPassLegacyRigidbodyConversionSystem))] + public class LegacyCapsuleColliderConversionSystem : BaseLegacyColliderConversionSystem + { + protected override BlobAssetReference ProduceColliderBlob(LegacyCapsule shape) + { + var linearScale = (float3)shape.transform.lossyScale; + + // radius is max of the two non-height axes + var radius = shape.radius * math.cmax(new float3(math.abs(linearScale)) { [shape.direction] = 0f }); + + var ax = new float3 { [shape.direction] = 1f }; + var vertex = ax * (0.5f * shape.height); + var rt = new RigidTransform(shape.transform.rotation, shape.transform.position); + var worldCenter = math.mul(shape.transform.localToWorldMatrix, new float4(shape.center, 0f)); + var offset = math.mul(math.inverse(new float4x4(rt)), worldCenter).xyz - shape.center * math.abs(linearScale); + + var v0 = offset + ((float3)shape.center + vertex) * math.abs(linearScale) - ax * radius; + var v1 = offset + ((float3)shape.center - vertex) * math.abs(linearScale) + ax * radius; + + return CapsuleCollider.Create( + v0, + v1, + radius, + ProduceCollisionFilter(shape), + ProduceMaterial(shape) + ); + } + } + + [UpdateAfter(typeof(FirstPassPhysicsBodyConversionSystem))] + [UpdateAfter(typeof(FirstPassLegacyRigidbodyConversionSystem))] + public class LegacySphereColliderConversionSystem : BaseLegacyColliderConversionSystem + { + protected override BlobAssetReference ProduceColliderBlob(LegacySphere shape) + { + var scale = (float3)shape.transform.lossyScale; + return SphereCollider.Create( + shape.center * scale, + shape.radius * math.cmax(math.abs(scale)), + ProduceCollisionFilter(shape), + ProduceMaterial(shape) + ); + } + } + + [UpdateAfter(typeof(FirstPassPhysicsBodyConversionSystem))] + [UpdateAfter(typeof(FirstPassLegacyRigidbodyConversionSystem))] + public class LegacyMeshColliderConversionSystem : BaseLegacyColliderConversionSystem + { + List m_Vertices = new List(65535 / 2); + + protected override BlobAssetReference ProduceColliderBlob(LegacyMesh shape) + { + if (shape.sharedMesh == null) + { + throw new InvalidOperationException( + $"No {nameof(LegacyMesh.sharedMesh)} assigned to {typeof(MeshCollider)} on {shape.name}." + ); + } + + var filter = ProduceCollisionFilter(shape); + var material = ProduceMaterial(shape); + + using (var pointCloud = new NativeList(shape.sharedMesh.vertexCount, Allocator.Temp)) + { + // transform points into world space and back into shape space + shape.sharedMesh.GetVertices(m_Vertices); + var shapeFromWorld = math.inverse( + new float4x4(new RigidTransform(shape.transform.rotation, shape.transform.position)) + ); + for (int i = 0, count = m_Vertices.Count; i < count; ++i) + { + var worldPt = math.mul(shape.transform.localToWorldMatrix, new float4(m_Vertices[i], 1f)); + pointCloud.Add(math.mul(shapeFromWorld, worldPt).xyz); + } + + return shape.convex + ? ConvexCollider.Create(pointCloud, PhysicsShape.k_DefaultConvexRadius, new float3(1f), filter, material) + : MeshCollider.Create(pointCloud.ToArray(), shape.sharedMesh.triangles, filter, material); + } + } + } +} diff --git a/package/Unity.Physics.Authoring/Conversion/LegacyColliderConversionSystem.cs.meta b/package/Unity.Physics.Authoring/Conversion/LegacyColliderConversionSystem.cs.meta new file mode 100755 index 000000000..bf39c1ac7 --- /dev/null +++ b/package/Unity.Physics.Authoring/Conversion/LegacyColliderConversionSystem.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 0a60928f473384760b8b9ece4d81a8af +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Unity.Physics.Authoring/Conversion/PhysicsShapeConversionSystem.cs b/package/Unity.Physics.Authoring/Conversion/PhysicsShapeConversionSystem.cs new file mode 100755 index 000000000..6c65fe12d --- /dev/null +++ b/package/Unity.Physics.Authoring/Conversion/PhysicsShapeConversionSystem.cs @@ -0,0 +1,148 @@ +using System; +using Unity.Collections; +using Unity.Entities; +using Unity.Mathematics; +using UnityEngine; + +namespace Unity.Physics.Authoring +{ + [UpdateAfter(typeof(FirstPassPhysicsBodyConversionSystem))] + [UpdateAfter(typeof(FirstPassLegacyRigidbodyConversionSystem))] + public class PhysicsShapeConversionSystem : BaseShapeConversionSystem + { + static Material ProduceMaterial(PhysicsShape shape) + { + // TODO: TBD how we will author editor content for other shape flags + var flags = new Material.MaterialFlags(); + if (shape.IsTrigger) + { + flags = Material.MaterialFlags.IsTrigger; + } + else if (shape.RaisesCollisionEvents) + { + flags = Material.MaterialFlags.EnableCollisionEvents; + } + + return new Material + { + Friction = shape.Friction.Value, + FrictionCombinePolicy = shape.Friction.CombineMode, + Restitution = shape.Restitution.Value, + RestitutionCombinePolicy = shape.Restitution.CombineMode, + Flags = flags + }; + } + + static CollisionFilter ProduceCollisionFilter(PhysicsShape shape) + { + // TODO: determine optimal workflow for specifying group index + return new CollisionFilter + { + CategoryBits = unchecked((uint)shape.BelongsTo), + MaskBits = unchecked((uint)shape.CollidesWith), + }; + } + + protected override bool ShouldConvertShape(PhysicsShape shape) => shape.enabled; + + protected override GameObject GetPrimaryBody(PhysicsShape shape) => shape.GetPrimaryBody(); + + protected override BlobAssetReference ProduceColliderBlob(PhysicsShape shape) + { + var material = ProduceMaterial(shape); + var collisionFilter = ProduceCollisionFilter(shape); + + var blob = new BlobAssetReference(); + shape.GetBakeTransformation(out var linearScalar, out var radiusScalar); + switch (shape.ShapeType) + { + case ShapeType.Box: + shape.GetBoxProperties(out var center, out var size, out quaternion orientation); + blob = BoxCollider.Create( + center * linearScalar, + orientation, + size * linearScalar, + shape.ConvexRadius * radiusScalar, + collisionFilter, + material); + break; + case ShapeType.Capsule: + shape.GetCapsuleProperties(out var v0, out var v1, out var radius); + blob = CapsuleCollider.Create( + v0 * linearScalar, + v1 * linearScalar, + radius * radiusScalar, + collisionFilter, + material); + break; + case ShapeType.Sphere: + shape.GetSphereProperties(out center, out radius, out orientation); + blob = SphereCollider.Create( + center * linearScalar, + radius * radiusScalar, + collisionFilter, + material); + break; + case ShapeType.Cylinder: + shape.GetCylinderProperties(out center, out var height, out radius, out orientation); + blob = CylinderCollider.Create( + center, + height, + radius, + orientation, + shape.ConvexRadius * radiusScalar, + linearScalar, + collisionFilter, + material); + break; + case ShapeType.Plane: + shape.GetPlaneProperties(out v0, out v1, out var v2, out var v3); + blob = PolygonCollider.CreateQuad( + v0 * linearScalar, + v1 * linearScalar, + v2 * linearScalar, + v3 * linearScalar, + collisionFilter, + material); + break; + case ShapeType.ConvexHull: + var pointCloud = new NativeList(65535, Allocator.Temp); + shape.GetConvexHullProperties(pointCloud); + if (pointCloud.Length == 0) + { + pointCloud.Dispose(); + throw new InvalidOperationException( + $"No vertices associated with {shape.name}. Add a {typeof(MeshFilter)} component or assign {nameof(PhysicsShape.CustomMesh)}." + ); + } + blob = ConvexCollider.Create( + pointCloud, + shape.ConvexRadius * radiusScalar, + linearScalar, + collisionFilter, + material); + pointCloud.Dispose(); + break; + case ShapeType.Mesh: + // TODO: no convex radius? + var mesh = shape.GetMesh(); + if (mesh == null) + { + throw new InvalidOperationException( + $"No mesh associated with {shape.name}. Add a {typeof(MeshFilter)} component or assign {nameof(PhysicsShape.CustomMesh)}." + ); + } + else + { + blob = MeshCollider.Create(mesh.GetScaledVertices(linearScalar), mesh.triangles, collisionFilter, material); + } + break; + default: + break; + } + return blob; + } + + protected override byte GetCustomFlags(PhysicsShape shape) => shape.CustomFlags; + } +} diff --git a/package/Unity.Physics.Authoring/Conversion/PhysicsShapeConversionSystem.cs.meta b/package/Unity.Physics.Authoring/Conversion/PhysicsShapeConversionSystem.cs.meta new file mode 100755 index 000000000..f3fd74ada --- /dev/null +++ b/package/Unity.Physics.Authoring/Conversion/PhysicsShapeConversionSystem.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: acd55f5f1097e43ce8eb22700a265fe6 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Unity.Physics.Authoring/Conversion/SecondPassLegacyRigidbodyConversionSystem.cs b/package/Unity.Physics.Authoring/Conversion/SecondPassLegacyRigidbodyConversionSystem.cs new file mode 100755 index 000000000..eff90e653 --- /dev/null +++ b/package/Unity.Physics.Authoring/Conversion/SecondPassLegacyRigidbodyConversionSystem.cs @@ -0,0 +1,47 @@ +using Unity.Entities; + +namespace Unity.Physics.Authoring +{ + [UpdateAfter(typeof(SecondPassPhysicsBodyConversionSystem))] + public class SecondPassLegacyRigidbodyConversionSystem : GameObjectConversionSystem + { + protected override void OnUpdate() + { + Entities.ForEach( + (UnityEngine.Rigidbody body) => + { + var entity = GetPrimaryEntity(body.gameObject); + + // prefer conversions from non-legacy data if they have already been performed + if (DstEntityManager.HasComponent(entity)) + return; + + if (body.gameObject.isStatic) + return; + + // Build mass component + var massProperties = DstEntityManager.GetComponentData(entity).MassProperties; + // n.b. no way to know if CoM was manually adjusted, so all legacy Rigidbody objects use auto CoM + DstEntityManager.AddOrSetComponent(entity, !body.isKinematic ? + PhysicsMass.CreateDynamic(massProperties, body.mass) : + PhysicsMass.CreateKinematic(massProperties)); + + DstEntityManager.AddOrSetComponent(entity, new PhysicsVelocity()); + + if (!body.isKinematic) + { + DstEntityManager.AddOrSetComponent(entity, new PhysicsDamping + { + Linear = body.drag, + Angular = body.angularDrag + }); + if (!body.useGravity) + DstEntityManager.AddOrSetComponent(entity, new PhysicsGravityFactor { Value = 0f }); + } + else + DstEntityManager.AddOrSetComponent(entity, new PhysicsGravityFactor { Value = 0 }); + } + ); + } + } +} diff --git a/package/Unity.Physics.Authoring/Conversion/SecondPassLegacyRigidbodyConversionSystem.cs.meta b/package/Unity.Physics.Authoring/Conversion/SecondPassLegacyRigidbodyConversionSystem.cs.meta new file mode 100755 index 000000000..4e444e1a8 --- /dev/null +++ b/package/Unity.Physics.Authoring/Conversion/SecondPassLegacyRigidbodyConversionSystem.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: e1f8d968837784bd28087085e97040f9 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Unity.Physics.Authoring/Conversion/SecondPassPhysicsBodyConversionSystem.cs b/package/Unity.Physics.Authoring/Conversion/SecondPassPhysicsBodyConversionSystem.cs new file mode 100755 index 000000000..bf6464dd3 --- /dev/null +++ b/package/Unity.Physics.Authoring/Conversion/SecondPassPhysicsBodyConversionSystem.cs @@ -0,0 +1,71 @@ +using Unity.Entities; +using Unity.Mathematics; + +namespace Unity.Physics.Authoring +{ + [UpdateAfter(typeof(LegacyBoxColliderConversionSystem))] + [UpdateAfter(typeof(LegacyCapsuleColliderConversionSystem))] + [UpdateAfter(typeof(LegacySphereColliderConversionSystem))] + [UpdateAfter(typeof(LegacyMeshColliderConversionSystem))] + [UpdateAfter(typeof(PhysicsShapeConversionSystem))] + [UpdateBefore(typeof(SecondPassLegacyRigidbodyConversionSystem))] + public class SecondPassPhysicsBodyConversionSystem : GameObjectConversionSystem + { + protected override void OnUpdate() + { + Entities.ForEach( + (PhysicsBody body) => + { + if (!body.enabled) + return; + var entity = GetPrimaryEntity(body.gameObject); + + if (body.MotionType == BodyMotionType.Static) + return; + + // Build mass component + var massProperties = DstEntityManager.GetComponentData(entity).MassProperties; + if (body.OverrideDefaultMassDistribution) + { + massProperties.MassDistribution = body.CustomMassDistribution; + // Increase the angular expansion factor to account for the shift in center of mass + massProperties.AngularExpansionFactor += math.length(massProperties.MassDistribution.Transform.pos - body.CustomMassDistribution.Transform.pos); + } + DstEntityManager.AddOrSetComponent(entity, body.MotionType == BodyMotionType.Dynamic ? + PhysicsMass.CreateDynamic(massProperties, body.Mass) : + PhysicsMass.CreateKinematic(massProperties)); + + DstEntityManager.AddOrSetComponent(entity, new PhysicsVelocity + { + Linear = body.InitialLinearVelocity, + Angular = body.InitialAngularVelocity + }); + + if (body.MotionType == BodyMotionType.Dynamic) + { + // TODO make these optional in editor? + DstEntityManager.AddOrSetComponent(entity, new PhysicsDamping + { + Linear = body.LinearDamping, + Angular = body.AngularDamping + }); + if (body.GravityFactor != 1) + { + DstEntityManager.AddOrSetComponent(entity, new PhysicsGravityFactor + { + Value = body.GravityFactor + }); + } + } + else if (body.MotionType == BodyMotionType.Kinematic) + { + DstEntityManager.AddOrSetComponent(entity, new PhysicsGravityFactor + { + Value = 0 + }); + } + } + ); + } + } +} diff --git a/package/Unity.Physics.Authoring/Conversion/SecondPassPhysicsBodyConversionSystem.cs.meta b/package/Unity.Physics.Authoring/Conversion/SecondPassPhysicsBodyConversionSystem.cs.meta new file mode 100755 index 000000000..2b2b7c19c --- /dev/null +++ b/package/Unity.Physics.Authoring/Conversion/SecondPassPhysicsBodyConversionSystem.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 04450748c0c43499bbae25b99d736c76 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Unity.Physics.Authoring/Data.meta b/package/Unity.Physics.Authoring/Data.meta new file mode 100755 index 000000000..2c08f24ee --- /dev/null +++ b/package/Unity.Physics.Authoring/Data.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: 32b93acb4de214bdbaf6e45c9efd5979 +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Unity.Physics.Authoring/Data/PhysicsMaterialProperties.cs b/package/Unity.Physics.Authoring/Data/PhysicsMaterialProperties.cs new file mode 100755 index 000000000..4bb5b16b4 --- /dev/null +++ b/package/Unity.Physics.Authoring/Data/PhysicsMaterialProperties.cs @@ -0,0 +1,249 @@ +using System; +using Unity.Mathematics; +using UnityEngine; + +namespace Unity.Physics.Authoring +{ + interface IPhysicsMaterialProperties + { + bool IsTrigger { get; set; } + + PhysicsMaterialCoefficient Friction { get; set; } + + PhysicsMaterialCoefficient Restitution { get; set; } + + int BelongsTo { get; set; } + bool GetBelongsTo(int categoryIndex); + void SetBelongsTo(int categoryIndex, bool value); + + int CollidesWith { get; set; } + bool GetCollidesWith(int categoryIndex); + void SetCollidesWith(int categoryIndex, bool value); + + bool RaisesCollisionEvents { get; set; } + + // TODO: Enable Mass Factors? + // TODO: Surface Velocity? + // TODO: Max Impulse? + + byte CustomFlags { get; set; } + bool GetCustomFlag(int customFlagIndex); + void SetCustomFlag(int customFlagIndex, bool value); + } + + interface IInheritPhysicsMaterialProperties : IPhysicsMaterialProperties + { + PhysicsMaterialTemplate Template { get; set; } + bool OverrideIsTrigger { get; set; } + bool OverrideFriction { get; set; } + bool OverrideRestitution { get; set; } + bool OverrideBelongsTo { get; set; } + bool OverrideCollidesWith { get; set; } + bool OverrideRaisesCollisionEvents { get; set; } + bool OverrideCustomFlags { get; set; } + } + + [Serializable] + public struct PhysicsMaterialCoefficient + { + [SoftRange(0f, 1f, TextFieldMax = float.MaxValue)] + public float Value; + public Material.CombinePolicy CombineMode; + } + + abstract class OverridableValue + { + public bool Override { get => m_Override; set => m_Override = value; } + [SerializeField] + bool m_Override; + } + + abstract class OverridableValue : OverridableValue where T : struct + { + public T Value + { + get => m_Value; + set + { + m_Value = value; + Override = true; + } + } + [SerializeField] + T m_Value; + + public void OnValidate() => OnValidate(ref m_Value); + protected virtual void OnValidate(ref T value) {} + } + + [Serializable] + class OverridableBool : OverridableValue {} + + [Serializable] + class OverridableMaterialCoefficient : OverridableValue + { + protected override void OnValidate(ref PhysicsMaterialCoefficient value) => + value.Value = math.max(0f, value.Value); + } + + [Serializable] + class OverridableFlags : OverridableValue + { + public OverridableFlags(int capacity) => m_Value = new bool[capacity]; + + public int Value + { + get + { + var result = 0; + for (var i = 0; i < m_Value.Length; ++i) + result |= (m_Value[i] ? 1 : 0) << i; + return result; + } + set + { + for (var i = 0; i < m_Value.Length; ++i) + m_Value[i] = (value & (1 << i)) != 0; + Override = true; + } + } + public bool this[int index] + { + get => m_Value[index]; + set + { + m_Value[index] = value; + Override = true; + } + } + + [SerializeField] + bool[] m_Value; + + public void OnValidate(int capacity) => Array.Resize(ref m_Value, capacity); + } + + [Serializable] + class PhysicsMaterialProperties : IInheritPhysicsMaterialProperties + { + public PhysicsMaterialProperties(bool supportsTemplate) => m_SupportsTemplate = supportsTemplate; + + [SerializeField, HideInInspector] + bool m_SupportsTemplate; + + public PhysicsMaterialTemplate Template + { + get => m_Template; + set => m_Template = m_SupportsTemplate ? value : null; + } + [SerializeField] + [Tooltip("Assign a template to use its values.")] + PhysicsMaterialTemplate m_Template; + + static T Get(OverridableValue value, T? templateValue) where T : struct => + value.Override || templateValue == null ? value.Value : templateValue.Value; + + public bool OverrideIsTrigger { get => m_IsTrigger.Override; set => m_IsTrigger.Override = value; } + public bool IsTrigger + { + get => Get(m_IsTrigger, m_Template == null ? null : m_Template?.IsTrigger); + set => m_IsTrigger.Value = value; + } + [SerializeField] + OverridableBool m_IsTrigger = new OverridableBool(); + + public bool OverrideFriction { get => m_Friction.Override; set => m_Friction.Override = value; } + public PhysicsMaterialCoefficient Friction + { + get => Get(m_Friction, m_Template == null ? null : m_Template?.Friction); + set => m_Friction.Value= value; + } + [SerializeField] + OverridableMaterialCoefficient m_Friction = new OverridableMaterialCoefficient + { + Value = new PhysicsMaterialCoefficient { Value = 0.5f, CombineMode = Material.CombinePolicy.GeometricMean }, + Override = false + }; + + public bool OverrideRestitution { get => m_Restitution.Override; set => m_Restitution.Override = value; } + public PhysicsMaterialCoefficient Restitution + { + get => Get(m_Restitution, m_Template == null ? null : m_Template?.Restitution); + set => m_Restitution.Value = value; + } + [SerializeField] + OverridableMaterialCoefficient m_Restitution = new OverridableMaterialCoefficient + { + Value = new PhysicsMaterialCoefficient { Value = 0f, CombineMode = Material.CombinePolicy.Maximum }, + Override = false + }; + + static int Get(OverridableFlags flags, int? templateValue) => + flags.Override || templateValue == null ? flags.Value : templateValue.Value; + + public bool OverrideBelongsTo { get => m_BelongsTo.Override; set => m_BelongsTo.Override = value; } + public int BelongsTo { get => Get(m_BelongsTo, m_Template?.BelongsTo); set => m_BelongsTo.Value = value; } + public bool GetBelongsTo(int categoryIndex) => m_BelongsTo.Override || m_Template == null + ? m_BelongsTo[categoryIndex] + : m_Template.GetBelongsTo(categoryIndex); + public void SetBelongsTo(int categoryIndex, bool value) => m_BelongsTo[categoryIndex] = value; + [SerializeField] + OverridableFlags m_BelongsTo = new OverridableFlags(32) { Value = ~0, Override = false}; + + public bool OverrideCollidesWith { get => m_CollidesWith.Override; set => m_CollidesWith.Override = value; } + public int CollidesWith + { + get => Get(m_CollidesWith, m_Template?.CollidesWith); + set => m_CollidesWith.Value = value; + } + public bool GetCollidesWith(int categoryIndex) => m_CollidesWith.Override || m_Template == null + ? m_CollidesWith[categoryIndex] + : m_Template.GetCollidesWith(categoryIndex); + public void SetCollidesWith(int categoryIndex, bool value) => m_CollidesWith[categoryIndex] = value; + [SerializeField] + OverridableFlags m_CollidesWith = new OverridableFlags(32) { Value = ~0, Override = false }; + + public bool OverrideRaisesCollisionEvents { get => m_RaisesCollisionEvents.Override; set => m_RaisesCollisionEvents.Override = value; } + public bool RaisesCollisionEvents + { + get => Get(m_RaisesCollisionEvents, m_Template == null ? null : m_Template?.RaisesCollisionEvents); + set => m_RaisesCollisionEvents.Value = value; + } + [SerializeField] + OverridableBool m_RaisesCollisionEvents = new OverridableBool(); + + public bool OverrideCustomFlags { get => m_CustomFlags.Override; set => m_CustomFlags.Override = value; } + public byte CustomFlags + { + get => (byte)Get(m_CustomFlags, m_Template?.CustomFlags); + set => m_CustomFlags.Value = value; + } + public bool GetCustomFlag(int customFlagIndex) => m_CustomFlags.Override || m_Template == null + ? m_CustomFlags[customFlagIndex] + : m_Template.GetCustomFlag(customFlagIndex); + public void SetCustomFlag(int customFlagIndex, bool value) => m_CustomFlags[customFlagIndex] = value; + [SerializeField] + OverridableFlags m_CustomFlags = new OverridableFlags(8); + + internal static void OnValidate(ref PhysicsMaterialProperties material, bool supportsTemplate) + { + material.m_SupportsTemplate = supportsTemplate; + if (!supportsTemplate) + { + material.m_Template = null; + material.m_IsTrigger.Override = true; + material.m_Friction.Override = true; + material.m_Restitution.Override = true; + material.m_BelongsTo.Override = true; + material.m_CollidesWith.Override = true; + material.m_RaisesCollisionEvents.Override = true; + material.m_CustomFlags.Override = true; + } + material.m_Friction.OnValidate(); + material.m_Restitution.OnValidate(); + material.m_BelongsTo.OnValidate(32); + material.m_CollidesWith.OnValidate(32); + material.m_CustomFlags.OnValidate(8); + } + } +} diff --git a/package/Unity.Physics.Authoring/Data/PhysicsMaterialProperties.cs.meta b/package/Unity.Physics.Authoring/Data/PhysicsMaterialProperties.cs.meta new file mode 100755 index 000000000..b503b9c98 --- /dev/null +++ b/package/Unity.Physics.Authoring/Data/PhysicsMaterialProperties.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: c1e08ac6ebe62466ea40565d8591ae9e +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Unity.Physics.Authoring/PropertyAttributes.meta b/package/Unity.Physics.Authoring/PropertyAttributes.meta new file mode 100755 index 000000000..eef48557b --- /dev/null +++ b/package/Unity.Physics.Authoring/PropertyAttributes.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: 8636ccd2301bb41d896662e81681697e +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Unity.Physics.Authoring/PropertyAttributes/PropertyAttributes.cs b/package/Unity.Physics.Authoring/PropertyAttributes/PropertyAttributes.cs new file mode 100755 index 000000000..03e506cc9 --- /dev/null +++ b/package/Unity.Physics.Authoring/PropertyAttributes/PropertyAttributes.cs @@ -0,0 +1,20 @@ +using UnityEngine; + +namespace Unity.Physics.Authoring +{ + sealed class EnumFlagsAttribute : PropertyAttribute {} + sealed class ExpandChildrenAttribute : PropertyAttribute {} + sealed class SoftRangeAttribute : PropertyAttribute + { + public readonly float SliderMin; + public readonly float SliderMax; + public float TextFieldMin { get; set; } + public float TextFieldMax { get; set; } + + public SoftRangeAttribute(float min, float max) + { + SliderMin = TextFieldMin = min; + SliderMax = TextFieldMax = max; + } + } +} \ No newline at end of file diff --git a/package/Unity.Physics.Authoring/PropertyAttributes/PropertyAttributes.cs.meta b/package/Unity.Physics.Authoring/PropertyAttributes/PropertyAttributes.cs.meta new file mode 100755 index 000000000..b6df10a6c --- /dev/null +++ b/package/Unity.Physics.Authoring/PropertyAttributes/PropertyAttributes.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: d0db1130e6fc44abea606b6aefc06d82 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Unity.Physics.Authoring/Unity.Physics.Authoring.asmdef b/package/Unity.Physics.Authoring/Unity.Physics.Authoring.asmdef new file mode 100755 index 000000000..04174a43a --- /dev/null +++ b/package/Unity.Physics.Authoring/Unity.Physics.Authoring.asmdef @@ -0,0 +1,20 @@ +{ + "name": "Unity.Physics.Authoring", + "references": [ + "Unity.Collections", + "Unity.Entities", + "Unity.Entities.Hybrid", + "Unity.Mathematics", + "Unity.Physics", + "Unity.Burst" + ], + "optionalUnityReferences": [], + "includePlatforms": [], + "excludePlatforms": [], + "allowUnsafeCode": true, + "overrideReferences": false, + "precompiledReferences": [], + "autoReferenced": true, + "defineConstraints": [], + "versionDefines": [] +} \ No newline at end of file diff --git a/package/Unity.Physics.Authoring/Unity.Physics.Authoring.asmdef.meta b/package/Unity.Physics.Authoring/Unity.Physics.Authoring.asmdef.meta new file mode 100755 index 000000000..cd21a6a53 --- /dev/null +++ b/package/Unity.Physics.Authoring/Unity.Physics.Authoring.asmdef.meta @@ -0,0 +1,7 @@ +fileFormatVersion: 2 +guid: e00140a815de944528348782854abe39 +AssemblyDefinitionImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Unity.Physics.Authoring/Utilities.meta b/package/Unity.Physics.Authoring/Utilities.meta new file mode 100755 index 000000000..815fe5f20 --- /dev/null +++ b/package/Unity.Physics.Authoring/Utilities.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: 97b3e5c1bb3194eb496a256796e4b428 +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Unity.Physics.Authoring/Utilities/DebugDisplay.meta b/package/Unity.Physics.Authoring/Utilities/DebugDisplay.meta new file mode 100755 index 000000000..8c0ae4647 --- /dev/null +++ b/package/Unity.Physics.Authoring/Utilities/DebugDisplay.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: 3b38fbd742f0e6b45bfcb2562c4c736a +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Unity.Physics.Authoring/Utilities/DebugDisplay/DebugStream.cs b/package/Unity.Physics.Authoring/Utilities/DebugDisplay/DebugStream.cs new file mode 100755 index 000000000..e29879c4a --- /dev/null +++ b/package/Unity.Physics.Authoring/Utilities/DebugDisplay/DebugStream.cs @@ -0,0 +1,396 @@ +using System.Collections.Generic; +using Unity.Collections; +using Unity.Entities; +using Unity.Mathematics; +using Unity.Physics; +using Unity.Physics.Systems; +using UnityEditor; +using UnityEngine; + +[UpdateBefore(typeof(BuildPhysicsWorld))] +public class DebugStream : ComponentSystem +{ + public struct Context + { + public void Begin(int index) + { + Writer.BeginForEachIndex(index); + } + + public void End() + { + Writer.EndForEachIndex(); + } + + public void Point(float3 x, float size, Color color) + { + Writer.Write(Type.Point); + Writer.Write(new Point { X = x, Size = size, Color = color }); + } + + public void Line(float3 x0, float3 x1, Color color) + { + Writer.Write(Type.Line); + Writer.Write(new Line { X0 = x0, X1 = x1, Color = color }); + } + + public void Arrow(float3 x, float3 v, Color color) + { + Writer.Write(Type.Arrow); + Writer.Write(new Line { X0 = x, X1 = x + v, Color = color }); + } + + public void Plane(float3 x, float3 v, Color color) + { + Writer.Write(Type.Plane); + Writer.Write(new Line { X0 = x, X1 = x + v, Color = color }); + } + + public void Circle(float3 x, float3 v, Color color) + { + Writer.Write(Type.Circle); + Writer.Write(new Line { X0 = x, X1 = x + v, Color = color }); + } + + public void Arc(float3 center, float3 normal, float3 arm, float angle, Color color) + { + Writer.Write(Type.Arc); + Writer.Write(new Arc { Center = center, Normal = normal, Arm = arm, Angle = angle, Color = color }); + } + + public void Cone(float3 point, float3 axis, float angle, Color color) + { + Writer.Write(Type.Cone); + Writer.Write(new Cone { Point = point, Axis = axis, Angle = angle, Color = color }); + } + + public void Box(float3 size, float3 center, quaternion orientation, Color color) + { + Writer.Write(Type.Box); + Writer.Write(new Box { Size = size, Center = center, Orientation = orientation, Color = color }); + } + + public void Text(char[] text, float3 x, Color color) + { + Writer.Write(Type.Text); + Writer.Write(new Text { X = x, Color = color, Length = text.Length }); + + foreach(char c in text) + { + Writer.Write(c); + } + } + + internal BlockStream.Writer Writer; + } + + public Context GetContext(int foreachCount) + { + var stream = new BlockStream(foreachCount, 0xcc1922b8); + + m_DebugStreams.Add(stream); + return new Context { Writer = stream }; + } + + public enum Type + { + Point, + Line, + Arrow, + Plane, + Circle, + Arc, + Cone, + Text, + Box + } + + public struct Point + { + public float3 X; + public float Size; + public Color Color; + + public void Draw() + { +#if UNITY_EDITOR + Handles.color = Color; + Handles.DrawLine(X - new float3(Size, 0, 0), X + new float3(Size, 0, 0)); + Handles.DrawLine(X - new float3(0, Size, 0), X + new float3(0, Size, 0)); + Handles.DrawLine(X - new float3(0, 0, Size), X + new float3(0, 0, Size)); +#endif + } + } + + public struct Line + { + public float3 X0; + public float3 X1; + public Color Color; + + public void Draw() + { +#if UNITY_EDITOR + Handles.color = Color; + Handles.DrawLine(X0, X1); +#endif + } + + public void DrawArrow() + { + if (!math.all(X0 == X1)) + { +#if UNITY_EDITOR + Handles.color = Color; + + Handles.DrawLine(X0, X1); + float3 v = X1 - X0; + float3 dir; + float length = Math.NormalizeWithLength(v, out dir); + float3 perp, perp2; + Math.CalculatePerpendicularNormalized(dir, out perp, out perp2); + float3 scale = length * 0.2f; + + Handles.DrawLine(X1, X1 + (perp - dir) * scale); + Handles.DrawLine(X1, X1 - (perp + dir) * scale); + Handles.DrawLine(X1, X1 + (perp2 - dir) * scale); + Handles.DrawLine(X1, X1 - (perp2 + dir) * scale); +#endif + } + } + + public void DrawPlane() + { + if (!math.all(X0 == X1)) + { +#if UNITY_EDITOR + Handles.color = Color; + + Handles.DrawLine(X0, X1); + float3 v = X1 - X0; + float3 dir; + float length = Math.NormalizeWithLength(v, out dir); + float3 perp, perp2; + Math.CalculatePerpendicularNormalized(dir, out perp, out perp2); + float3 scale = length * 0.2f; + + Handles.DrawLine(X1, X1 + (perp - dir) * scale); + Handles.DrawLine(X1, X1 - (perp + dir) * scale); + Handles.DrawLine(X1, X1 + (perp2 - dir) * scale); + Handles.DrawLine(X1, X1 - (perp2 + dir) * scale); + + perp *= length; + perp2 *= length; + Handles.DrawLine(X0 + perp + perp2, X0 + perp - perp2); + Handles.DrawLine(X0 + perp - perp2, X0 - perp - perp2); + Handles.DrawLine(X0 - perp - perp2, X0 - perp + perp2); + Handles.DrawLine(X0 - perp + perp2, X0 + perp + perp2); +#endif + } + } + + public void DrawCircle() + { + if (!math.all(X0 == X1)) + { +#if UNITY_EDITOR + Handles.color = Color; + + float3 v = X1 - X0; + float3 dir; + float length = Math.NormalizeWithLength(v, out dir); + float3 perp, perp2; + Math.CalculatePerpendicularNormalized(dir, out perp, out perp2); + float3 scale = length * 0.2f; + + const int res = 16; + quaternion q = quaternion.AxisAngle(dir, 2.0f * (float)math.PI / res); + float3 arm = perp * length; + for (int i = 0; i < res; i++) + { + float3 nextArm = math.mul(q, arm); + Handles.DrawLine(X0 + arm, X0 + nextArm); + arm = nextArm; + } +#endif + } + } + } + + public struct Arc + { + public float3 Center; + public float3 Normal; + public float3 Arm; + public float Angle; + public Color Color; + + public void Draw() + { +#if UNITY_EDITOR + Handles.color = Color; + + const int res = 16; + quaternion q = quaternion.AxisAngle(Normal, Angle / res); + float3 currentArm = Arm; + Handles.DrawLine(Center, Center + currentArm); + for (int i = 0; i < res; i++) + { + float3 nextArm = math.mul(q, currentArm); + Handles.DrawLine(Center + currentArm, Center + nextArm); + currentArm = nextArm; + } + Handles.DrawLine(Center, Center + currentArm); +#endif + } + } + + public struct Cone + { + public float3 Point; + public float3 Axis; + public float Angle; + public Color Color; + + public void Draw() + { +#if UNITY_EDITOR + Handles.color = Color; + + float3 dir; + float scale = Math.NormalizeWithLength(Axis, out dir); + + float3 arm; + { + float3 perp1, perp2; + Math.CalculatePerpendicularNormalized(dir, out perp1, out perp2); + arm = math.mul(quaternion.AxisAngle(perp1, Angle), dir) * scale; + } + + const int res = 16; + quaternion q = quaternion.AxisAngle(dir, 2.0f * (float)math.PI / res); + for (int i = 0; i < res; i++) + { + float3 nextArm = math.mul(q, arm); + Handles.DrawLine(Point, Point + arm); + Handles.DrawLine(Point + arm, Point + nextArm); + arm = nextArm; + } +#endif + } + } + + struct Box + { + public float3 Size; + public float3 Center; + public quaternion Orientation; + public Color Color; + + public void Draw() + { +#if UNITY_EDITOR + Matrix4x4 orig = Handles.matrix; + + Matrix4x4 mat = Matrix4x4.TRS(Center, Orientation, Vector3.one); + Handles.matrix = mat; + Handles.color = Color; + Handles.DrawWireCube(Vector3.zero, new Vector3(Size.x, Size.y, Size.z)); + + Handles.matrix = orig; +#endif + } + } + + struct Text + { + public float3 X; + public Color Color; + public int Length; + + public void Draw(ref BlockStream.Reader reader) + { + // Read string data. + char[] stringBuf = new char[Length]; + for (int i = 0; i < Length; i++) + { + stringBuf[i] = reader.Read(); + } + + GUIStyle style = new GUIStyle(); + style.normal.textColor = Color; +#if UNITY_EDITOR + Handles.Label(X, new string(stringBuf), style); +#endif + } + } + + private void Draw() + { + for (int i = 0; i < m_DebugStreams.Count; i++) + { + BlockStream.Reader reader = m_DebugStreams[i]; + for (int j = 0; j != reader.ForEachCount; j++) + { + reader.BeginForEachIndex(j); + while (reader.RemainingItemCount != 0) + { + switch (reader.Read()) + { + case Type.Point: reader.Read().Draw(); break; + case Type.Line: reader.Read().Draw(); break; + case Type.Arrow: reader.Read().DrawArrow(); break; + case Type.Plane: reader.Read().DrawPlane(); break; + case Type.Circle: reader.Read().DrawCircle(); break; + case Type.Arc: reader.Read().Draw(); break; + case Type.Cone: reader.Read().Draw(); break; + case Type.Text: reader.Read().Draw(ref reader); break; + case Type.Box: reader.Read().Draw(); break; + default: return; // unknown type + } + } + UnityEngine.Assertions.Assert.AreEqual(reader.RemainingItemCount, 0); + } + } + } + + private class DrawComponent : MonoBehaviour + { + public DebugStream DebugDraw; + public void OnDrawGizmos() + { + if (DebugDraw != null) + { + DebugDraw.Draw(); + } + } + } + + protected override void OnUpdate() + { + // Reset + for (int i = 0; i < m_DebugStreams.Count; i++) + { + m_DebugStreams[i].Dispose(); + } + m_DebugStreams.Clear(); + + // Set up component to draw + if (m_DrawComponent == null) + { + GameObject drawObject = new GameObject(); + m_DrawComponent = drawObject.AddComponent(); + m_DrawComponent.name = "DebugStream.DrawComponent"; + m_DrawComponent.DebugDraw = this; + } + } + + protected override void OnDestroyManager() + { + for (int i = 0; i < m_DebugStreams.Count; i++) + m_DebugStreams[i].Dispose(); + } + + private DrawComponent m_DrawComponent; + List m_DebugStreams = new List(); +} diff --git a/package/Unity.Physics.Authoring/Utilities/DebugDisplay/DebugStream.cs.meta b/package/Unity.Physics.Authoring/Utilities/DebugDisplay/DebugStream.cs.meta new file mode 100755 index 000000000..c38a50f54 --- /dev/null +++ b/package/Unity.Physics.Authoring/Utilities/DebugDisplay/DebugStream.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 5bbe9c6c1adee0b40bb4a382595b2994 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Unity.Physics.Authoring/Utilities/DebugDisplay/DisplayBroadphaseSystem.cs b/package/Unity.Physics.Authoring/Utilities/DebugDisplay/DisplayBroadphaseSystem.cs new file mode 100755 index 000000000..892525018 --- /dev/null +++ b/package/Unity.Physics.Authoring/Utilities/DebugDisplay/DisplayBroadphaseSystem.cs @@ -0,0 +1,96 @@ +using Unity.Physics; +using Unity.Physics.Systems; +using Unity.Collections; +using Unity.Entities; +using Unity.Jobs; +using Unity.Mathematics; +using UnityEngine; + +namespace Unity.Physics.Authoring +{ + /// Job which walks the broadphase tree and writes the + /// bounding box of leaf nodes to a DebugStream. + public struct DisplayBroadphaseJob : IJob // StaticNodes; + + [ReadOnly] + public NativeArray DynamicNodes; + + public void DrawLeavesRecursive(NativeArray nodes, Color color, int nodeIndex) + { + if (nodes[nodeIndex].IsLeaf) + { + bool4 leavesValid = nodes[nodeIndex].AreLeavesValid; + for (int l = 0; l < 4; l++) + { + if (leavesValid[l]) + { + Aabb aabb = nodes[nodeIndex].Bounds.GetAabb(l); + float3 center = aabb.Center; + OutputStream.Box(aabb.Extents, center, Quaternion.identity, color); + } + } + + return; + } + + for (int i = 0; i < 4; i++) + { + if (nodes[nodeIndex].IsChildValid(i)) + { + DrawLeavesRecursive(nodes, color, nodes[nodeIndex].Data[i]); + } + } + } + public void Execute() + { + OutputStream.Begin(0); + DrawLeavesRecursive(StaticNodes, Color.yellow, 1); + DrawLeavesRecursive(DynamicNodes, Color.red, 1); + OutputStream.End(); + } + } + + /// Creates DisplayBroadphaseJobs + [UpdateAfter(typeof(BuildPhysicsWorld)), UpdateBefore(typeof(EndFramePhysicsSystem))] + public class DisplayBroadphaseAabbsSystem : JobComponentSystem + { + BuildPhysicsWorld m_BuildPhysicsWorldSystem; + EndFramePhysicsSystem m_EndFramePhysicsSystem; + DebugStream m_DebugStreamSystem; + + protected override void OnCreateManager() + { + m_BuildPhysicsWorldSystem = World.GetOrCreateManager(); + m_EndFramePhysicsSystem = World.GetOrCreateManager(); + m_DebugStreamSystem = World.GetOrCreateManager(); + } + + protected override JobHandle OnUpdate(JobHandle inputDeps) + { + if (!(HasSingleton() && GetSingleton().DrawBroadphase != 0)) + { + return inputDeps; + } + + inputDeps = JobHandle.CombineDependencies(inputDeps, m_BuildPhysicsWorldSystem.FinalJobHandle); + + ref Broadphase broadphase = ref m_BuildPhysicsWorldSystem.PhysicsWorld.CollisionWorld.Broadphase; + + JobHandle handle = new DisplayBroadphaseJob + { + OutputStream = m_DebugStreamSystem.GetContext(1), + StaticNodes = broadphase.StaticTree.Nodes, + DynamicNodes = broadphase.DynamicTree.Nodes, + }.Schedule(inputDeps); + + m_EndFramePhysicsSystem.HandlesToWaitFor.Add(handle); + + return handle; + } + } +} diff --git a/package/Unity.Physics.Authoring/Utilities/DebugDisplay/DisplayBroadphaseSystem.cs.meta b/package/Unity.Physics.Authoring/Utilities/DebugDisplay/DisplayBroadphaseSystem.cs.meta new file mode 100755 index 000000000..f6e45b147 --- /dev/null +++ b/package/Unity.Physics.Authoring/Utilities/DebugDisplay/DisplayBroadphaseSystem.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 70e89a22f5bb2544a85f3d8aad4ceead +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Unity.Physics.Authoring/Utilities/DebugDisplay/DisplayColliderBoundingVolumeSystem.cs b/package/Unity.Physics.Authoring/Utilities/DebugDisplay/DisplayColliderBoundingVolumeSystem.cs new file mode 100755 index 000000000..92985fe41 --- /dev/null +++ b/package/Unity.Physics.Authoring/Utilities/DebugDisplay/DisplayColliderBoundingVolumeSystem.cs @@ -0,0 +1,76 @@ +using Unity.Physics; +using Unity.Physics.Systems; +using Unity.Collections; +using Unity.Entities; +using Unity.Jobs; +using Unity.Mathematics; +using UnityEngine; + +namespace Unity.Physics.Authoring +{ + /// Job to iterate over all the bodies in a scene, for any + /// which have a collider, calculate the bounding box and + /// write it to a debug stream. + public unsafe struct DisplayColliderAabbsJob : IJob // Bodies; + + public void Execute() + { + OutputStream.Begin(0); + + for (int b = 0; b < Bodies.Length; b++) + { + if (Bodies[b].Collider != null) + { + Aabb aabb = Bodies[b].Collider->CalculateAabb(Bodies[b].WorldFromBody); + + float3 center = aabb.Center; + OutputStream.Box(aabb.Extents, center, Quaternion.identity, new Color(0.7f, 0.125f, 0.125f)); + } + } + OutputStream.End(); + } + } + + /// Create a DisplayColliderAabbsJob + [UpdateAfter(typeof(BuildPhysicsWorld)), UpdateBefore(typeof(EndFramePhysicsSystem))] + public class DisplayColliderAabbsSystem : JobComponentSystem + { + BuildPhysicsWorld m_BuildPhysicsWorldSystem; + StepPhysicsWorld m_StepPhysicsWorld; + DebugStream m_DebugStreamSystem; + + protected override void OnCreateManager() + { + m_BuildPhysicsWorldSystem = World.GetOrCreateManager(); + m_StepPhysicsWorld = World.GetOrCreateManager(); + m_DebugStreamSystem = World.GetOrCreateManager(); + } + + protected override JobHandle OnUpdate(JobHandle inputDeps) + { + if (!(HasSingleton() && GetSingleton().DrawColliderAabbs != 0)) + { + return inputDeps; + } + + if (m_BuildPhysicsWorldSystem.PhysicsWorld.NumBodies == 0) + { + return inputDeps; + } + + SimulationCallbacks.Callback callback = (ref ISimulation simulation, JobHandle deps) => + { + return new DisplayColliderAabbsJob + { + OutputStream = m_DebugStreamSystem.GetContext(1), + Bodies = m_BuildPhysicsWorldSystem.PhysicsWorld.Bodies + }.Schedule(deps); + }; + m_StepPhysicsWorld.EnqueueCallback(SimulationCallbacks.Phase.PostCreateDispatchPairs, callback); + return inputDeps; + } + } +} diff --git a/package/Unity.Physics.Authoring/Utilities/DebugDisplay/DisplayColliderBoundingVolumeSystem.cs.meta b/package/Unity.Physics.Authoring/Utilities/DebugDisplay/DisplayColliderBoundingVolumeSystem.cs.meta new file mode 100755 index 000000000..55f60e448 --- /dev/null +++ b/package/Unity.Physics.Authoring/Utilities/DebugDisplay/DisplayColliderBoundingVolumeSystem.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 4af8c92c2fe59bb4a8e4345ac0eb2a8c +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Unity.Physics.Authoring/Utilities/DebugDisplay/DisplayCollidersSystem.cs b/package/Unity.Physics.Authoring/Utilities/DebugDisplay/DisplayCollidersSystem.cs new file mode 100755 index 000000000..13eb8291f --- /dev/null +++ b/package/Unity.Physics.Authoring/Utilities/DebugDisplay/DisplayCollidersSystem.cs @@ -0,0 +1,484 @@ +using System.Collections.Generic; +using Unity.Physics; +using Unity.Physics.Systems; +using Unity.Collections; +using Unity.Entities; +using Unity.Mathematics; +using UnityEngine; +using Collider = Unity.Physics.Collider; +using SphereCollider = Unity.Physics.SphereCollider; +using CapsuleCollider = Unity.Physics.CapsuleCollider; +using MeshCollider = Unity.Physics.MeshCollider; +using Mesh = Unity.Physics.Mesh; + +namespace Unity.Physics.Authoring +{ + /// A system to display debug geometry for all body colliders + [UpdateAfter(typeof(BuildPhysicsWorld))] + public class DisplayBodyColliders : ComponentSystem + { + BuildPhysicsWorld m_BuildPhysicsWorldSystem; + + // Bodies; + public int NumDynamicBodies; + public int Enabled; + public int EnableConvexConnectivity; + public int EnableMeshEdges; + + protected static UnityEngine.Mesh ReferenceSphere => GetReferenceMesh(ref CachedReferenceSphere, PrimitiveType.Sphere); + protected static UnityEngine.Mesh ReferenceCylinder => GetReferenceMesh(ref CachedReferenceCylinder, PrimitiveType.Cylinder); + + protected static UnityEngine.Mesh CachedReferenceSphere; + protected static UnityEngine.Mesh CachedReferenceCylinder; + + static UnityEngine.Mesh GetReferenceMesh(ref UnityEngine.Mesh cache, PrimitiveType type) + { + if (cache == null) + { + cache = CreateReferenceMesh(type); + } + return cache; + } + + static UnityEngine.Mesh CreateReferenceMesh(PrimitiveType type) + { + GameObject refGo = GameObject.CreatePrimitive(type); + UnityEngine.Mesh mesh = refGo.GetComponent().sharedMesh; + Destroy(refGo); + return mesh; + } + + // Combination mesh+scale, to enable sharing spheres + public class DisplayResult + { + public UnityEngine.Mesh Mesh; + public Vector3 Scale; + public Vector3 Position; + public Quaternion Orientation; + } + + private static void AppendConvex(ref ConvexHull hull, RigidTransform worldFromCollider, ref List results) + { + int totalNumVertices = 0; + for (int f = 0; f < hull.NumFaces; f++) + { + totalNumVertices += hull.Faces[f].NumVertices + 1; + } + + Vector3[] vertices = new Vector3[totalNumVertices]; + Vector3[] normals = new Vector3[totalNumVertices]; + int[] triangles = new int[(totalNumVertices - hull.NumFaces) * 3]; + + int startVertexIndex = 0; + int curTri = 0; + for (int f = 0; f < hull.NumFaces; f++) + { + Vector3 avgFace = Vector3.zero; + Vector3 faceNormal = hull.Planes[f].Normal; + + for (int fv = 0; fv < hull.Faces[f].NumVertices; fv++) + { + int origV = hull.FaceVertexIndices[hull.Faces[f].FirstIndex + fv]; + vertices[startVertexIndex + fv] = hull.Vertices[origV]; + normals[startVertexIndex + fv] = faceNormal; + + Vector3 v = hull.Vertices[origV]; + avgFace += v; + + triangles[curTri * 3 + 0] = startVertexIndex + fv; + triangles[curTri * 3 + 1] = startVertexIndex + (fv + 1) % hull.Faces[f].NumVertices; + triangles[curTri * 3 + 2] = startVertexIndex + hull.Faces[f].NumVertices; + curTri++; + } + avgFace *= 1.0f / hull.Faces[f].NumVertices; + vertices[startVertexIndex + hull.Faces[f].NumVertices] = avgFace; + normals[startVertexIndex + hull.Faces[f].NumVertices] = faceNormal; + + startVertexIndex += hull.Faces[f].NumVertices + 1; + } + + UnityEngine.Mesh mesh = new UnityEngine.Mesh(); + mesh.vertices = vertices; + mesh.normals = normals; + mesh.triangles = triangles; + + results.Add(new DisplayResult + { + Mesh = mesh, + Scale = Vector3.one, + Position = worldFromCollider.pos, + Orientation = worldFromCollider.rot + }); + } + + public static void AppendSphere(SphereCollider* sphere, RigidTransform worldFromCollider, ref List results) + { + float r = sphere->Radius; + results.Add(new DisplayResult + { + Mesh = ReferenceSphere, + Scale = new Vector4(r * 2.0f, r * 2.0f, r * 2.0f), + Position = math.transform(worldFromCollider, sphere->Center), + Orientation = worldFromCollider.rot, + }); + } + + public static void AppendCapsule(CapsuleCollider* capsule, RigidTransform worldFromCollider, ref List results) + { + float r = capsule->Radius; + results.Add(new DisplayResult + { + Mesh = ReferenceSphere, + Scale = new Vector4(r * 2.0f, r * 2.0f, r * 2.0f), + Position = math.transform(worldFromCollider, capsule->Vertex0), + Orientation = worldFromCollider.rot + }); + results.Add(new DisplayResult + { + Mesh = ReferenceSphere, + Scale = new Vector4(r * 2.0f, r * 2.0f, r * 2.0f), + Position = math.transform(worldFromCollider, capsule->Vertex1), + Orientation = worldFromCollider.rot + }); + results.Add(new DisplayResult + { + Mesh = ReferenceCylinder, + Scale = new Vector4(r * 2.0f, math.length(capsule->Vertex1 - capsule->Vertex0) * 0.5f, r * 2.0f), + Position = math.transform(worldFromCollider, (capsule->Vertex0 + capsule->Vertex1) * 0.5f), + Orientation = math.mul(worldFromCollider.rot, Quaternion.FromToRotation(new float3(0, 1, 0), math.normalizesafe(capsule->Vertex1 - capsule->Vertex0))) + }); + } + + public static void AppendMesh(MeshCollider* meshCollider, RigidTransform worldFromCollider, ref List results) + { + var vertices = new List(); + var normals = new List(); + var triangles = new List(); + int vertexIndex = 0; + + ref Mesh mesh = ref meshCollider->Mesh; + + for (int sectionIndex = 0; sectionIndex < mesh.Sections.Length; sectionIndex++) + { + ref Mesh.Section section = ref mesh.Sections[sectionIndex]; + for (int primitiveIndex = 0; primitiveIndex < section.PrimitiveVertexIndices.Length; primitiveIndex++) + { + Mesh.PrimitiveVertexIndices vertexIndices = section.PrimitiveVertexIndices[primitiveIndex]; + Mesh.PrimitiveFlags flags = section.PrimitiveFlags[primitiveIndex]; + int numTriangles = flags.HasFlag(Mesh.PrimitiveFlags.IsTrianglePair) ? 2 : 1; + + float3x4 v = new float3x4( + section.Vertices[vertexIndices.A], + section.Vertices[vertexIndices.B], + section.Vertices[vertexIndices.C], + section.Vertices[vertexIndices.D]); + + for (int triangleIndex = 0; triangleIndex < numTriangles; triangleIndex++) + { + float3 a = v[0]; + float3 b = v[1 + triangleIndex]; + float3 c = v[2 + triangleIndex]; + vertices.Add(a); + vertices.Add(b); + vertices.Add(c); + + triangles.Add(vertexIndex++); + triangles.Add(vertexIndex++); + triangles.Add(vertexIndex++); + + float3 n = math.normalize(math.cross((b - a), (c - a))); + normals.Add(n); + normals.Add(n); + normals.Add(n); + } + } + } + + var displayMesh = new UnityEngine.Mesh(); + displayMesh.vertices = vertices.ToArray(); + displayMesh.normals = normals.ToArray(); + displayMesh.triangles = triangles.ToArray(); + + results.Add(new DisplayResult + { + Mesh = displayMesh, + Scale = Vector3.one, + Position = worldFromCollider.pos, + Orientation = worldFromCollider.rot + }); + } + + public static void AppendCompound(CompoundCollider* compoundCollider, RigidTransform worldFromCollider, ref List results) + { + for (int i = 0; i < compoundCollider->Children.Length; i++) + { + ref CompoundCollider.Child child = ref compoundCollider->Children[i]; + RigidTransform worldFromChild = math.mul(worldFromCollider, child.CompoundFromChild); + AppendCollider(child.Collider, worldFromChild, ref results); + } + } + + public static void AppendCollider(Collider* collider, RigidTransform worldFromCollider, ref List results) + { + switch (collider->Type) + { + case ColliderType.Box: + case ColliderType.Triangle: + case ColliderType.Quad: + case ColliderType.Convex: + AppendConvex(ref ((ConvexCollider*)collider)->ConvexHull, worldFromCollider, ref results); + break; + case ColliderType.Sphere: + AppendSphere((SphereCollider*)collider, worldFromCollider, ref results); + break; + case ColliderType.Capsule: + AppendCapsule((CapsuleCollider*)collider, worldFromCollider, ref results); + break; + case ColliderType.Mesh: + AppendMesh((MeshCollider*)collider, worldFromCollider, ref results); + break; + case ColliderType.Compound: + AppendCompound((CompoundCollider*)collider, worldFromCollider, ref results); + break; + } + } + + public static List BuildDebugDisplayMesh(Collider* collider) + { + List results = new List(); + AppendCollider(collider, RigidTransform.identity, ref results); + return results; + } + + public void DrawConnectivity(RigidBody body) + { + if (body.Collider->CollisionType != CollisionType.Convex) + { + return; + } + + Matrix4x4 originalMatrix = Gizmos.matrix; + Gizmos.matrix = math.float4x4(body.WorldFromBody); + + ref ConvexHull hull = ref ((ConvexCollider*)body.Collider)->ConvexHull; + + void GetEdge(ref ConvexHull hullIn, ConvexHull.Face faceIn, int edgeIndex, out float3 from, out float3 to) + { + byte fromIndex = hullIn.FaceVertexIndices[faceIn.FirstIndex + edgeIndex]; + byte toIndex = hullIn.FaceVertexIndices[faceIn.FirstIndex + (edgeIndex + 1) % faceIn.NumVertices]; + from = hullIn.Vertices[fromIndex]; + to = hullIn.Vertices[toIndex]; + } + + if (hull.VertexEdges.Length > 0) + { + Gizmos.color = new Color(1.0f, 0.0f, 0.0f); + foreach (ConvexHull.Edge vertexEdge in hull.VertexEdges) + { + ConvexHull.Face face = hull.Faces[vertexEdge.FaceIndex]; + GetEdge(ref hull, face, vertexEdge.EdgeIndex, out float3 from, out float3 to); + float3 direction = (to - from) * 0.25f; + + Gizmos.DrawSphere(from, 0.01f); + Gizmos.DrawRay(from, direction); + } + } + + if (hull.FaceLinks.Length > 0) + { + Gizmos.color = new Color(0.0f, 1.0f, 0.0f); + foreach (ConvexHull.Face face in hull.Faces) + { + for (int edgeIndex = 0; edgeIndex < face.NumVertices; edgeIndex++) + { + ConvexHull.Edge linkedEdge = hull.FaceLinks[face.FirstIndex + edgeIndex]; + ConvexHull.Face linkedFace = hull.Faces[linkedEdge.FaceIndex]; + + GetEdge(ref hull, face, edgeIndex, out float3 from, out float3 to); + GetEdge(ref hull, linkedFace, linkedEdge.EdgeIndex, out float3 linkedFrom, out float3 linkedTo); + + Gizmos.DrawLine(math.lerp(from, to, 0.333f), math.lerp(from, to, 0.666f)); + Gizmos.DrawLine(math.lerp(linkedFrom, linkedTo, 0.333f), math.lerp(linkedFrom, linkedTo, 0.666f)); + Gizmos.DrawLine(math.lerp(from, to, 0.5f), math.lerp(linkedFrom, linkedTo, 0.5f)); + } + } + } + + Gizmos.matrix = originalMatrix; + } + + private struct Edge + { + internal Vector3 A; + internal Vector3 B; + } + + public void DrawMeshEdges(RigidBody body) + { + if (body.Collider->Type != ColliderType.Mesh) + { + return; + } + + Matrix4x4 originalMatrix = Gizmos.matrix; + Gizmos.matrix = math.float4x4(body.WorldFromBody); + + MeshCollider* meshCollider = (MeshCollider*)body.Collider; + ref Mesh mesh = ref meshCollider->Mesh; + + var triangleEdges = new List(); + var trianglePairEdges = new List(); + var quadEdges = new List(); + + for (int sectionIndex = 0; sectionIndex < mesh.Sections.Length; sectionIndex++) + { + ref Mesh.Section section = ref mesh.Sections[sectionIndex]; + for (int primitiveIndex = 0; primitiveIndex < section.PrimitiveVertexIndices.Length; primitiveIndex++) + { + Mesh.PrimitiveVertexIndices vertexIndices = section.PrimitiveVertexIndices[primitiveIndex]; + Mesh.PrimitiveFlags flags = section.PrimitiveFlags[primitiveIndex]; + bool isTrianglePair = flags.HasFlag(Mesh.PrimitiveFlags.IsTrianglePair); + bool isQuad = flags.HasFlag(Mesh.PrimitiveFlags.IsQuad); + + float3x4 v = new float3x4( + section.Vertices[vertexIndices.A], + section.Vertices[vertexIndices.B], + section.Vertices[vertexIndices.C], + section.Vertices[vertexIndices.D]); + + if (isQuad) + { + quadEdges.Add(new Edge { A = v[0], B = v[1] }); + quadEdges.Add(new Edge { A = v[1], B = v[2] }); + quadEdges.Add(new Edge { A = v[2], B = v[3] }); + quadEdges.Add(new Edge { A = v[3], B = v[0] }); + } + else if (isTrianglePair) + { + trianglePairEdges.Add(new Edge { A = v[0], B = v[1] }); + trianglePairEdges.Add(new Edge { A = v[1], B = v[2] }); + trianglePairEdges.Add(new Edge { A = v[2], B = v[3] }); + trianglePairEdges.Add(new Edge { A = v[3], B = v[0] }); + } + else + { + triangleEdges.Add(new Edge { A = v[0], B = v[1] }); + triangleEdges.Add(new Edge { A = v[1], B = v[2] }); + triangleEdges.Add(new Edge { A = v[2], B = v[0] }); + } + } + } + + Gizmos.color = Color.black; + foreach (Edge edge in triangleEdges) + { + Gizmos.DrawLine(edge.A, edge.B); + } + + Gizmos.color = Color.blue; + foreach (Edge edge in trianglePairEdges) + { + Gizmos.DrawLine(edge.A, edge.B); + } + + Gizmos.color = Color.cyan; + foreach (Edge edge in quadEdges) + { + Gizmos.DrawLine(edge.A, edge.B); + } + + Gizmos.matrix = originalMatrix; + } + + public void OnDrawGizmos() + { + if (Enabled == 0) + { + return; + } + + for (int b = 0; b < Bodies.Length; b++) + { + if (Bodies[b].Collider == null) + { + continue; + } + + // Draw collider + { + List displayResults = BuildDebugDisplayMesh(Bodies[b].Collider); + if (displayResults.Count == 0) + { + continue; + } + + if (b < NumDynamicBodies) + { + Gizmos.color = new Color(1.0f, 0.7f, 0.0f); + } + else + { + Gizmos.color = new Color(0.7f, 0.7f, 0.7f); + } + + foreach (DisplayResult dr in displayResults) + { + Vector3 position = math.transform(Bodies[b].WorldFromBody, dr.Position); + Quaternion orientation = math.mul(Bodies[b].WorldFromBody.rot, dr.Orientation); + Gizmos.DrawWireMesh(dr.Mesh, position, orientation, dr.Scale); + } + } + + // Draw connectivity + if (EnableConvexConnectivity != 0) + { + DrawConnectivity(Bodies[b]); + } + + if (EnableMeshEdges != 0) + { + DrawMeshEdges(Bodies[b]); + } + } + } + } + + private DrawComponent m_DrawComponent; + + protected override void OnCreateManager() + { + m_BuildPhysicsWorldSystem = World.GetOrCreateManager(); + } + + protected override void OnUpdate() + { + if (!HasSingleton()) + { + return; + } + + int drawColliders = GetSingleton().DrawColliders; + int drawMeshEdges = GetSingleton().DrawMeshEdges; + + if (m_DrawComponent == null) + { + /// Need to make a GO and attach our DrawComponent MonoBehaviour + /// so that the rendering can happen on the main thread. + GameObject drawObject = new GameObject(); + m_DrawComponent = drawObject.AddComponent(); + drawObject.name = "DebugColliderDisplay"; + } + + m_DrawComponent.Bodies = m_BuildPhysicsWorldSystem.PhysicsWorld.Bodies; + m_DrawComponent.NumDynamicBodies = m_BuildPhysicsWorldSystem.PhysicsWorld.NumDynamicBodies; + m_DrawComponent.Enabled = drawColliders; + m_DrawComponent.EnableMeshEdges = drawColliders * drawMeshEdges; + } + } +} diff --git a/package/Unity.Physics.Authoring/Utilities/DebugDisplay/DisplayCollidersSystem.cs.meta b/package/Unity.Physics.Authoring/Utilities/DebugDisplay/DisplayCollidersSystem.cs.meta new file mode 100755 index 000000000..681ae5348 --- /dev/null +++ b/package/Unity.Physics.Authoring/Utilities/DebugDisplay/DisplayCollidersSystem.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: c4a80566cda492d448fef3998e5422d5 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Unity.Physics.Authoring/Utilities/DebugDisplay/DisplayCollisionEventsSystem.cs b/package/Unity.Physics.Authoring/Utilities/DebugDisplay/DisplayCollisionEventsSystem.cs new file mode 100755 index 000000000..fcf178304 --- /dev/null +++ b/package/Unity.Physics.Authoring/Utilities/DebugDisplay/DisplayCollisionEventsSystem.cs @@ -0,0 +1,97 @@ +using Unity.Physics; +using Unity.Physics.Systems; +using Unity.Collections; +using Unity.Entities; +using Unity.Jobs; +using Unity.Mathematics; +using UnityEngine; +using Collider = Unity.Physics.Collider; + +namespace Unity.Physics.Authoring +{ + // A systems which draws any collision events produced by the physics step system + [UpdateAfter(typeof(StepPhysicsWorld)), UpdateBefore(typeof(EndFramePhysicsSystem))] + public class DisplayCollisionEventsSystem : JobComponentSystem + { + BuildPhysicsWorld m_BuildPhysicsWorldSystem; + StepPhysicsWorld m_StepPhysicsWorldSystem; + EndFramePhysicsSystem m_EndFramePhysicsSystem; + DebugStream m_DebugStreamSystem; + + protected override void OnCreateManager() + { + m_BuildPhysicsWorldSystem = World.GetOrCreateManager(); + m_StepPhysicsWorldSystem = World.GetOrCreateManager(); + m_EndFramePhysicsSystem = World.GetOrCreateManager(); + m_DebugStreamSystem = World.GetOrCreateManager(); + } + + protected override JobHandle OnUpdate(JobHandle inputDeps) + { + if (!(HasSingleton() && GetSingleton().DrawCollisionEvents != 0)) + { + return inputDeps; + } + + inputDeps = JobHandle.CombineDependencies(inputDeps, m_BuildPhysicsWorldSystem.FinalJobHandle, m_StepPhysicsWorldSystem.FinalSimulationJobHandle); + + JobHandle handle = new DisplayCollisionEventsJob + { + World = m_BuildPhysicsWorldSystem.PhysicsWorld, + CollisionEvents = m_StepPhysicsWorldSystem.Simulation.CollisionEvents, + OutputStream = m_DebugStreamSystem.GetContext(1) + }.Schedule(1, 1, inputDeps); + + m_EndFramePhysicsSystem.HandlesToWaitFor.Add(handle); + + return handle; + } + + // Job which iterates over collision events and writes display info to a DebugStream. + //[BurstCompile] + struct DisplayCollisionEventsJob : IJobParallelFor + { + [ReadOnly] public PhysicsWorld World; + [ReadOnly] public CollisionEvents CollisionEvents; + public DebugStream.Context OutputStream; + + public unsafe void Execute(int workItemIndex) + { + OutputStream.Begin(workItemIndex); + foreach (CollisionEvent collisionEvent in CollisionEvents) + { + float3 offset = new float3(0, 1, 0); + + RigidBody bodyA = World.Bodies[collisionEvent.BodyIndices.BodyAIndex]; + RigidBody bodyB = World.Bodies[collisionEvent.BodyIndices.BodyBIndex]; + float totalImpulse = math.csum(collisionEvent.AccumulatedImpulses); + + bool AreCollisionEventsEnabled(Collider* collider, ColliderKey key) + { + if (collider->CollisionType == CollisionType.Convex) + { + return ((ConvexColliderHeader*)collider)->Material.EnableCollisionEvents; + } + else + { + collider->GetLeaf(key, out ChildCollider child); + collider = child.Collider; + UnityEngine.Assertions.Assert.IsTrue(collider->CollisionType == CollisionType.Convex); + return ((ConvexColliderHeader*)collider)->Material.EnableCollisionEvents; + } + } + + if (AreCollisionEventsEnabled(bodyA.Collider, collisionEvent.ColliderKeys.ColliderKeyA)) + { + OutputStream.Text(totalImpulse.ToString().ToCharArray(), bodyA.WorldFromBody.pos + offset, Color.blue); + } + if (AreCollisionEventsEnabled(bodyB.Collider, collisionEvent.ColliderKeys.ColliderKeyB)) + { + OutputStream.Text(totalImpulse.ToString().ToCharArray(), bodyB.WorldFromBody.pos + offset, Color.blue); + } + } + OutputStream.End(); + } + } + } +} diff --git a/package/Unity.Physics.Authoring/Utilities/DebugDisplay/DisplayCollisionEventsSystem.cs.meta b/package/Unity.Physics.Authoring/Utilities/DebugDisplay/DisplayCollisionEventsSystem.cs.meta new file mode 100755 index 000000000..24786dc31 --- /dev/null +++ b/package/Unity.Physics.Authoring/Utilities/DebugDisplay/DisplayCollisionEventsSystem.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: deb7cde93d0c3c241a183811d83b89ac +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Unity.Physics.Authoring/Utilities/DebugDisplay/DisplayContactsSystem.cs b/package/Unity.Physics.Authoring/Utilities/DebugDisplay/DisplayContactsSystem.cs new file mode 100755 index 000000000..3202a245a --- /dev/null +++ b/package/Unity.Physics.Authoring/Utilities/DebugDisplay/DisplayContactsSystem.cs @@ -0,0 +1,82 @@ +using Unity.Physics; +using Unity.Physics.Systems; +using Unity.Burst; +using Unity.Entities; +using Unity.Jobs; +using Unity.Mathematics; +using UnityEngine; + +namespace Unity.Physics.Authoring +{ + // Job which iterates over contacts from narrowphase and writes display info to a DebugStream. + [BurstCompile] + public struct DisplayContactsJob : IJobParallelFor + { + public SimulationData.Contacts.Iterator ManifoldIterator; + public DebugStream.Context OutputStream; + + public void Execute(int workItemIndex) + { + OutputStream.Begin(workItemIndex); + + while (ManifoldIterator.HasItemsLeft()) + { + ContactHeader contactHeader = ManifoldIterator.GetNextContactHeader(); + for (int c = 0; c < contactHeader.NumContacts; c++) + { + Unity.Physics.ContactPoint contact = ManifoldIterator.GetNextContact(); + float3 x0 = contact.Position; + float3 x1 = contactHeader.Normal * contact.Distance; + Color color = Color.green; + OutputStream.Arrow(x0, x1, color); + } + } + + OutputStream.End(); + } + } + + // Create DisplayContactsJob + [UpdateBefore(typeof(StepPhysicsWorld))] + public class DisplayContactsSystem : JobComponentSystem + { + StepPhysicsWorld m_StepWorld; + DebugStream m_DebugStreamSystem; + + protected override void OnCreateManager() + { + m_StepWorld = World.GetOrCreateManager(); + m_DebugStreamSystem = World.GetOrCreateManager(); + } + + protected override JobHandle OnUpdate(JobHandle inputDeps) + { + if (!(HasSingleton() && GetSingleton().DrawContacts != 0)) + { + return inputDeps; + } + + SimulationCallbacks.Callback callback = (ref ISimulation simulation, JobHandle inDeps) => + { + inDeps.Complete(); // 0) + { + return new DisplayContactsJob + { + ManifoldIterator = contacts.GetIterator(), + OutputStream = m_DebugStreamSystem.GetContext(numWorkItems) + }.Schedule(numWorkItems, 1, inDeps); + } + return inDeps; + }; + + m_StepWorld.EnqueueCallback(SimulationCallbacks.Phase.PostCreateContacts, callback); + + + return inputDeps; + } + } +} diff --git a/package/Unity.Physics.Authoring/Utilities/DebugDisplay/DisplayContactsSystem.cs.meta b/package/Unity.Physics.Authoring/Utilities/DebugDisplay/DisplayContactsSystem.cs.meta new file mode 100755 index 000000000..4ab638d72 --- /dev/null +++ b/package/Unity.Physics.Authoring/Utilities/DebugDisplay/DisplayContactsSystem.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: da12b23e95dd67a4c9b9f475c52713c5 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Unity.Physics.Authoring/Utilities/DebugDisplay/DisplayJointsSystem.cs b/package/Unity.Physics.Authoring/Utilities/DebugDisplay/DisplayJointsSystem.cs new file mode 100755 index 000000000..c1e229a1c --- /dev/null +++ b/package/Unity.Physics.Authoring/Utilities/DebugDisplay/DisplayJointsSystem.cs @@ -0,0 +1,225 @@ +using System; +using Unity.Physics; +using Unity.Physics.Systems; +using Unity.Collections; +using Unity.Entities; +using Unity.Jobs; +using Unity.Mathematics; +using UnityEngine; +using static Unity.Physics.Math; +using Joint = Unity.Physics.Joint; + +namespace Unity.Physics.Authoring +{ + /// Job which draws every joint + public struct DisplayJointsJob : IJob + { + private const float k_Scale = 0.5f; + + public DebugStream.Context OutputStream; + [ReadOnly] public NativeSlice Bodies; + [ReadOnly] public NativeSlice Joints; + + public unsafe void Execute() + { + // Color palette + Color colorA = Color.cyan; + Color colorB = Color.magenta; + Color colorError = Color.red; + Color colorRange = Color.yellow; + + OutputStream.Begin(0); + + for (int iJoint = 0; iJoint < Joints.Length; iJoint++) + { + Joint joint = Joints[iJoint]; + JointData* jointData = joint.JointData; + + RigidBody bodyA = Bodies[joint.BodyPair.BodyAIndex]; + RigidBody bodyB = Bodies[joint.BodyPair.BodyBIndex]; + + MTransform worldFromA, worldFromB; + MTransform worldFromJointA, worldFromJointB; + { + worldFromA = new MTransform(bodyA.WorldFromBody); + worldFromB = new MTransform(bodyB.WorldFromBody); + + worldFromJointA = Mul(worldFromA, jointData->AFromJoint); + worldFromJointB = Mul(worldFromB, jointData->BFromJoint); + } + + float3 pivotA = worldFromJointA.Translation; + float3 pivotB = worldFromJointB.Translation; + + for (int iConstraint = 0; iConstraint < jointData->NumConstraints; iConstraint++) + { + Constraint constraint = jointData->Constraints[iConstraint]; + + switch (constraint.Type) + { + case ConstraintType.Linear: + + float3 diff = pivotA - pivotB; + + // Draw the feature on B and find the range for A + float3 rangeOrigin; + float3 rangeDirection; + float rangeDistance; + switch (constraint.Dimension) + { + case 1: + float3 normal = worldFromJointB.Rotation[constraint.ConstrainedAxis1D]; + OutputStream.Plane(pivotB, normal * k_Scale, colorB); + rangeDistance = math.dot(normal, diff); + rangeOrigin = pivotA - normal * rangeDistance; + rangeDirection = normal; + break; + case 2: + float3 direction = worldFromJointB.Rotation[constraint.FreeAxis2D]; + OutputStream.Line(pivotB - direction * k_Scale, pivotB + direction * k_Scale, colorB); + float dot = math.dot(direction, diff); + rangeOrigin = pivotB + direction * dot; + rangeDirection = diff - direction * dot; + rangeDistance = math.length(rangeDirection); + rangeDirection = math.select(rangeDirection / rangeDistance, float3.zero, rangeDistance < 1e-5); + break; + case 3: + OutputStream.Point(pivotB, k_Scale, colorB); + rangeOrigin = pivotB; + rangeDistance = math.length(diff); + rangeDirection = math.select(diff / rangeDistance, float3.zero, rangeDistance < 1e-5); + break; + default: + throw new NotImplementedException(); + } + + // Draw the pivot on A + OutputStream.Point(pivotA, k_Scale, colorA); + + // Draw error + float3 rangeA = rangeOrigin + rangeDistance * rangeDirection; + float3 rangeMin = rangeOrigin + constraint.Min * rangeDirection; + float3 rangeMax = rangeOrigin + constraint.Max * rangeDirection; + if (rangeDistance < constraint.Min) + { + OutputStream.Line(rangeA, rangeMin, colorError); + } + else if (rangeDistance > constraint.Max) + { + OutputStream.Line(rangeA, rangeMax, colorError); + } + if (math.length(rangeA - pivotA) > 1e-5f) + { + OutputStream.Line(rangeA, pivotA, colorError); + } + + // Draw the range + if (constraint.Min != constraint.Max) + { + OutputStream.Line(rangeMin, rangeMax, colorRange); + } + + break; + case ConstraintType.Angular: + switch (constraint.Dimension) + { + case 1: + // Get the limited axis and perpendicular in joint space + int constrainedAxis = constraint.ConstrainedAxis1D; + float3 axisInWorld = worldFromJointA.Rotation[constrainedAxis]; + float3 perpendicularInWorld = worldFromJointA.Rotation[(constrainedAxis + 1) % 3] * k_Scale; + + // Draw the angle of A + OutputStream.Line(pivotA, pivotA + perpendicularInWorld, colorA); + + // Calculate the relative angle + float angle; + { + float3x3 jointBFromA = math.mul(math.inverse(worldFromJointB.Rotation), worldFromJointA.Rotation); + angle = CalculateTwistAngle(new quaternion(jointBFromA), constrainedAxis); + } + + // Draw the range in B + float3 axis = worldFromJointA.Rotation[constraint.ConstrainedAxis1D]; + OutputStream.Arc(pivotB, axis, math.mul(quaternion.AxisAngle(axis, constraint.Min - angle), perpendicularInWorld), constraint.Max - constraint.Min, colorB); + + break; + case 2: + // Get axes in world space + int axisIndex = constraint.FreeAxis2D; + float3 axisA = worldFromJointA.Rotation[axisIndex]; + float3 axisB = worldFromJointB.Rotation[axisIndex]; + + // Draw the cones in B + if (constraint.Min == 0.0f) + { + OutputStream.Line(pivotB, pivotB + axisB * k_Scale, colorB); + } + else + { + OutputStream.Cone(pivotB, axisB * k_Scale, constraint.Min, colorB); + } + if (constraint.Max != constraint.Min) + { + OutputStream.Cone(pivotB, axisB * k_Scale, constraint.Max, colorB); + } + + // Draw the axis in A + OutputStream.Arrow(pivotA, axisA * k_Scale, colorA); + + break; + case 3: + // TODO - no idea how to visualize this if the limits are nonzero :) + break; + default: + throw new NotImplementedException(); + } + break; + default: + throw new NotImplementedException(); + } + } + } + + OutputStream.End(); + } + } + + /// Creates DisplayJointsJobs + [UpdateAfter(typeof(BuildPhysicsWorld)), UpdateBefore(typeof(StepPhysicsWorld))] + public class DisplayJointsSystem : JobComponentSystem + { + + BuildPhysicsWorld m_BuildPhysicsWorldSystem; + EndFramePhysicsSystem m_EndFramePhysicsSystem; + DebugStream m_DebugStreamSystem; + + protected override void OnCreateManager() + { + m_BuildPhysicsWorldSystem = World.GetOrCreateManager(); + m_EndFramePhysicsSystem = World.GetOrCreateManager(); + m_DebugStreamSystem = World.GetOrCreateManager(); + } + + protected override JobHandle OnUpdate(JobHandle inputDeps) + { + if (!(HasSingleton() && GetSingleton().DrawJoints != 0)) + { + return inputDeps; + } + + inputDeps = JobHandle.CombineDependencies(inputDeps, m_BuildPhysicsWorldSystem.FinalJobHandle); + + JobHandle handle = new DisplayJointsJob + { + OutputStream = m_DebugStreamSystem.GetContext(1), + Bodies = m_BuildPhysicsWorldSystem.PhysicsWorld.Bodies, + Joints = m_BuildPhysicsWorldSystem.PhysicsWorld.Joints + }.Schedule(inputDeps); + + m_EndFramePhysicsSystem.HandlesToWaitFor.Add(handle); + + return handle; + } + } +} diff --git a/package/Unity.Physics.Authoring/Utilities/DebugDisplay/DisplayJointsSystem.cs.meta b/package/Unity.Physics.Authoring/Utilities/DebugDisplay/DisplayJointsSystem.cs.meta new file mode 100755 index 000000000..d4e8a4ed1 --- /dev/null +++ b/package/Unity.Physics.Authoring/Utilities/DebugDisplay/DisplayJointsSystem.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 1e5519c0d7e56104f97392ab7aec2f17 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Unity.Physics.Authoring/Utilities/DebugDisplay/DisplayMassPropertiesSystem.cs b/package/Unity.Physics.Authoring/Utilities/DebugDisplay/DisplayMassPropertiesSystem.cs new file mode 100755 index 000000000..fc62dcacf --- /dev/null +++ b/package/Unity.Physics.Authoring/Utilities/DebugDisplay/DisplayMassPropertiesSystem.cs @@ -0,0 +1,95 @@ +using Unity.Physics; +using Unity.Physics.Systems; +using Unity.Collections; +using Unity.Entities; +using Unity.Jobs; +using Unity.Mathematics; +using UnityEngine; + +namespace Unity.Physics.Authoring +{ + /// Create and dispatch a DisplayMassPropertiesJob + [UpdateAfter(typeof(StepPhysicsWorld)), UpdateBefore(typeof(EndFramePhysicsSystem))] + public class DisplayMassPropertiesSystem : JobComponentSystem + { + BuildPhysicsWorld m_BuildPhysicsWorldSystem; + StepPhysicsWorld m_StepPhysicsWorld; + EndFramePhysicsSystem m_EndFramePhysicsSystem; + DebugStream m_DebugStreamSystem; + + protected override void OnCreateManager() + { + m_BuildPhysicsWorldSystem = World.GetOrCreateManager(); + m_StepPhysicsWorld = World.GetOrCreateManager(); + m_EndFramePhysicsSystem = World.GetOrCreateManager(); + m_DebugStreamSystem = World.GetOrCreateManager(); + } + + protected override JobHandle OnUpdate(JobHandle inputDeps) + { + if (!(HasSingleton() && GetSingleton().DrawMassProperties != 0)) + { + return inputDeps; + } + + inputDeps = JobHandle.CombineDependencies(inputDeps, m_StepPhysicsWorld.FinalSimulationJobHandle); + + JobHandle handle = new DisplayMassPropertiesJob + { + OutputStream = m_DebugStreamSystem.GetContext(1), + MotionDatas = m_BuildPhysicsWorldSystem.PhysicsWorld.MotionDatas, + MotionVelocities = m_BuildPhysicsWorldSystem.PhysicsWorld.MotionVelocities + }.Schedule(inputDeps); + + m_EndFramePhysicsSystem.HandlesToWaitFor.Add(handle); + return handle; + } + + // Job to write mass properties info to a DebugStream for any moving bodies + // Attempts to build a box which has the same inertia tensor as the body. + struct DisplayMassPropertiesJob : IJob // MotionDatas; + [ReadOnly] public NativeSlice MotionVelocities; + + public void Execute() + { + OutputStream.Begin(0); + for (int m = 0; m < MotionDatas.Length; m++) + { + float3 com = MotionDatas[m].WorldFromMotion.pos; + quaternion o = MotionDatas[m].WorldFromMotion.rot; + + float3 invInertiaLocal = MotionVelocities[m].InverseInertiaAndMass.xyz; + float3 il = new float3(1.0f / invInertiaLocal.x, 1.0f / invInertiaLocal.y, 1.0f / invInertiaLocal.z); + float invMass = MotionVelocities[m].InverseInertiaAndMass.w; + + // Reverse the inertia tensor computation to build a box which has the inerta tensor 'il' + // The diagonal inertia of a box with dimensions h,w,d and mass m is: + // Ix = 1/12 m (ww + dd) + // Iy = 1/12 m (dd + hh) + // Iz = 1/12 m (ww + hh) + // + // For simplicity, set K = I * 12 / m + // Then K = (ww + dd, dd + hh, ww + hh) + // => ww = Kx - dd, dd = Ky - hh, hh = Kz - ww + // By manipulation: + // 2ww = Kx - Ky + Kz + // => w = ((0.5)(Kx - Ky + Kz))^-1 + // Then, substitution gives h and d. + + float3 k = new float3(il.x * 12 * invMass, il.y * 12 * invMass, il.z * 12 * invMass); + float w = math.sqrt((k.x - k.y + k.z) * 0.5f); + float h = math.sqrt(k.z - w * w); + float d = math.sqrt(k.y - h * h); + + float3 boxSize = new float3(h, w, d); + OutputStream.Box(boxSize, com, o, Color.magenta); + } + OutputStream.End(); + } + } + } +} diff --git a/package/Unity.Physics.Authoring/Utilities/DebugDisplay/DisplayMassPropertiesSystem.cs.meta b/package/Unity.Physics.Authoring/Utilities/DebugDisplay/DisplayMassPropertiesSystem.cs.meta new file mode 100755 index 000000000..1508da1ea --- /dev/null +++ b/package/Unity.Physics.Authoring/Utilities/DebugDisplay/DisplayMassPropertiesSystem.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 5d8c8225a9bb32748b1f69645f6284c5 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Unity.Physics.Authoring/Utilities/DebugDisplay/DisplayTriggerEventsSystem.cs b/package/Unity.Physics.Authoring/Utilities/DebugDisplay/DisplayTriggerEventsSystem.cs new file mode 100755 index 000000000..405558fa9 --- /dev/null +++ b/package/Unity.Physics.Authoring/Utilities/DebugDisplay/DisplayTriggerEventsSystem.cs @@ -0,0 +1,94 @@ +using Unity.Physics; +using Unity.Physics.Systems; +using Unity.Collections; +using Unity.Entities; +using Unity.Jobs; +using UnityEngine; +using Collider = Unity.Physics.Collider; + +namespace Unity.Physics.Authoring +{ + // A systems which draws any trigger events produced by the physics step system + [UpdateAfter(typeof(StepPhysicsWorld)), UpdateBefore(typeof(EndFramePhysicsSystem))] + public class DisplayTriggerEventsSystem : JobComponentSystem + { + BuildPhysicsWorld m_BuildPhysicsWorldSystem; + StepPhysicsWorld m_StepPhysicsWorldSystem; + EndFramePhysicsSystem m_EndFramePhysicsSystem; + DebugStream m_DebugStreamSystem; + + protected override void OnCreateManager() + { + m_BuildPhysicsWorldSystem = World.GetOrCreateManager(); + m_StepPhysicsWorldSystem = World.GetOrCreateManager(); + m_EndFramePhysicsSystem = World.GetOrCreateManager(); + m_DebugStreamSystem = World.GetOrCreateManager(); + } + + protected override JobHandle OnUpdate(JobHandle inputDeps) + { + if (!(HasSingleton() && GetSingleton().DrawTriggerEvents != 0)) + { + return inputDeps; + } + + inputDeps = JobHandle.CombineDependencies(inputDeps, m_BuildPhysicsWorldSystem.FinalJobHandle, m_StepPhysicsWorldSystem.FinalSimulationJobHandle); + + JobHandle handle = new DisplayTriggerEventsJob + { + World = m_BuildPhysicsWorldSystem.PhysicsWorld, + TriggerEvents = m_StepPhysicsWorldSystem.Simulation.TriggerEvents, + OutputStream = m_DebugStreamSystem.GetContext(1) + }.Schedule(1, 1, inputDeps); + + m_EndFramePhysicsSystem.HandlesToWaitFor.Add(handle); + + return handle; + } + + // Job which iterates over trigger events and writes display info to a DebugStream. + //[BurstCompile] + struct DisplayTriggerEventsJob : IJobParallelFor + { + [ReadOnly] public PhysicsWorld World; + [ReadOnly] public TriggerEvents TriggerEvents; + public DebugStream.Context OutputStream; + + public unsafe void Execute(int workItemIndex) + { + OutputStream.Begin(workItemIndex); + foreach (TriggerEvent triggerEvent in TriggerEvents) + { + RigidBody bodyA = World.Bodies[triggerEvent.BodyIndices.BodyAIndex]; + RigidBody bodyB = World.Bodies[triggerEvent.BodyIndices.BodyBIndex]; + + bool IsTrigger(Collider* collider, ColliderKey key) + { + if (collider->CollisionType == CollisionType.Convex) + { + return ((ConvexColliderHeader*)collider)->Material.IsTrigger; + } + else + { + collider->GetLeaf(key, out ChildCollider child); + collider = child.Collider; + UnityEngine.Assertions.Assert.IsTrue(collider->CollisionType == CollisionType.Convex); + return ((ConvexColliderHeader*)collider)->Material.IsTrigger; + } + } + + char[] text = "Triggered".ToCharArray(); + if (IsTrigger(bodyA.Collider, triggerEvent.ColliderKeys.ColliderKeyA)) + { + OutputStream.Text(text, bodyA.WorldFromBody.pos, Color.green); + } + if (IsTrigger(bodyB.Collider, triggerEvent.ColliderKeys.ColliderKeyB)) + { + OutputStream.Text(text, bodyB.WorldFromBody.pos, Color.green); + } + } + OutputStream.End(); + } + } + } +} diff --git a/package/Unity.Physics.Authoring/Utilities/DebugDisplay/DisplayTriggerEventsSystem.cs.meta b/package/Unity.Physics.Authoring/Utilities/DebugDisplay/DisplayTriggerEventsSystem.cs.meta new file mode 100755 index 000000000..590a115c0 --- /dev/null +++ b/package/Unity.Physics.Authoring/Utilities/DebugDisplay/DisplayTriggerEventsSystem.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 9a598846d29b2c54981e4a1fed75cc31 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Unity.Physics.Authoring/Utilities/PhysicsShapeExtensions.cs b/package/Unity.Physics.Authoring/Utilities/PhysicsShapeExtensions.cs new file mode 100755 index 000000000..a5516061b --- /dev/null +++ b/package/Unity.Physics.Authoring/Utilities/PhysicsShapeExtensions.cs @@ -0,0 +1,91 @@ +using System; +using System.Collections.Generic; +using Unity.Mathematics; +using UnityEngine; +using UnityCollider = UnityEngine.Collider; + +namespace Unity.Physics.Authoring +{ + static class PhysicsShapeExtensions + { + internal static int GetDeviantAxis(this float3 v) + { + var deviation = math.abs(v - math.csum(v) / 3f); + return math.cmax(deviation) == deviation.z ? 2 : math.cmax(deviation) == deviation.y ? 1 : 0; + } + + internal static int GetMaxAxis(this float3 v) + { + var cmax = math.cmax(v); + return cmax == v.z ? 2 : cmax == v.y ? 1 : 0; + } + + internal static GameObject GetPrimaryBody(this UnityCollider collider) => GetPrimaryBody(collider.gameObject); + internal static GameObject GetPrimaryBody(this PhysicsShape shape) => GetPrimaryBody(shape.gameObject); + + static readonly List s_CollidersBuffer = new List(16); + static readonly List s_ShapesBuffer = new List(16); + + internal static GameObject GetPrimaryBody(GameObject shape) + { + var pb = shape.GetComponentInParent(); + var rb = shape.GetComponentInParent(); + if (pb != null) + { + return rb == null ? pb.gameObject : + pb.transform.IsChildOf(rb.transform) ? pb.gameObject : rb.gameObject; + } + if (rb != null) + return rb.gameObject; + // for implicit static shape, find topmost Collider or PhysicsShape + shape.gameObject.GetComponentsInParent(false, s_CollidersBuffer); + shape.gameObject.GetComponentsInParent(false, s_ShapesBuffer); + var topCollider = s_CollidersBuffer.Count > 0 + ? s_CollidersBuffer[s_CollidersBuffer.Count - 1].gameObject + : null; + s_CollidersBuffer.Clear(); + var topShape = s_ShapesBuffer.Count > 0 ? s_ShapesBuffer[s_ShapesBuffer.Count - 1].gameObject : null; + s_ShapesBuffer.Clear(); + return topCollider == null + ? topShape == null ? shape.gameObject : topShape + : topShape == null + ? topCollider + : topShape.transform.IsChildOf(topCollider.transform) + ? topCollider + : topShape; + } + + internal static void GetBakeTransformation( + this PhysicsShape shape, out float3 linearScalar, out float radiusScalar + ) + { + linearScalar = shape.transform.lossyScale; + + radiusScalar = 1f; + var s = math.abs(linearScalar); + switch (shape.ShapeType) + { + case ShapeType.Box: + radiusScalar = math.cmin(s); + break; + case ShapeType.Sphere: + radiusScalar = math.cmax(s); + break; + case ShapeType.Capsule: + var cmax = math.cmax(s); + var cmin = math.cmin(s); + var cmaxI = cmax == s.z ? 2 : cmax == s.y ? 1 : 0; + var cminI = cmin == s.z ? 2 : cmin == s.y ? 1 : 0; + var cmidI = cmaxI == 2 ? (cminI == 1 ? 0 : 1) : (cminI == 1 ? 2 : 0); + radiusScalar = s[cmidI]; + break; + case ShapeType.Cylinder: + radiusScalar = math.cmax(s); + break; + case ShapeType.ConvexHull: + radiusScalar = math.cmax(s); + break; + } + } + } +} diff --git a/package/Unity.Physics.Authoring/Utilities/PhysicsShapeExtensions.cs.meta b/package/Unity.Physics.Authoring/Utilities/PhysicsShapeExtensions.cs.meta new file mode 100755 index 000000000..ff49ca085 --- /dev/null +++ b/package/Unity.Physics.Authoring/Utilities/PhysicsShapeExtensions.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: f4fc8685ca8bc4f45970ed1527afa7a5 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Unity.Physics.Editor.meta b/package/Unity.Physics.Editor.meta new file mode 100755 index 000000000..aec5422ea --- /dev/null +++ b/package/Unity.Physics.Editor.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: d756f2269480e3b4895796bafd6bcfbd +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Unity.Physics.Editor/AssemblyInfo.cs b/package/Unity.Physics.Editor/AssemblyInfo.cs new file mode 100755 index 000000000..bc949e149 --- /dev/null +++ b/package/Unity.Physics.Editor/AssemblyInfo.cs @@ -0,0 +1,3 @@ +using System.Runtime.CompilerServices; + +[assembly: InternalsVisibleTo("Unity.Physics.EditModeTests")] \ No newline at end of file diff --git a/package/Unity.Physics.Editor/AssemblyInfo.cs.meta b/package/Unity.Physics.Editor/AssemblyInfo.cs.meta new file mode 100755 index 000000000..9ccaa403c --- /dev/null +++ b/package/Unity.Physics.Editor/AssemblyInfo.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 7b3cc3eb9b3304aaabd9bef2198277a3 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Unity.Physics.Editor/Editor.meta b/package/Unity.Physics.Editor/Editor.meta new file mode 100755 index 000000000..630b22275 --- /dev/null +++ b/package/Unity.Physics.Editor/Editor.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: 683bc849d791a4714930b4563e0b66cc +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Unity.Physics.Editor/Editor/Resources.meta b/package/Unity.Physics.Editor/Editor/Resources.meta new file mode 100755 index 000000000..34ef657a7 --- /dev/null +++ b/package/Unity.Physics.Editor/Editor/Resources.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: ab526351563b84e9a82eed9f3e7b3f22 +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Unity.Physics.Editor/Editor/Resources/CustomFlagNames Icon.psd b/package/Unity.Physics.Editor/Editor/Resources/CustomFlagNames Icon.psd new file mode 100755 index 000000000..7714a7df4 Binary files /dev/null and b/package/Unity.Physics.Editor/Editor/Resources/CustomFlagNames Icon.psd differ diff --git a/package/Unity.Physics.Editor/Editor/Resources/CustomFlagNames Icon.psd.meta b/package/Unity.Physics.Editor/Editor/Resources/CustomFlagNames Icon.psd.meta new file mode 100755 index 000000000..8d62d7690 --- /dev/null +++ b/package/Unity.Physics.Editor/Editor/Resources/CustomFlagNames Icon.psd.meta @@ -0,0 +1,101 @@ +fileFormatVersion: 2 +guid: 7c7a0159cb0d5433b90c970978f6cf5c +TextureImporter: + internalIDToNameTable: [] + externalObjects: {} + serializedVersion: 9 + mipmaps: + mipMapMode: 0 + enableMipMap: 0 + sRGBTexture: 1 + linearTexture: 0 + fadeOut: 0 + borderMipMap: 0 + mipMapsPreserveCoverage: 0 + alphaTestReferenceValue: 0.5 + mipMapFadeDistanceStart: 1 + mipMapFadeDistanceEnd: 3 + bumpmap: + convertToNormalMap: 0 + externalNormalMap: 0 + heightScale: 0.25 + normalMapFilter: 0 + isReadable: 0 + streamingMipmaps: 0 + streamingMipmapsPriority: 0 + grayScaleToAlpha: 0 + generateCubemap: 6 + cubemapConvolution: 0 + seamlessCubemap: 0 + textureFormat: 1 + maxTextureSize: 2048 + textureSettings: + serializedVersion: 2 + filterMode: -1 + aniso: 1 + mipBias: -100 + wrapU: 1 + wrapV: 1 + wrapW: -1 + nPOTScale: 0 + lightmap: 0 + compressionQuality: 50 + spriteMode: 0 + spriteExtrude: 1 + spriteMeshType: 1 + alignment: 0 + spritePivot: {x: 0.5, y: 0.5} + spritePixelsToUnits: 100 + spriteBorder: {x: 0, y: 0, z: 0, w: 0} + spriteGenerateFallbackPhysicsShape: 1 + alphaUsage: 1 + alphaIsTransparency: 1 + spriteTessellationDetail: -1 + textureType: 2 + textureShape: 1 + singleChannelComponent: 0 + maxTextureSizeSet: 0 + compressionQualitySet: 0 + textureFormatSet: 0 + platformSettings: + - serializedVersion: 2 + buildTarget: DefaultTexturePlatform + maxTextureSize: 2048 + resizeAlgorithm: 0 + textureFormat: -1 + textureCompression: 1 + compressionQuality: 50 + crunchedCompression: 0 + allowsAlphaSplitting: 0 + overridden: 0 + androidETC2FallbackOverride: 0 + - serializedVersion: 2 + buildTarget: Standalone + maxTextureSize: 2048 + resizeAlgorithm: 0 + textureFormat: -1 + textureCompression: 1 + compressionQuality: 50 + crunchedCompression: 0 + allowsAlphaSplitting: 0 + overridden: 0 + androidETC2FallbackOverride: 0 + spriteSheet: + serializedVersion: 2 + sprites: [] + outline: [] + physicsShape: [] + bones: [] + spriteID: + internalID: 0 + vertices: [] + indices: + edges: [] + weights: [] + secondaryTextures: [] + spritePackingTag: + pSDRemoveMatte: 0 + pSDShowRemoveMatteOption: 0 + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Unity.Physics.Editor/Editor/Resources/PhysicsCategoryNames Icon.psd b/package/Unity.Physics.Editor/Editor/Resources/PhysicsCategoryNames Icon.psd new file mode 100755 index 000000000..b33bb0b57 Binary files /dev/null and b/package/Unity.Physics.Editor/Editor/Resources/PhysicsCategoryNames Icon.psd differ diff --git a/package/Unity.Physics.Editor/Editor/Resources/PhysicsCategoryNames Icon.psd.meta b/package/Unity.Physics.Editor/Editor/Resources/PhysicsCategoryNames Icon.psd.meta new file mode 100755 index 000000000..514ec8f35 --- /dev/null +++ b/package/Unity.Physics.Editor/Editor/Resources/PhysicsCategoryNames Icon.psd.meta @@ -0,0 +1,101 @@ +fileFormatVersion: 2 +guid: 269c31d3570d84742a0d8739b06d5a18 +TextureImporter: + internalIDToNameTable: [] + externalObjects: {} + serializedVersion: 9 + mipmaps: + mipMapMode: 0 + enableMipMap: 0 + sRGBTexture: 1 + linearTexture: 0 + fadeOut: 0 + borderMipMap: 0 + mipMapsPreserveCoverage: 0 + alphaTestReferenceValue: 0.5 + mipMapFadeDistanceStart: 1 + mipMapFadeDistanceEnd: 3 + bumpmap: + convertToNormalMap: 0 + externalNormalMap: 0 + heightScale: 0.25 + normalMapFilter: 0 + isReadable: 0 + streamingMipmaps: 0 + streamingMipmapsPriority: 0 + grayScaleToAlpha: 0 + generateCubemap: 6 + cubemapConvolution: 0 + seamlessCubemap: 0 + textureFormat: 1 + maxTextureSize: 2048 + textureSettings: + serializedVersion: 2 + filterMode: -1 + aniso: 1 + mipBias: -100 + wrapU: 1 + wrapV: 1 + wrapW: -1 + nPOTScale: 0 + lightmap: 0 + compressionQuality: 50 + spriteMode: 0 + spriteExtrude: 1 + spriteMeshType: 1 + alignment: 0 + spritePivot: {x: 0.5, y: 0.5} + spritePixelsToUnits: 100 + spriteBorder: {x: 0, y: 0, z: 0, w: 0} + spriteGenerateFallbackPhysicsShape: 1 + alphaUsage: 1 + alphaIsTransparency: 1 + spriteTessellationDetail: -1 + textureType: 2 + textureShape: 1 + singleChannelComponent: 0 + maxTextureSizeSet: 0 + compressionQualitySet: 0 + textureFormatSet: 0 + platformSettings: + - serializedVersion: 2 + buildTarget: DefaultTexturePlatform + maxTextureSize: 2048 + resizeAlgorithm: 0 + textureFormat: -1 + textureCompression: 1 + compressionQuality: 50 + crunchedCompression: 0 + allowsAlphaSplitting: 0 + overridden: 0 + androidETC2FallbackOverride: 0 + - serializedVersion: 2 + buildTarget: Standalone + maxTextureSize: 2048 + resizeAlgorithm: 0 + textureFormat: -1 + textureCompression: 1 + compressionQuality: 50 + crunchedCompression: 0 + allowsAlphaSplitting: 0 + overridden: 0 + androidETC2FallbackOverride: 0 + spriteSheet: + serializedVersion: 2 + sprites: [] + outline: [] + physicsShape: [] + bones: [] + spriteID: + internalID: 0 + vertices: [] + indices: + edges: [] + weights: [] + secondaryTextures: [] + spritePackingTag: + pSDRemoveMatte: 0 + pSDShowRemoveMatteOption: 0 + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Unity.Physics.Editor/Editor/Resources/PhysicsMaterialTemplate Icon.psd b/package/Unity.Physics.Editor/Editor/Resources/PhysicsMaterialTemplate Icon.psd new file mode 100755 index 000000000..5ab6c1fed Binary files /dev/null and b/package/Unity.Physics.Editor/Editor/Resources/PhysicsMaterialTemplate Icon.psd differ diff --git a/package/Unity.Physics.Editor/Editor/Resources/PhysicsMaterialTemplate Icon.psd.meta b/package/Unity.Physics.Editor/Editor/Resources/PhysicsMaterialTemplate Icon.psd.meta new file mode 100755 index 000000000..dffe754cb --- /dev/null +++ b/package/Unity.Physics.Editor/Editor/Resources/PhysicsMaterialTemplate Icon.psd.meta @@ -0,0 +1,101 @@ +fileFormatVersion: 2 +guid: dea88969a14a54552b290b80692e0785 +TextureImporter: + internalIDToNameTable: [] + externalObjects: {} + serializedVersion: 9 + mipmaps: + mipMapMode: 0 + enableMipMap: 0 + sRGBTexture: 1 + linearTexture: 0 + fadeOut: 0 + borderMipMap: 0 + mipMapsPreserveCoverage: 0 + alphaTestReferenceValue: 0.5 + mipMapFadeDistanceStart: 1 + mipMapFadeDistanceEnd: 3 + bumpmap: + convertToNormalMap: 0 + externalNormalMap: 0 + heightScale: 0.25 + normalMapFilter: 0 + isReadable: 0 + streamingMipmaps: 0 + streamingMipmapsPriority: 0 + grayScaleToAlpha: 0 + generateCubemap: 6 + cubemapConvolution: 0 + seamlessCubemap: 0 + textureFormat: 1 + maxTextureSize: 2048 + textureSettings: + serializedVersion: 2 + filterMode: -1 + aniso: 1 + mipBias: -100 + wrapU: 1 + wrapV: 1 + wrapW: -1 + nPOTScale: 0 + lightmap: 0 + compressionQuality: 50 + spriteMode: 0 + spriteExtrude: 1 + spriteMeshType: 1 + alignment: 0 + spritePivot: {x: 0.5, y: 0.5} + spritePixelsToUnits: 100 + spriteBorder: {x: 0, y: 0, z: 0, w: 0} + spriteGenerateFallbackPhysicsShape: 1 + alphaUsage: 1 + alphaIsTransparency: 1 + spriteTessellationDetail: -1 + textureType: 2 + textureShape: 1 + singleChannelComponent: 0 + maxTextureSizeSet: 0 + compressionQualitySet: 0 + textureFormatSet: 0 + platformSettings: + - serializedVersion: 2 + buildTarget: DefaultTexturePlatform + maxTextureSize: 2048 + resizeAlgorithm: 0 + textureFormat: -1 + textureCompression: 1 + compressionQuality: 50 + crunchedCompression: 0 + allowsAlphaSplitting: 0 + overridden: 0 + androidETC2FallbackOverride: 0 + - serializedVersion: 2 + buildTarget: Standalone + maxTextureSize: 2048 + resizeAlgorithm: 0 + textureFormat: -1 + textureCompression: 1 + compressionQuality: 50 + crunchedCompression: 0 + allowsAlphaSplitting: 0 + overridden: 0 + androidETC2FallbackOverride: 0 + spriteSheet: + serializedVersion: 2 + sprites: [] + outline: [] + physicsShape: [] + bones: [] + spriteID: + internalID: 0 + vertices: [] + indices: + edges: [] + weights: [] + secondaryTextures: [] + spritePackingTag: + pSDRemoveMatte: 0 + pSDShowRemoveMatteOption: 0 + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Unity.Physics.Editor/Editor/Resources/ShapeHandle.shader b/package/Unity.Physics.Editor/Editor/Resources/ShapeHandle.shader new file mode 100755 index 000000000..53e44878a --- /dev/null +++ b/package/Unity.Physics.Editor/Editor/Resources/ShapeHandle.shader @@ -0,0 +1,57 @@ +Shader "Hidden/Physics/ShapeHandle" +{ + Properties + { + _HandleZTest ("_HandleZTest", Int) = 8 + } + SubShader + { + Tags { "Queue" = "Transparent" "ForceSupported" = "True" } + Fog { Mode Off } + Blend SrcAlpha OneMinusSrcAlpha + ZWrite Off + ZTest [_HandleZTest] + + Pass + { + CGPROGRAM + #pragma vertex vert + #pragma fragment frag + #pragma target 2.0 + #pragma multi_compile_fog + + #include "UnityCG.cginc" + + struct appdata_t + { + float4 vertex : POSITION; + UNITY_VERTEX_INPUT_INSTANCE_ID + }; + + struct v2f + { + float4 vertex : SV_POSITION; + UNITY_FOG_COORDS(0) + UNITY_VERTEX_OUTPUT_STEREO + }; + + uniform float4 _HandleColor; + + v2f vert (appdata_t v) + { + v2f o; + UNITY_SETUP_INSTANCE_ID(v); + UNITY_INITIALIZE_VERTEX_OUTPUT_STEREO(o); + o.vertex = UnityObjectToClipPos(v.vertex); + UNITY_TRANSFER_FOG(o,o.vertex); + return o; + } + + fixed4 frag (v2f i) : COLOR + { + return _HandleColor; + } + ENDCG + } + } +} diff --git a/package/Unity.Physics.Editor/Editor/Resources/ShapeHandle.shader.meta b/package/Unity.Physics.Editor/Editor/Resources/ShapeHandle.shader.meta new file mode 100755 index 000000000..2851cf215 --- /dev/null +++ b/package/Unity.Physics.Editor/Editor/Resources/ShapeHandle.shader.meta @@ -0,0 +1,9 @@ +fileFormatVersion: 2 +guid: 5a340c089627c4b60b12dfae149bf1a1 +ShaderImporter: + externalObjects: {} + defaultTextures: [] + nonModifiableTextures: [] + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Unity.Physics.Editor/EditorTools.meta b/package/Unity.Physics.Editor/EditorTools.meta new file mode 100755 index 000000000..ba62bc088 --- /dev/null +++ b/package/Unity.Physics.Editor/EditorTools.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: eafd2df9888a843d98dad6b614dda084 +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Unity.Physics.Editor/EditorTools/ConvexBoxBoundsHandle.cs b/package/Unity.Physics.Editor/EditorTools/ConvexBoxBoundsHandle.cs new file mode 100755 index 000000000..010e3a7aa --- /dev/null +++ b/package/Unity.Physics.Editor/EditorTools/ConvexBoxBoundsHandle.cs @@ -0,0 +1,73 @@ +using System; +using Unity.Mathematics; +using UnityEditor; +using UnityEditor.IMGUI.Controls; +using UnityEngine; + +namespace Unity.Physics.Editor +{ + class ConvexBoxBoundsHandle : BoxBoundsHandle + { + public float ConvexRadius { get; set; } + + protected override void DrawWireframe() + { + if (ConvexRadius <= 0f) + { + base.DrawWireframe(); + return; + } + + var center = (float3)this.center; + var size = (float3)this.size; + DrawFace(center, size * new float3( 1f, 1f, 1f), 0, 1, 2); + DrawFace(center, size * new float3(-1f, 1f, 1f), 0, 1, 2); + DrawFace(center, size * new float3( 1f, 1f, 1f), 1, 0, 2); + DrawFace(center, size * new float3( 1f, -1f, 1f), 1, 0, 2); + DrawFace(center, size * new float3( 1f, 1f, 1f), 2, 0, 1); + DrawFace(center, size * new float3( 1f, 1f, -1f), 2, 0, 1); + + var corner = 0.5f * size - new float3(1f) * ConvexRadius; + var rgt = new float3(1f, 0f, 0f); + var up = new float3(0f, 1f, 0f); + var fwd = new float3(0f, 0f, 1f); + DrawCorner(center + corner * new float3( 1f, 1f, 1f), quaternion.LookRotation( fwd, up)); + DrawCorner(center + corner * new float3(-1f, 1f, 1f), quaternion.LookRotation(-rgt, up)); + DrawCorner(center + corner * new float3( 1f, -1f, 1f), quaternion.LookRotation( rgt, -up)); + DrawCorner(center + corner * new float3( 1f, 1f, -1f), quaternion.LookRotation(-fwd, rgt)); + DrawCorner(center + corner * new float3(-1f, -1f, 1f), quaternion.LookRotation( fwd, -up)); + DrawCorner(center + corner * new float3(-1f, 1f, -1f), quaternion.LookRotation(-fwd, up)); + DrawCorner(center + corner * new float3( 1f, -1f, -1f), quaternion.LookRotation(-fwd, -up)); + DrawCorner(center + corner * new float3(-1f, -1f, -1f), quaternion.LookRotation(-rgt, -up)); + } + + static Vector3[] s_FacePoints = new Vector3[8]; + + void DrawFace(float3 center, float3 size, int a, int b, int c) + { + size *= 0.5f; + var ctr = center + new float3 { [a] = size[a] }; + var i = 0; + size -= new float3(ConvexRadius); + s_FacePoints[i++] = ctr + new float3 { [b] = size[b], [c] = size[c] }; + s_FacePoints[i++] = ctr + new float3 { [b] = -size[b], [c] = size[c] }; + s_FacePoints[i++] = ctr + new float3 { [b] = -size[b], [c] = size[c] }; + s_FacePoints[i++] = ctr + new float3 { [b] = -size[b], [c] = -size[c] }; + s_FacePoints[i++] = ctr + new float3 { [b] = -size[b], [c] = -size[c] }; + s_FacePoints[i++] = ctr + new float3 { [b] = size[b], [c] = -size[c] }; + s_FacePoints[i++] = ctr + new float3 { [b] = size[b], [c] = -size[c] }; + s_FacePoints[i++] = ctr + new float3 { [b] = size[b], [c] = size[c] }; + Handles.DrawLines(s_FacePoints); + } + + void DrawCorner(float3 point, quaternion orientation) + { + var rgt = math.mul(orientation, new float3(1f, 0f, 0f)); + var up = math.mul(orientation, new float3(0f, 1f, 0f)); + var fwd = math.mul(orientation, new float3(0f, 0f, 1f)); + Handles.DrawWireArc(point, fwd, rgt, 90f, ConvexRadius); + Handles.DrawWireArc(point, rgt, up, 90f, ConvexRadius); + Handles.DrawWireArc(point, up, fwd, 90f, ConvexRadius); + } + } +} \ No newline at end of file diff --git a/package/Unity.Physics.Editor/EditorTools/ConvexBoxBoundsHandle.cs.meta b/package/Unity.Physics.Editor/EditorTools/ConvexBoxBoundsHandle.cs.meta new file mode 100755 index 000000000..fdee911d0 --- /dev/null +++ b/package/Unity.Physics.Editor/EditorTools/ConvexBoxBoundsHandle.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 3d159761cbfd64ab4b45c185b806856c +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Unity.Physics.Editor/EditorTools/ConvexCylinderBoundsHandle.cs b/package/Unity.Physics.Editor/EditorTools/ConvexCylinderBoundsHandle.cs new file mode 100755 index 000000000..e6cd8d61f --- /dev/null +++ b/package/Unity.Physics.Editor/EditorTools/ConvexCylinderBoundsHandle.cs @@ -0,0 +1,92 @@ +using System; +using Unity.Mathematics; +using UnityEditor; +using UnityEditor.IMGUI.Controls; +using UnityEngine; + +namespace Unity.Physics.Editor +{ + class ConvexCylinderBoundsHandle : PrimitiveBoundsHandle + { + public float ConvexRadius { get; set; } + + public float Height + { + get => GetSize().z; + set + { + var size = GetSize(); + size.z = math.max(0f, value); + SetSize(size); + } + } + + public float Radius + { + get => GetSize().x; + set + { + var size = GetSize(); + size.x = size.y = math.max(0f, value); + SetSize(size); + } + } + + const int k_NumSpans = ConvexConvexManifoldQueries.Manifold.k_MaxNumContacts; + static readonly Vector3[] s_Points = new Vector3[k_NumSpans * 6]; + static readonly Vector3[] s_PointsWithRadius = new Vector3[k_NumSpans * 10]; + + protected override void DrawWireframe() + { + var halfHeight = new float3(0f, 0f, Height * 0.5f); + var t = 2f * 31 / k_NumSpans; + var prevXY = new float3((float)math.cos(math.PI * t), (float)math.sin(math.PI * t), 0f) * Radius; + int step; + Vector3[] points; + if (ConvexRadius > 0f) + { + points = s_PointsWithRadius; + step = 10; + } + else + { + points = s_Points; + step = 6; + } + for (var i = 0; i < k_NumSpans; ++i) + { + t = 2f * i / k_NumSpans; + var xy = new float3((float)math.cos(math.PI * t), (float)math.sin(math.PI * t), 0f) * Radius; + var xyCvx = math.normalizesafe(xy) * ConvexRadius; + var idx = i * step; + // height + points[idx++] = xy + halfHeight - new float3 { z = ConvexRadius }; + points[idx++] = xy - halfHeight + new float3 { z = ConvexRadius }; + // top + points[idx++] = prevXY + halfHeight - xyCvx; + points[idx++] = xy + halfHeight - xyCvx; + // bottom + points[idx++] = prevXY - halfHeight - xyCvx; + points[idx++] = xy - halfHeight - xyCvx; + // convex + if (ConvexRadius > 0f) + { + // top + points[idx++] = prevXY + halfHeight - new float3 { z = ConvexRadius }; + points[idx++] = xy + halfHeight - new float3 { z = ConvexRadius }; + // bottom + points[idx++] = prevXY - halfHeight + new float3 { z = ConvexRadius }; + points[idx++] = xy - halfHeight + new float3 { z = ConvexRadius }; + // corners + var normal = math.cross(new float3(0f, 0f, 1f), xy); + var p = new float3(xy.x, xy.y, halfHeight.z) - new float3(xyCvx.x, xyCvx.y, ConvexRadius); + Handles.DrawWireArc(p, normal, xy, -90f, ConvexRadius); + p *= new float3(1f, 1f, -1f); + Handles.DrawWireArc(p, normal, xy, 90f, ConvexRadius); + } + prevXY = xy; + } + Handles.DrawLines(points); + } + } +} \ No newline at end of file diff --git a/package/Unity.Physics.Editor/EditorTools/ConvexCylinderBoundsHandle.cs.meta b/package/Unity.Physics.Editor/EditorTools/ConvexCylinderBoundsHandle.cs.meta new file mode 100755 index 000000000..8211713d9 --- /dev/null +++ b/package/Unity.Physics.Editor/EditorTools/ConvexCylinderBoundsHandle.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 7a64b7883aec4411eb5718b49553d58a +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Unity.Physics.Editor/Editors.meta b/package/Unity.Physics.Editor/Editors.meta new file mode 100755 index 000000000..38116c431 --- /dev/null +++ b/package/Unity.Physics.Editor/Editors.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: a40c9f4dd335149c8aebb97c6fe09e12 +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Unity.Physics.Editor/Editors/BaseEditor.cs b/package/Unity.Physics.Editor/Editors/BaseEditor.cs new file mode 100755 index 000000000..a3b0dc097 --- /dev/null +++ b/package/Unity.Physics.Editor/Editors/BaseEditor.cs @@ -0,0 +1,121 @@ +using System; +using System.Collections.Generic; +using System.Linq; +using System.Reflection; +using UnityEditor; +using UnityEditorInternal; +using UnityEngine; + +namespace Unity.Physics.Editor +{ + [AttributeUsage(AttributeTargets.Field)] + sealed class AutoPopulateAttribute : Attribute + { + public string ElementFormatString { get; set; } + public bool Reorderable { get; set; } = true; + public bool Resizable { get; set; } = true; + } + + abstract class BaseEditor : UnityEditor.Editor + { + static class Content + { + public static readonly string UnableToLocateFormatString = L10n.Tr("Cannot find SerializedProperty {0}"); + } + + List m_AutoFieldGUIControls = new List(); + + protected virtual void OnEnable() + { + const BindingFlags bindingFlags = + BindingFlags.NonPublic | BindingFlags.Instance | BindingFlags.FlattenHierarchy; + var autoFields = GetType().GetFields(bindingFlags) + .Where(f => Attribute.IsDefined(f, typeof(AutoPopulateAttribute))) + .ToArray(); + + foreach (var field in autoFields) + { + var sp = serializedObject.FindProperty(field.Name); + + if (sp == null) + { + var message = string.Format(Content.UnableToLocateFormatString, field.Name); + m_AutoFieldGUIControls.Add(() => EditorGUILayout.HelpBox(message, MessageType.Error)); + Debug.LogError(message); + continue; + } + + if (field.FieldType == typeof(SerializedProperty)) + { + field.SetValue(this, sp); + m_AutoFieldGUIControls.Add(() => EditorGUILayout.PropertyField(sp, true)); + } + else if (field.FieldType == typeof(ReorderableList)) + { + var list = new ReorderableList(serializedObject, sp); + + var label = EditorGUIUtility.TrTextContent(sp.displayName); + list.drawHeaderCallback = rect => EditorGUI.LabelField(rect, label); + + list.elementHeightCallback = index => + { + var element = list.serializedProperty.GetArrayElementAtIndex(index); + return EditorGUI.GetPropertyHeight(element) + EditorGUIUtility.standardVerticalSpacing; + }; + + var attr = + field.GetCustomAttributes(typeof(AutoPopulateAttribute)).Single() as AutoPopulateAttribute; + var formatString = attr.ElementFormatString; + if (formatString == null) + { + list.drawElementCallback = (rect, index, active, focused) => + { + var element = list.serializedProperty.GetArrayElementAtIndex(index); + EditorGUI.PropertyField( + new Rect(rect) { height = EditorGUI.GetPropertyHeight(element) }, element, true + ); + }; + } + else + { + var noLabel = formatString == string.Empty; + if (!noLabel) + formatString = L10n.Tr(formatString); + var elementLabel = new GUIContent(); + list.drawElementCallback = (rect, index, active, focused) => + { + var element = list.serializedProperty.GetArrayElementAtIndex(index); + if (!noLabel) + elementLabel.text = string.Format(formatString, index); + EditorGUI.PropertyField( + new Rect(rect) { height = EditorGUI.GetPropertyHeight(element) }, + element, + noLabel ? GUIContent.none : elementLabel, + true + ); + }; + } + + list.draggable = attr.Reorderable; + list.displayAdd = list.displayRemove = attr.Resizable; + + field.SetValue(this, list); + m_AutoFieldGUIControls.Add(() => list.DoLayoutList()); + } + } + } + + public override void OnInspectorGUI() + { + serializedObject.Update(); + + EditorGUI.BeginChangeCheck(); + + foreach (var guiControl in m_AutoFieldGUIControls) + guiControl(); + + if (EditorGUI.EndChangeCheck()) + serializedObject.ApplyModifiedProperties(); + } + } +} diff --git a/package/Unity.Physics.Editor/Editors/BaseEditor.cs.meta b/package/Unity.Physics.Editor/Editors/BaseEditor.cs.meta new file mode 100755 index 000000000..f9b004101 --- /dev/null +++ b/package/Unity.Physics.Editor/Editors/BaseEditor.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 0459f91158f584084a9b3f9d3efe572a +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Unity.Physics.Editor/Editors/CustomFlagNamesEditor.cs b/package/Unity.Physics.Editor/Editors/CustomFlagNamesEditor.cs new file mode 100755 index 000000000..50238d345 --- /dev/null +++ b/package/Unity.Physics.Editor/Editors/CustomFlagNamesEditor.cs @@ -0,0 +1,17 @@ +using Unity.Physics.Authoring; +using UnityEditor; +using UnityEditorInternal; +using UnityEngine; + +namespace Unity.Physics.Editor +{ + [CustomEditor(typeof(CustomFlagNames))] + [CanEditMultipleObjects] + class CustomFlagNamesEditor : BaseEditor + { + #pragma warning disable 649 + [AutoPopulate(ElementFormatString = "Custom Flag {0}", Resizable = false, Reorderable = false)] + ReorderableList m_FlagNames; + #pragma warning restore 649 + } +} diff --git a/package/Unity.Physics.Editor/Editors/CustomFlagNamesEditor.cs.meta b/package/Unity.Physics.Editor/Editors/CustomFlagNamesEditor.cs.meta new file mode 100755 index 000000000..093540de8 --- /dev/null +++ b/package/Unity.Physics.Editor/Editors/CustomFlagNamesEditor.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 381742f9fa30c4306b9067a7e36f6cb1 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Unity.Physics.Editor/Editors/PhysicsBodyEditor.cs b/package/Unity.Physics.Editor/Editors/PhysicsBodyEditor.cs new file mode 100755 index 000000000..2cbae7468 --- /dev/null +++ b/package/Unity.Physics.Editor/Editors/PhysicsBodyEditor.cs @@ -0,0 +1,135 @@ +using System.Collections.Generic; +using Unity.Mathematics; +using Unity.Physics.Authoring; +using UnityEditor; +using UnityEngine; + +namespace Unity.Physics.Editor +{ + [CustomEditor(typeof(PhysicsBody))] + [CanEditMultipleObjects] + class PhysicsBodyEditor : BaseEditor + { + static class Content + { + public static readonly GUIContent MassLabel = EditorGUIUtility.TrTextContent("Mass"); + public static readonly GUIContent CenterOfMassLabel = EditorGUIUtility.TrTextContent( + "Center of Mass", "Center of mass in the space of this body's transform." + ); + public static readonly GUIContent InertiaTensorLabel = EditorGUIUtility.TrTextContent( + "Inertia Tensor", "Resistance to angular motion about each axis of rotation." + ); + public static readonly GUIContent OrientationLabel = EditorGUIUtility.TrTextContent( + "Orientation", "Orientation of the body's inertia tensor in the space of its transform." + ); + } + + #pragma warning disable 649 + [AutoPopulate] SerializedProperty m_MotionType; + [AutoPopulate] SerializedProperty m_Mass; + [AutoPopulate] SerializedProperty m_GravityFactor; + [AutoPopulate] SerializedProperty m_LinearDamping; + [AutoPopulate] SerializedProperty m_AngularDamping; + [AutoPopulate] SerializedProperty m_InitialLinearVelocity; + [AutoPopulate] SerializedProperty m_InitialAngularVelocity; + [AutoPopulate] SerializedProperty m_OverrideDefaultMassDistribution; + [AutoPopulate] SerializedProperty m_CenterOfMass; + [AutoPopulate] SerializedProperty m_Orientation; + [AutoPopulate] SerializedProperty m_InertiaTensor; + #pragma warning restore 649 + + public override void OnInspectorGUI() + { + serializedObject.Update(); + + EditorGUI.BeginChangeCheck(); + + EditorGUILayout.PropertyField(m_MotionType); + + var dynamic = m_MotionType.intValue == (int)BodyMotionType.Dynamic; + + if (dynamic) + EditorGUILayout.PropertyField(m_Mass, Content.MassLabel); + else + { + EditorGUI.BeginDisabledGroup(true); + var position = EditorGUILayout.GetControlRect(true, EditorGUIUtility.singleLineHeight); + EditorGUI.BeginProperty(position, Content.MassLabel, m_Mass); + EditorGUI.FloatField(position, Content.MassLabel, float.PositiveInfinity); + EditorGUI.EndProperty(); + EditorGUI.EndDisabledGroup(); + } + + if (m_MotionType.intValue == (int)BodyMotionType.Dynamic) + { + EditorGUILayout.PropertyField(m_LinearDamping, true); + EditorGUILayout.PropertyField(m_AngularDamping, true); + } + + if (m_MotionType.intValue != (int)BodyMotionType.Static) + { + EditorGUILayout.PropertyField(m_InitialLinearVelocity, true); + EditorGUILayout.PropertyField(m_InitialAngularVelocity, true); + } + + if (m_MotionType.intValue == (int)BodyMotionType.Dynamic) + { + EditorGUILayout.PropertyField(m_GravityFactor, true); + } + + if (m_MotionType.intValue != (int)BodyMotionType.Static) + { + EditorGUILayout.PropertyField(m_OverrideDefaultMassDistribution); + if (m_OverrideDefaultMassDistribution.boolValue) + { + ++EditorGUI.indentLevel; + EditorGUILayout.PropertyField(m_CenterOfMass, Content.CenterOfMassLabel); + + EditorGUI.BeginDisabledGroup(!dynamic); + if (dynamic) + { + EditorGUILayout.PropertyField(m_Orientation, Content.OrientationLabel); + EditorGUILayout.PropertyField(m_InertiaTensor, Content.InertiaTensorLabel); + } + else + { + EditorGUI.BeginDisabledGroup(true); + var position = + EditorGUILayout.GetControlRect(true, EditorGUI.GetPropertyHeight(m_InertiaTensor)); + EditorGUI.BeginProperty(position, Content.InertiaTensorLabel, m_InertiaTensor); + EditorGUI.Vector3Field(position, Content.InertiaTensorLabel, + Vector3.one * float.PositiveInfinity); + EditorGUI.EndProperty(); + EditorGUI.EndDisabledGroup(); + } + + EditorGUI.EndDisabledGroup(); + + --EditorGUI.indentLevel; + } + } + + if (EditorGUI.EndChangeCheck()) + serializedObject.ApplyModifiedProperties(); + + DisplayStatusMessages(); + } + + List m_MatrixStates = new List(); + + void DisplayStatusMessages() + { + m_MatrixStates.Clear(); + foreach (var t in targets) + { + var localToWorld = (float4x4)(t as PhysicsBody).transform.localToWorldMatrix; + m_MatrixStates.Add(ManipulatorUtility.GetMatrixState(ref localToWorld)); + } + + string matrixStatusMessage; + var matrixStatus = MatrixGUIUtility.GetMatrixStatusMessage(m_MatrixStates, out matrixStatusMessage); + if (matrixStatus != MessageType.None) + EditorGUILayout.HelpBox(matrixStatusMessage, MessageType.Warning); + } + } +} diff --git a/package/Unity.Physics.Editor/Editors/PhysicsBodyEditor.cs.meta b/package/Unity.Physics.Editor/Editors/PhysicsBodyEditor.cs.meta new file mode 100755 index 000000000..4091516c1 --- /dev/null +++ b/package/Unity.Physics.Editor/Editors/PhysicsBodyEditor.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 174eb3fcbd301439589b61d0568ab6b5 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Unity.Physics.Editor/Editors/PhysicsCategoryNamesEditor.cs b/package/Unity.Physics.Editor/Editors/PhysicsCategoryNamesEditor.cs new file mode 100755 index 000000000..7e0cec272 --- /dev/null +++ b/package/Unity.Physics.Editor/Editors/PhysicsCategoryNamesEditor.cs @@ -0,0 +1,17 @@ +using Unity.Physics.Authoring; +using UnityEditor; +using UnityEditorInternal; +using UnityEngine; + +namespace Unity.Physics.Editor +{ + [CustomEditor(typeof(PhysicsCategoryNames))] + [CanEditMultipleObjects] + class PhysicsCategoryNamesEditor : BaseEditor + { + #pragma warning disable 649 + [AutoPopulate(ElementFormatString = "Category {0}", Resizable = false, Reorderable = false)] + ReorderableList m_CategoryNames; + #pragma warning restore 649 + } +} diff --git a/package/Unity.Physics.Editor/Editors/PhysicsCategoryNamesEditor.cs.meta b/package/Unity.Physics.Editor/Editors/PhysicsCategoryNamesEditor.cs.meta new file mode 100755 index 000000000..a2bfe4b88 --- /dev/null +++ b/package/Unity.Physics.Editor/Editors/PhysicsCategoryNamesEditor.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: a26a407942e11406eb77bf271e041364 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Unity.Physics.Editor/Editors/PhysicsShapeEditor.cs b/package/Unity.Physics.Editor/Editors/PhysicsShapeEditor.cs new file mode 100755 index 000000000..889d192e4 --- /dev/null +++ b/package/Unity.Physics.Editor/Editors/PhysicsShapeEditor.cs @@ -0,0 +1,467 @@ +using System; +using System.Collections.Generic; +using System.Linq; +using Unity.Collections; +using Unity.Mathematics; +using Unity.Physics.Authoring; +using UnityEditor; +using UnityEditor.IMGUI.Controls; +using UnityEngine; + +namespace Unity.Physics.Editor +{ + [CustomEditor(typeof(PhysicsShape))] + [CanEditMultipleObjects] + class PhysicsShapeEditor : BaseEditor + { + static class Styles + { + public static readonly string GenericUndoMessage = L10n.Tr("Change Shape"); + public static readonly string MultipleShapeTypesLabel = + L10n.Tr("Multiple shape types in current selection."); + public static readonly string StaticColliderStatusMessage = L10n.Tr( + $"This {ObjectNames.NicifyVariableName(typeof(PhysicsShape).Name)} will be considered static. " + + $"Add a {ObjectNames.NicifyVariableName(typeof(PhysicsBody).Name)} component if you will move it at run-time." + ); + public static readonly string StaticCollidersStatusMessage = L10n.Tr( + $"One or more selected {ObjectNames.NicifyVariableName(typeof(PhysicsShape).Name)}s will be considered static. " + + $"Add a {ObjectNames.NicifyVariableName(typeof(PhysicsBody).Name)} component if you will move them at run-time." + ); + + public static readonly GUIContent FitToRenderMeshesLabel = + EditorGUIUtility.TrTextContent("Fit to Render Meshes"); + public static readonly GUIContent CenterLabel = EditorGUIUtility.TrTextContent("Center"); + public static readonly GUIContent SizeLabel = EditorGUIUtility.TrTextContent("Size"); + public static readonly GUIContent OrientationLabel = EditorGUIUtility.TrTextContent( + "Orientation", "Euler orientation in the shape's local space (ZXY order)." + ); + public static readonly GUIContent RadiusLabel = EditorGUIUtility.TrTextContent("Radius"); + public static readonly GUIContent MaterialLabel = EditorGUIUtility.TrTextContent("Material"); + } + + #pragma warning disable 649 + [AutoPopulate] SerializedProperty m_ShapeType; + [AutoPopulate] SerializedProperty m_PrimitiveCenter; + [AutoPopulate] SerializedProperty m_PrimitiveSize; + [AutoPopulate] SerializedProperty m_PrimitiveOrientation; + [AutoPopulate] SerializedProperty m_Capsule; + [AutoPopulate] SerializedProperty m_Cylinder; + [AutoPopulate] SerializedProperty m_SphereRadius; + [AutoPopulate] SerializedProperty m_ConvexRadius; + [AutoPopulate] SerializedProperty m_CustomMesh; + [AutoPopulate] SerializedProperty m_Material; + #pragma warning restore 649 + + bool m_HasGeometry; + bool m_AtLeastOneStatic; + + protected override void OnEnable() + { + base.OnEnable(); + + var pointCloud = new NativeList(65535, Allocator.Temp); + (target as PhysicsShape).GetConvexHullProperties(pointCloud); + m_HasGeometry = pointCloud.Length > 0; + pointCloud.Dispose(); + + m_AtLeastOneStatic = targets.Cast() + .Select(shape => shape.GetComponentInParent()) + .Any(rb => rb == null); + + var wireframeShader = Shader.Find("Hidden/Physics/ShapeHandle"); + m_PreviewMeshMaterial = new UnityEngine.Material(wireframeShader) { hideFlags = HideFlags.HideAndDontSave }; + } + + void OnDisable() + { + foreach (var preview in m_PreviewMeshes.Values) + { + if (preview.Mesh != null) + DestroyImmediate(preview.Mesh); + } + if (m_PreviewMeshMaterial != null) + DestroyImmediate(m_PreviewMeshMaterial); + } + + class PreviewMeshData + { + public ShapeType Type; + public UnityEngine.Mesh SourceMesh; + public UnityEngine.Mesh Mesh; + } + + Dictionary m_PreviewMeshes = new Dictionary(); + UnityEngine.Material m_PreviewMeshMaterial; + + UnityEngine.Mesh GetPreviewMesh(PhysicsShape shape) + { + if (shape.ShapeType != ShapeType.ConvexHull && shape.ShapeType != ShapeType.Mesh) + return null; + + if (!m_PreviewMeshes.TryGetValue(shape, out var preview)) + preview = m_PreviewMeshes[shape] = new PreviewMeshData(); + + if (preview.Type != shape.ShapeType || preview.SourceMesh != shape.GetMesh()) + { + if (preview.Mesh == null) + preview.Mesh = new UnityEngine.Mesh { hideFlags = HideFlags.HideAndDontSave }; + preview.SourceMesh = shape.GetMesh(); + preview.Type = shape.ShapeType; + preview.Mesh.Clear(); + if (shape.ShapeType == ShapeType.ConvexHull) + { + // TODO: populate with the actual collision data + } + else + { + // TODO: populate with the actual collision data + if (preview.SourceMesh != null) + { + preview.Mesh.vertices = preview.SourceMesh.vertices; + preview.Mesh.normals = preview.SourceMesh.normals; + preview.Mesh.triangles = preview.SourceMesh.triangles; + } + } + preview.Mesh.RecalculateBounds(); + } + return preview.Mesh; + } + + public override void OnInspectorGUI() + { + serializedObject.Update(); + + EditorGUI.BeginChangeCheck(); + + DisplayShapeSelector(); + + ++EditorGUI.indentLevel; + + if (m_ShapeType.hasMultipleDifferentValues) + EditorGUILayout.HelpBox(Styles.MultipleShapeTypesLabel, MessageType.None); + else + { + switch ((ShapeType)m_ShapeType.intValue) + { + case ShapeType.Box: + DisplayBoxControls(); + break; + case ShapeType.Capsule: + DisplayCapsuleControls(); + break; + case ShapeType.Sphere: + DisplaySphereControls(); + break; + case ShapeType.Cylinder: + DisplayCylinderControls(); + break; + case ShapeType.Plane: + DisplayPlaneControls(); + break; + case ShapeType.ConvexHull: + case ShapeType.Mesh: + DisplayMeshControls(); + break; + } + } + + --EditorGUI.indentLevel; + + EditorGUILayout.LabelField(Styles.MaterialLabel); + + ++EditorGUI.indentLevel; + + EditorGUILayout.PropertyField(m_Material); + + --EditorGUI.indentLevel; + + DisplayStatusMessages(); + + if (EditorGUI.EndChangeCheck()) + serializedObject.ApplyModifiedProperties(); + } + + void DisplayShapeSelector() + { + EditorGUI.BeginChangeCheck(); + EditorGUILayout.PropertyField(m_ShapeType); + if (!EditorGUI.EndChangeCheck()) + return; + + Undo.RecordObjects(targets, Styles.GenericUndoMessage); + foreach (PhysicsShape shape in targets) + { + switch ((ShapeType)m_ShapeType.intValue) + { + case ShapeType.Box: + shape.GetBoxProperties(out var center, out var size, out EulerAngles orientation); + shape.SetBox(center, size, orientation); + break; + case ShapeType.Capsule: + shape.GetCapsuleProperties(out center, out var height, out var radius, out orientation); + shape.SetCapsule(center, height, radius, orientation); + break; + case ShapeType.Sphere: + shape.GetSphereProperties(out center, out radius, out orientation); + shape.SetSphere(center, radius, orientation); + break; + case ShapeType.Cylinder: + shape.GetCylinderProperties(out center, out height, out radius, out orientation); + shape.SetCylinder(center, height, radius, orientation); + break; + case ShapeType.Plane: + shape.GetPlaneProperties(out center, out var size2D, out orientation); + shape.SetPlane(center, size2D, orientation); + break; + default: + return; + } + EditorUtility.SetDirty(shape); + } + + GUIUtility.ExitGUI(); + } + + void FitToRenderMeshesButton() + { + EditorGUI.BeginDisabledGroup(!m_HasGeometry || EditorUtility.IsPersistent(target)); + var rect = EditorGUI.IndentedRect( + EditorGUILayout.GetControlRect(false, EditorGUIUtility.singleLineHeight, EditorStyles.miniButton) + ); + if (GUI.Button(rect, Styles.FitToRenderMeshesLabel, EditorStyles.miniButton)) + { + Undo.RecordObjects(targets, Styles.FitToRenderMeshesLabel.text); + foreach (PhysicsShape shape in targets) + { + shape.FitToGeometry(); + EditorUtility.SetDirty(shape); + } + } + EditorGUI.EndDisabledGroup(); + } + + void DisplayBoxControls() + { + EditorGUILayout.PropertyField(m_PrimitiveSize, Styles.SizeLabel, true); + + EditorGUILayout.PropertyField(m_PrimitiveCenter, Styles.CenterLabel, true); + EditorGUILayout.PropertyField(m_PrimitiveOrientation, Styles.OrientationLabel, true); + + EditorGUILayout.PropertyField(m_ConvexRadius); + + FitToRenderMeshesButton(); + } + + void DisplayCapsuleControls() + { + EditorGUI.BeginChangeCheck(); + EditorGUILayout.PropertyField(m_Capsule); + if (EditorGUI.EndChangeCheck()) + { + Undo.RecordObjects(targets, Styles.GenericUndoMessage); + foreach (PhysicsShape shape in targets) + { + shape.GetCapsuleProperties( + out var center, out var height, out var radius, out EulerAngles orientation + ); + shape.SetCapsule(center, height, radius, orientation); + EditorUtility.SetDirty(shape); + } + } + + EditorGUILayout.PropertyField(m_PrimitiveCenter, Styles.CenterLabel, true); + EditorGUILayout.PropertyField(m_PrimitiveOrientation, Styles.OrientationLabel, true); + + FitToRenderMeshesButton(); + } + + void DisplaySphereControls() + { + EditorGUI.BeginChangeCheck(); + EditorGUILayout.PropertyField(m_SphereRadius, Styles.RadiusLabel); + if (EditorGUI.EndChangeCheck()) + { + Undo.RecordObjects(targets, Styles.GenericUndoMessage); + foreach (PhysicsShape shape in targets) + { + shape.GetSphereProperties(out var center, out var radius, out EulerAngles orientation); + shape.SetSphere(center, m_SphereRadius.floatValue, orientation); + EditorUtility.SetDirty(shape); + } + } + + EditorGUILayout.PropertyField(m_PrimitiveCenter, Styles.CenterLabel, true); + EditorGUILayout.PropertyField(m_PrimitiveOrientation, Styles.OrientationLabel, true); + + FitToRenderMeshesButton(); + } + + void DisplayCylinderControls() + { + EditorGUI.BeginChangeCheck(); + EditorGUILayout.PropertyField(m_Cylinder); + if (EditorGUI.EndChangeCheck()) + { + Undo.RecordObjects(targets, Styles.GenericUndoMessage); + foreach (PhysicsShape shape in targets) + { + shape.GetCylinderProperties( + out var center, out var height, out var radius, out EulerAngles orientation + ); + shape.SetCylinder(center, height, radius, orientation); + EditorUtility.SetDirty(shape); + } + } + + EditorGUILayout.PropertyField(m_PrimitiveCenter, Styles.CenterLabel, true); + EditorGUILayout.PropertyField(m_PrimitiveOrientation, Styles.OrientationLabel, true); + + EditorGUILayout.PropertyField(m_ConvexRadius); + + FitToRenderMeshesButton(); + } + + void DisplayPlaneControls() + { + EditorGUILayout.PropertyField(m_PrimitiveSize, Styles.SizeLabel, true); + + EditorGUILayout.PropertyField(m_PrimitiveCenter, Styles.CenterLabel, true); + EditorGUILayout.PropertyField(m_PrimitiveOrientation, Styles.OrientationLabel, true); + + FitToRenderMeshesButton(); + } + + void DisplayMeshControls() + { + // TODO: warn if no render meshes and no custom mesh + EditorGUILayout.PropertyField(m_CustomMesh); + } + + void DisplayStatusMessages() + { + if (!m_AtLeastOneStatic) + return; + EditorGUILayout.HelpBox( + targets.Length == 1 ? Styles.StaticColliderStatusMessage : Styles.StaticCollidersStatusMessage, + MessageType.None + ); + } + + // TODO: implement interactive tool modes + static readonly ConvexBoxBoundsHandle s_Box = + new ConvexBoxBoundsHandle { handleColor = Color.clear }; + static readonly CapsuleBoundsHandle s_Capsule = + new CapsuleBoundsHandle { handleColor = Color.clear, heightAxis = CapsuleBoundsHandle.HeightAxis.Z }; + static readonly ConvexCylinderBoundsHandle s_ConvexCylinder = + new ConvexCylinderBoundsHandle { handleColor = Color.clear }; + static readonly SphereBoundsHandle s_Sphere = + new SphereBoundsHandle { handleColor = Color.clear }; + static readonly BoxBoundsHandle s_Plane = new BoxBoundsHandle + { + handleColor = Color.clear, + axes = PrimitiveBoundsHandle.Axes.X | PrimitiveBoundsHandle.Axes.Z + }; + + static readonly Color k_ShapeHandleColor = new Color32(145, 244, 139, 210); + static readonly Color k_ShapeHandleColorDisabled = new Color32(84, 200, 77, 140); + + void OnSceneGUI() + { + if (Event.current.type != EventType.Repaint) + return; + + var shape = target as PhysicsShape; + + shape.GetBakeTransformation(out var linearScalar, out var radiusScalar); + + var handleColor = shape.enabled ? k_ShapeHandleColor : k_ShapeHandleColorDisabled; + var handleMatrix = new float4x4(new RigidTransform(shape.transform.rotation, shape.transform.position)); + using (new Handles.DrawingScope(handleColor, handleMatrix)) + { + switch (shape.ShapeType) + { + case ShapeType.Box: + shape.GetBoxProperties(out var center, out var size, out EulerAngles orientation); + s_Box.ConvexRadius = shape.ConvexRadius * radiusScalar; + s_Box.center = float3.zero; + s_Box.size = size * linearScalar; + using (new Handles.DrawingScope(math.mul(Handles.matrix, float4x4.TRS(center * linearScalar, orientation, 1f)))) + s_Box.DrawHandle(); + break; + case ShapeType.Capsule: + s_Capsule.center = float3.zero; + s_Capsule.height = s_Capsule.radius = 0f; + shape.GetCapsuleProperties(out center, out var height, out var radius, out orientation); + shape.GetCapsuleProperties(out var v0, out var v1, out radius); + var ax = (v0 - v1) * linearScalar; + s_Capsule.height = math.length(ax) + radius * radiusScalar * 2f; + s_Capsule.radius = radius * radiusScalar; + ax = math.normalizesafe(ax, new float3(0f, 0f, 1f)); + var up = math.mul(orientation, math.up()); + var m = float4x4.TRS(center * linearScalar, quaternion.LookRotationSafe(ax, up), 1f); + using (new Handles.DrawingScope(math.mul(Handles.matrix, m))) + s_Capsule.DrawHandle(); + break; + case ShapeType.Sphere: + shape.GetSphereProperties(out center, out radius, out orientation); + s_Sphere.center = float3.zero; + s_Sphere.radius = radius; + using (new Handles.DrawingScope(math.mul(Handles.matrix, float4x4.TRS(center * linearScalar, orientation, radiusScalar)))) + s_Sphere.DrawHandle(); + break; + case ShapeType.Cylinder: + shape.GetCylinderProperties(out center, out height, out radius, out orientation); + s_ConvexCylinder.ConvexRadius = shape.ConvexRadius * radiusScalar; + s_ConvexCylinder.center = float3.zero; + s_ConvexCylinder.Height = height; + s_ConvexCylinder.Radius = radius; + using (new Handles.DrawingScope(math.mul(Handles.matrix, float4x4.TRS(center * linearScalar, orientation, linearScalar)))) + s_ConvexCylinder.DrawHandle(); + break; + case ShapeType.Plane: + shape.GetPlaneProperties(out center, out var size2, out orientation); + s_Plane.center = float3.zero; + s_Plane.size = new float3(size2.x, 0, size2.y) * linearScalar; + using (new Handles.DrawingScope(math.mul(Handles.matrix, float4x4.TRS(center * linearScalar, orientation, 1f)))) + { + Handles.DrawLine( + new float3(0f), + new float3(0f, math.lerp(math.cmax(size2), math.cmin(size2), 0.5f), 0) * 0.5f + ); + s_Plane.DrawHandle(); + } + break; + case ShapeType.ConvexHull: + case ShapeType.Mesh: + if (Event.current.type != EventType.Repaint) + break; + var mesh = GetPreviewMesh(shape); + if (mesh == null || mesh.vertexCount == 0) + break; + var localToWorld = new RigidTransform(shape.transform.rotation, shape.transform.position); + shape.GetBakeTransformation(out linearScalar, out radiusScalar); + DrawMesh(mesh, float4x4.TRS(localToWorld.pos, localToWorld.rot, linearScalar)); + break; + } + } + } + + void DrawMesh(UnityEngine.Mesh mesh, float4x4 localToWorld) + { + if (Event.current.type != EventType.Repaint) + return; + var wireFrame = GL.wireframe; + GL.wireframe = true; + var lighting = Handles.lighting; + Handles.lighting = false; + Shader.SetGlobalColor("_HandleColor", Handles.color); + Shader.SetGlobalFloat("_HandleSize", 1f); + Shader.SetGlobalMatrix("_ObjectToWorld", localToWorld); + m_PreviewMeshMaterial.SetInt("_HandleZTest", (int)Handles.zTest); + m_PreviewMeshMaterial.SetPass(0); + Graphics.DrawMeshNow(mesh, localToWorld); + Handles.lighting = lighting; + GL.wireframe = wireFrame; + } + } +} diff --git a/package/Unity.Physics.Editor/Editors/PhysicsShapeEditor.cs.meta b/package/Unity.Physics.Editor/Editors/PhysicsShapeEditor.cs.meta new file mode 100755 index 000000000..6ba5cec76 --- /dev/null +++ b/package/Unity.Physics.Editor/Editors/PhysicsShapeEditor.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 6b059aae3b9b54e05acdf7b88a1b8447 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Unity.Physics.Editor/PropertyDrawers.meta b/package/Unity.Physics.Editor/PropertyDrawers.meta new file mode 100755 index 000000000..9db7cca16 --- /dev/null +++ b/package/Unity.Physics.Editor/PropertyDrawers.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: 5906842964bca4d179d3cef1d1db2752 +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Unity.Physics.Editor/PropertyDrawers/BaseDrawer.cs b/package/Unity.Physics.Editor/PropertyDrawers/BaseDrawer.cs new file mode 100755 index 000000000..b7d23feb8 --- /dev/null +++ b/package/Unity.Physics.Editor/PropertyDrawers/BaseDrawer.cs @@ -0,0 +1,27 @@ +using UnityEditor; +using UnityEngine; + +namespace Unity.Physics.Editor +{ + abstract class BaseDrawer : PropertyDrawer + { + protected abstract bool IsCompatible(SerializedProperty property); + + public override float GetPropertyHeight(SerializedProperty property, GUIContent label) + { + return IsCompatible(property) + ? EditorGUI.GetPropertyHeight(property) + : EditorGUIUtility.singleLineHeight; + } + + public override void OnGUI(Rect position, SerializedProperty property, GUIContent label) + { + if (IsCompatible(property)) + DoGUI(position, property, label); + else + EditorGUIControls.DisplayCompatibilityWarning(position, label, ObjectNames.NicifyVariableName(GetType().Name)); + } + + protected abstract void DoGUI(Rect position, SerializedProperty property, GUIContent label); + } +} diff --git a/package/Unity.Physics.Editor/PropertyDrawers/BaseDrawer.cs.meta b/package/Unity.Physics.Editor/PropertyDrawers/BaseDrawer.cs.meta new file mode 100755 index 000000000..9a406eb43 --- /dev/null +++ b/package/Unity.Physics.Editor/PropertyDrawers/BaseDrawer.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 259c5757af5a441ddb6fce219e7ee22d +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Unity.Physics.Editor/PropertyDrawers/EnumFlagsDrawer.cs b/package/Unity.Physics.Editor/PropertyDrawers/EnumFlagsDrawer.cs new file mode 100755 index 000000000..d38d0dd47 --- /dev/null +++ b/package/Unity.Physics.Editor/PropertyDrawers/EnumFlagsDrawer.cs @@ -0,0 +1,31 @@ +using System; +using Unity.Physics.Authoring; +using UnityEditor; +using UnityEngine; + +namespace Unity.Physics.Editor +{ + [CustomPropertyDrawer(typeof(EnumFlagsAttribute))] + class EnumFlagsDrawer : BaseDrawer + { + protected override bool IsCompatible(SerializedProperty property) + { + return property.propertyType == SerializedPropertyType.Enum; + } + + protected override void DoGUI(Rect position, SerializedProperty property, GUIContent label) + { + EditorGUI.BeginProperty(position, label, property); + + var value = property.longValue; + EditorGUI.BeginChangeCheck(); + value = Convert.ToInt64( + EditorGUI.EnumFlagsField(position, label, (Enum)Enum.ToObject(fieldInfo.FieldType, value)) + ); + if (EditorGUI.EndChangeCheck()) + property.longValue = value; + + EditorGUI.EndProperty(); + } + } +} \ No newline at end of file diff --git a/package/Unity.Physics.Editor/PropertyDrawers/EnumFlagsDrawer.cs.meta b/package/Unity.Physics.Editor/PropertyDrawers/EnumFlagsDrawer.cs.meta new file mode 100755 index 000000000..02135f942 --- /dev/null +++ b/package/Unity.Physics.Editor/PropertyDrawers/EnumFlagsDrawer.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: feb062e63d134472d97507fdf53a0a03 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Unity.Physics.Editor/PropertyDrawers/EulerAnglesDrawer.cs b/package/Unity.Physics.Editor/PropertyDrawers/EulerAnglesDrawer.cs new file mode 100755 index 000000000..67f4567e0 --- /dev/null +++ b/package/Unity.Physics.Editor/PropertyDrawers/EulerAnglesDrawer.cs @@ -0,0 +1,24 @@ +using Unity.Physics.Authoring; +using UnityEditor; +using UnityEngine; + +namespace Unity.Physics.Editor +{ + [CustomPropertyDrawer(typeof(EulerAngles))] + class EulerAnglesDrawer : BaseDrawer + { + protected override bool IsCompatible(SerializedProperty property) => true; + + public override float GetPropertyHeight(SerializedProperty property, GUIContent label) + { + var value = property.FindPropertyRelative(nameof(EulerAngles.Value)); + return EditorGUI.GetPropertyHeight(value); + } + + protected override void DoGUI(Rect position, SerializedProperty property, GUIContent label) + { + var value = property.FindPropertyRelative(nameof(EulerAngles.Value)); + EditorGUI.PropertyField(position, value, label, true); + } + } +} diff --git a/package/Unity.Physics.Editor/PropertyDrawers/EulerAnglesDrawer.cs.meta b/package/Unity.Physics.Editor/PropertyDrawers/EulerAnglesDrawer.cs.meta new file mode 100755 index 000000000..4ccb47666 --- /dev/null +++ b/package/Unity.Physics.Editor/PropertyDrawers/EulerAnglesDrawer.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 6685365a7ded846d6836b0f4f8c9ce36 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Unity.Physics.Editor/PropertyDrawers/ExpandChildrenDrawer.cs b/package/Unity.Physics.Editor/PropertyDrawers/ExpandChildrenDrawer.cs new file mode 100755 index 000000000..b6012d918 --- /dev/null +++ b/package/Unity.Physics.Editor/PropertyDrawers/ExpandChildrenDrawer.cs @@ -0,0 +1,37 @@ +using Unity.Physics.Authoring; +using UnityEditor; +using UnityEngine; + +namespace Unity.Physics.Editor +{ + [CustomPropertyDrawer(typeof(ExpandChildrenAttribute))] + class ExpandChildrenDrawer : PropertyDrawer + { + public override float GetPropertyHeight(SerializedProperty property, GUIContent label) + { + property.isExpanded = true; + return EditorGUI.GetPropertyHeight(property) + - EditorGUIUtility.standardVerticalSpacing + - EditorGUIUtility.singleLineHeight; + } + + public override void OnGUI(Rect position, SerializedProperty property, GUIContent label) + { + var endProperty = property.GetEndProperty(); + var childProperty = property.Copy(); + childProperty.NextVisible(true); + while (!SerializedProperty.EqualContents(childProperty, endProperty)) + { + position.height = EditorGUI.GetPropertyHeight(childProperty); + OnChildPropertyGUI(position, childProperty); + position.y += position.height + EditorGUIUtility.standardVerticalSpacing; + childProperty.NextVisible(false); + } + } + + protected virtual void OnChildPropertyGUI(Rect position, SerializedProperty childProperty) + { + EditorGUI.PropertyField(position, childProperty, true); + } + } +} diff --git a/package/Unity.Physics.Editor/PropertyDrawers/ExpandChildrenDrawer.cs.meta b/package/Unity.Physics.Editor/PropertyDrawers/ExpandChildrenDrawer.cs.meta new file mode 100755 index 000000000..44fcedc9e --- /dev/null +++ b/package/Unity.Physics.Editor/PropertyDrawers/ExpandChildrenDrawer.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 0f5af7bcfa14d4f7dbc94fc8b718a674 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Unity.Physics.Editor/PropertyDrawers/PhysicsMaterialCoefficientDrawer.cs b/package/Unity.Physics.Editor/PropertyDrawers/PhysicsMaterialCoefficientDrawer.cs new file mode 100755 index 000000000..756ceb255 --- /dev/null +++ b/package/Unity.Physics.Editor/PropertyDrawers/PhysicsMaterialCoefficientDrawer.cs @@ -0,0 +1,41 @@ +using System; +using Unity.Physics.Authoring; +using UnityEditor; +using UnityEngine; + +namespace Unity.Physics.Editor +{ + [CustomPropertyDrawer(typeof(PhysicsMaterialCoefficient))] + class PhysicsMaterialCoefficientDrawer : BaseDrawer + { + static class Styles + { + public const float PopupWidth = 100f; + } + + public override float GetPropertyHeight(SerializedProperty property, GUIContent label) => + EditorGUIUtility.singleLineHeight; + + protected override bool IsCompatible(SerializedProperty property) => true; + + protected override void DoGUI(Rect position, SerializedProperty property, GUIContent label) + { + EditorGUI.BeginProperty(position, label, property); + EditorGUI.PropertyField( + new Rect(position) { xMax = position.xMax - Styles.PopupWidth }, + property.FindPropertyRelative("Value"), + label + ); + + var indent = EditorGUI.indentLevel; + EditorGUI.indentLevel = 0; + EditorGUI.PropertyField( + new Rect(position) { xMin = position.xMax - Styles.PopupWidth + EditorGUIUtility.standardVerticalSpacing }, + property.FindPropertyRelative("CombineMode"), + GUIContent.none + ); + EditorGUI.indentLevel = indent; + EditorGUI.EndProperty(); + } + } +} \ No newline at end of file diff --git a/package/Unity.Physics.Editor/PropertyDrawers/PhysicsMaterialCoefficientDrawer.cs.meta b/package/Unity.Physics.Editor/PropertyDrawers/PhysicsMaterialCoefficientDrawer.cs.meta new file mode 100755 index 000000000..68875799d --- /dev/null +++ b/package/Unity.Physics.Editor/PropertyDrawers/PhysicsMaterialCoefficientDrawer.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: f6db3d3a916df49d1a0d65c3d08bfe8b +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Unity.Physics.Editor/PropertyDrawers/PhysicsMaterialPropertiesDrawer.cs b/package/Unity.Physics.Editor/PropertyDrawers/PhysicsMaterialPropertiesDrawer.cs new file mode 100755 index 000000000..127bd3c06 --- /dev/null +++ b/package/Unity.Physics.Editor/PropertyDrawers/PhysicsMaterialPropertiesDrawer.cs @@ -0,0 +1,295 @@ +using System; +using System.Collections.Generic; +using System.Linq; +using Unity.Physics.Authoring; +using UnityEditor; +using UnityEngine; + +namespace Unity.Physics.Editor +{ + [CustomPropertyDrawer(typeof(PhysicsMaterialProperties))] + class PhysicsMaterialPropertiesDrawer : BaseDrawer, + ICustomOptionNamesProvider, + ICustomOptionNamesProvider + { + static class Content + { + static readonly string DefaultCategoryFormatString = L10n.Tr("(Undefined Category {0})"); + static readonly string DefaultCustomFlagFormatString = L10n.Tr("Custom Flag {0}"); + + public static readonly string[] DefaultCategoriesOptions = + Enumerable.Range(0, 32).Select(i => string.Format(DefaultCategoryFormatString, i)).ToArray(); + public static readonly string[] DefaultCustomFlags = + Enumerable.Range(0, 8).Select(i => string.Format(DefaultCustomFlagFormatString, i)).ToArray(); + + public static readonly GUIContent AdvancedGroupFoldout = EditorGUIUtility.TrTextContent("Advanced"); + public static readonly GUIContent BelongsToLabel = EditorGUIUtility.TrTextContent( + "Belongs To", + "Specifies the categories to which this object belongs." + ); + public static readonly GUIContent CollidesWithLabel = EditorGUIUtility.TrTextContent( + "Collides With", + "Specifies the categories of objects with which this object will collide, " + + "or with which it will raise events if intersecting a trigger." + ); + public static readonly GUIContent CollisionFilterGroupFoldout = + EditorGUIUtility.TrTextContent("Collision Filter"); + public static readonly GUIContent CustomFlagsLabel = + EditorGUIUtility.TrTextContent("Custom Flags", "Specify custom flags to read at run-time."); + public static readonly GUIContent FrictionLabel = EditorGUIUtility.TrTextContent( + "Friction", + "Specifies how resistant the body is to motion when sliding along other surfaces, " + + "as well as what value should be used when colliding with an object that has a different value." + ); + public static readonly GUIContent RaisesCollisionEventsLabel = EditorGUIUtility.TrTextContent( + "Raises Collision Events", + "Specifies whether the shape should raise notifications of collisions with other shapes." + ); + public static readonly GUIContent RestitutionLabel = EditorGUIUtility.TrTextContent( + "Restitution", + "Specifies how bouncy the object will be when colliding with other surfaces, " + + "as well as what value should be used when colliding with an object that has a different value." + ); + public static readonly GUIContent TriggerLabel = EditorGUIUtility.TrTextContent( + "Is Trigger", + "Specifies that the shape is a volume that will raise events when intersecting other shapes, but will not cause a collision response." + ); + } + + string[] ICustomOptionNamesProvider.GetOptions() + { + this.GetOptionsAndNameAssets(ref m_CategoriesOptionNames, ref m_CategoriesAssets, Content.DefaultCategoriesOptions); + return m_CategoriesOptionNames; + } + string[] m_CategoriesOptionNames; + + IReadOnlyList ICustomOptionNamesProvider.NameAssets => m_CategoriesAssets; + PhysicsCategoryNames[] m_CategoriesAssets; + + void ICustomOptionNamesProvider.Update() => m_CategoriesOptionNames = null; + + string[] ICustomOptionNamesProvider.GetOptions() + { + this.GetOptionsAndNameAssets(ref m_CustomFlagsOptionNames, ref m_CustomFlagsAssets, Content.DefaultCustomFlags); + return m_CustomFlagsOptionNames; + } + string[] m_CustomFlagsOptionNames; + + IReadOnlyList ICustomOptionNamesProvider.NameAssets => m_CustomFlagsAssets; + CustomFlagNames[] m_CustomFlagsAssets; + + void ICustomOptionNamesProvider.Update() => m_CustomFlagsOptionNames = null; + + const string k_CollisionFilterGroupKey = "m_BelongsTo"; + const string k_AdvancedGroupKey = "m_RaisesCollisionEvents"; + + Dictionary m_SerializedTemplates = new Dictionary(); + + SerializedProperty GetTemplateValueProperty(SerializedProperty property) + { + var key = property.propertyPath; + var template = property.FindPropertyRelative("m_Template").objectReferenceValue; + SerializedObject serializedTemplate; + if ( + !m_SerializedTemplates.TryGetValue(key, out serializedTemplate) + || serializedTemplate?.targetObject != template + ) + m_SerializedTemplates[key] = serializedTemplate = template == null ? null : new SerializedObject(template); + serializedTemplate?.Update(); + return serializedTemplate?.FindProperty("m_Value"); + } + + void FindToggleAndValueProperties( + SerializedProperty property, SerializedProperty templateValueProperty, string relativePath, + out SerializedProperty toggle, out SerializedProperty value + ) + { + var relative = property.FindPropertyRelative(relativePath); + toggle = relative.FindPropertyRelative("m_Override"); + value = toggle.boolValue || templateValueProperty == null + ? relative.FindPropertyRelative("m_Value") + : templateValueProperty.FindPropertyRelative(relativePath).FindPropertyRelative("m_Value"); + } + + public override float GetPropertyHeight(SerializedProperty property, GUIContent label) + { + var templateValueProperty = GetTemplateValueProperty(property); + + // m_IsTrigger, collision filter foldout, advanced foldout + var height = 3f * EditorGUIUtility.singleLineHeight + 2f * EditorGUIUtility.standardVerticalSpacing; + + // m_BelongsTo, m_CollidesWith + var group = property.FindPropertyRelative(k_CollisionFilterGroupKey); + if (group.isExpanded) + height += 2f * (EditorGUIUtility.singleLineHeight + EditorGUIUtility.standardVerticalSpacing); + + // m_RaisesCollisionEvents, m_CustomFlags + group = property.FindPropertyRelative(k_AdvancedGroupKey); + if (group.isExpanded) + height += 2f * (EditorGUIUtility.singleLineHeight + EditorGUIUtility.standardVerticalSpacing); + + // m_Template + if (property.FindPropertyRelative("m_SupportsTemplate").boolValue) + height += EditorGUIUtility.singleLineHeight + EditorGUIUtility.standardVerticalSpacing; + + // m_Friction, m_Restitution + FindToggleAndValueProperties(property, templateValueProperty, "m_IsTrigger", out _, out var trigger); + if (!trigger.boolValue) + height += 2f * (EditorGUIUtility.singleLineHeight + EditorGUIUtility.standardVerticalSpacing); + + return height; + } + + protected override bool IsCompatible(SerializedProperty property) => true; + + delegate bool DisplayPropertyCallback( + Rect position, SerializedProperty property, GUIContent label, bool includeChildren + ); + + static void DisplayOverridableProperty( + Rect position, GUIContent label, SerializedProperty toggle, SerializedProperty value, bool + templateAssigned, DisplayPropertyCallback drawPropertyField + ) + { + if (templateAssigned) + { + var labelWidth = EditorGUIUtility.labelWidth; + EditorGUIUtility.labelWidth -= 16f; + var togglePosition = new Rect(position) { width = EditorGUIUtility.labelWidth + 16f }; + EditorGUI.PropertyField(togglePosition, toggle, label); + EditorGUIUtility.labelWidth = labelWidth; + + EditorGUI.BeginDisabledGroup(!toggle.boolValue); + var indent = EditorGUI.indentLevel; + EditorGUI.indentLevel = 0; + drawPropertyField( + new Rect(position) { xMin = togglePosition.xMax }, value, GUIContent.none, true + ); + EditorGUI.indentLevel = indent; + EditorGUI.EndDisabledGroup(); + } + else + { + drawPropertyField(position, value, label, true); + } + } + + static void DisplayOverridableProperty( + Rect position, GUIContent label, SerializedProperty toggle, SerializedProperty value, bool templateAssigned + ) + { + DisplayOverridableProperty(position, label, toggle, value, templateAssigned, EditorGUI.PropertyField); + } + + bool CategoriesPopup(Rect position, SerializedProperty property, GUIContent label, bool includeChildren) + { + EditorGUIControls.DoCustomNamesPopup( + position, property, label, this as ICustomOptionNamesProvider + ); + return includeChildren && property.hasChildren && property.isExpanded; + } + + bool FlagsPopup(Rect position, SerializedProperty property, GUIContent label, bool includeChildren) + { + EditorGUIControls.DoCustomNamesPopup( + position, property, label, this as ICustomOptionNamesProvider + ); + return includeChildren && property.hasChildren && property.isExpanded; + } + + protected override void DoGUI(Rect position, SerializedProperty property, GUIContent label) + { + var template = property.FindPropertyRelative("m_Template"); + var templateAssigned = template.objectReferenceValue != null; + var supportsTemplate = property.FindPropertyRelative("m_SupportsTemplate"); + if (supportsTemplate.boolValue) + { + position.height = EditorGUI.GetPropertyHeight(template); + EditorGUI.PropertyField(position, template); + + position.y = position.yMax + EditorGUIUtility.standardVerticalSpacing; + } + + var templateValue = GetTemplateValueProperty(property); + + FindToggleAndValueProperties(property, templateValue, "m_IsTrigger", out var toggle, out var trigger); + position.height = EditorGUIUtility.singleLineHeight; + DisplayOverridableProperty(position, Content.TriggerLabel, toggle, trigger, templateAssigned); + + if (!trigger.boolValue) + { + FindToggleAndValueProperties(property, templateValue, "m_Friction", out toggle, out var friction); + position.y = position.yMax + EditorGUIUtility.standardVerticalSpacing; + position.height = EditorGUIUtility.singleLineHeight; + DisplayOverridableProperty(position, Content.FrictionLabel, toggle, friction, templateAssigned); + + FindToggleAndValueProperties(property, templateValue, "m_Restitution", out toggle, out var restitution); + position.y = position.yMax + EditorGUIUtility.standardVerticalSpacing; + position.height = EditorGUIUtility.singleLineHeight; + DisplayOverridableProperty(position, Content.RestitutionLabel, toggle, restitution, templateAssigned); + } + + // collision filter group + var collisionFilterGroup = property.FindPropertyRelative(k_CollisionFilterGroupKey); + position.y = position.yMax + EditorGUIUtility.standardVerticalSpacing; + position.height = EditorGUIUtility.singleLineHeight; + collisionFilterGroup.isExpanded = + EditorGUI.Foldout(position, collisionFilterGroup.isExpanded, Content.CollisionFilterGroupFoldout); + if (collisionFilterGroup.isExpanded) + { + ++EditorGUI.indentLevel; + + FindToggleAndValueProperties(property, templateValue, "m_BelongsTo", out toggle, out var belongsTo); + position.y = position.yMax + EditorGUIUtility.standardVerticalSpacing; + position.height = EditorGUIUtility.singleLineHeight; + DisplayOverridableProperty( + position, Content.BelongsToLabel, toggle, belongsTo, templateAssigned, CategoriesPopup + ); + + FindToggleAndValueProperties(property, templateValue, "m_CollidesWith", out toggle, out var collidesWith); + position.y = position.yMax + EditorGUIUtility.standardVerticalSpacing; + position.height = EditorGUIUtility.singleLineHeight; + DisplayOverridableProperty( + position, Content.CollidesWithLabel, toggle, collidesWith, templateAssigned, CategoriesPopup + ); + + --EditorGUI.indentLevel; + } + + // advanced group + var advancedGroup = property.FindPropertyRelative(k_AdvancedGroupKey); + position.y = position.yMax + EditorGUIUtility.standardVerticalSpacing; + position.height = EditorGUIUtility.singleLineHeight; + advancedGroup.isExpanded = + EditorGUI.Foldout(position, advancedGroup.isExpanded, Content.AdvancedGroupFoldout); + if (advancedGroup.isExpanded) + { + ++EditorGUI.indentLevel; + + if (!trigger.boolValue) + { + FindToggleAndValueProperties(property, templateValue, "m_RaisesCollisionEvents", out toggle, out var raisesEvents); + position.y = position.yMax + EditorGUIUtility.standardVerticalSpacing; + position.height = EditorGUIUtility.singleLineHeight; + DisplayOverridableProperty( + position, + Content.RaisesCollisionEventsLabel, + toggle, + raisesEvents, + templateAssigned, + (DisplayPropertyCallback)EditorGUI.PropertyField + ); + } + + FindToggleAndValueProperties(property, templateValue, "m_CustomFlags", out toggle, out var customFlags); + position.y = position.yMax + EditorGUIUtility.standardVerticalSpacing; + position.height = EditorGUIUtility.singleLineHeight; + DisplayOverridableProperty( + position, Content.CustomFlagsLabel, toggle, customFlags, templateAssigned, FlagsPopup + ); + + --EditorGUI.indentLevel; + } + } + } +} diff --git a/package/Unity.Physics.Editor/PropertyDrawers/PhysicsMaterialPropertiesDrawer.cs.meta b/package/Unity.Physics.Editor/PropertyDrawers/PhysicsMaterialPropertiesDrawer.cs.meta new file mode 100755 index 000000000..3e489a622 --- /dev/null +++ b/package/Unity.Physics.Editor/PropertyDrawers/PhysicsMaterialPropertiesDrawer.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: ae8a5d9aeb3ea4f4babf68a9f13b35ec +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Unity.Physics.Editor/PropertyDrawers/SoftRangeDrawer.cs b/package/Unity.Physics.Editor/PropertyDrawers/SoftRangeDrawer.cs new file mode 100755 index 000000000..beba93a5f --- /dev/null +++ b/package/Unity.Physics.Editor/PropertyDrawers/SoftRangeDrawer.cs @@ -0,0 +1,23 @@ +using Unity.Physics.Authoring; +using UnityEditor; +using UnityEngine; + +namespace Unity.Physics.Editor +{ + [CustomPropertyDrawer(typeof(SoftRangeAttribute))] + class SoftRangeDrawer : BaseDrawer + { + protected override bool IsCompatible(SerializedProperty property) + { + return property.propertyType == SerializedPropertyType.Float; + } + + protected override void DoGUI(Rect position, SerializedProperty property, GUIContent label) + { + var attr = attribute as SoftRangeAttribute; + EditorGUIControls.SoftSlider( + position, label, property, attr.SliderMin, attr.SliderMax, attr.TextFieldMin, attr.TextFieldMax + ); + } + } +} diff --git a/package/Unity.Physics.Editor/PropertyDrawers/SoftRangeDrawer.cs.meta b/package/Unity.Physics.Editor/PropertyDrawers/SoftRangeDrawer.cs.meta new file mode 100755 index 000000000..ef499c573 --- /dev/null +++ b/package/Unity.Physics.Editor/PropertyDrawers/SoftRangeDrawer.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 66d1f8dbd2117405797f3d5878493cad +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Unity.Physics.Editor/Unity.Physics.Editor.asmdef b/package/Unity.Physics.Editor/Unity.Physics.Editor.asmdef new file mode 100755 index 000000000..83d839c6f --- /dev/null +++ b/package/Unity.Physics.Editor/Unity.Physics.Editor.asmdef @@ -0,0 +1,25 @@ +{ + "name": "Unity.Physics.Editor", + "references": [ + "Unity.Collections", + "Unity.Entities", + "Unity.Entities.Hybrid", + "Unity.Mathematics", + "Unity.Physics", + "Unity.Physics.Authoring", + "Unity.Rendering.Hybrid", + "Unity.Transforms", + "Unity.Transforms.Hybrid" + ], + "optionalUnityReferences": [], + "includePlatforms": [ + "Editor" + ], + "excludePlatforms": [], + "allowUnsafeCode": false, + "overrideReferences": false, + "precompiledReferences": [], + "autoReferenced": true, + "defineConstraints": [], + "versionDefines": [] +} \ No newline at end of file diff --git a/package/Unity.Physics.Editor/Unity.Physics.Editor.asmdef.meta b/package/Unity.Physics.Editor/Unity.Physics.Editor.asmdef.meta new file mode 100755 index 000000000..42bea4930 --- /dev/null +++ b/package/Unity.Physics.Editor/Unity.Physics.Editor.asmdef.meta @@ -0,0 +1,7 @@ +fileFormatVersion: 2 +guid: 3438610b0a83016488db5062ec247a8d +AssemblyDefinitionImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Unity.Physics.Editor/Utilities.meta b/package/Unity.Physics.Editor/Utilities.meta new file mode 100755 index 000000000..3e32a883f --- /dev/null +++ b/package/Unity.Physics.Editor/Utilities.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: a04730b2070d44cd1a3133a8e230ea92 +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Unity.Physics.Editor/Utilities/EditorGUIControls.cs b/package/Unity.Physics.Editor/Utilities/EditorGUIControls.cs new file mode 100755 index 000000000..62925d615 --- /dev/null +++ b/package/Unity.Physics.Editor/Utilities/EditorGUIControls.cs @@ -0,0 +1,202 @@ +using System; +using System.Collections.Generic; +using System.Linq; +using System.Reflection; +using Unity.Mathematics; +using Unity.Physics.Authoring; +using UnityEditor; +using UnityEngine; + +namespace Unity.Physics.Editor +{ + interface ICustomOptionNamesProvider where T : ScriptableObject + { + string[] GetOptions(); + IReadOnlyList NameAssets { get; } + void Update(); + } + + static class CustomNamesProviderExtensions + { + public static void GetOptionsAndNameAssets( + this ICustomOptionNamesProvider provider, + ref string[] optionNames, ref T[] nameAssets, string[] defaultOptions + ) where T : ScriptableObject, IFlagNames + { + if (optionNames != null) + return; + var guids = AssetDatabase.FindAssets($"t:{typeof(T).Name}"); + nameAssets = guids + .Select(AssetDatabase.GUIDToAssetPath) + .Select(AssetDatabase.LoadAssetAtPath) + .Where(c => c != null) + .ToArray(); + optionNames = nameAssets.FirstOrDefault()?.FlagNames.ToArray() ?? defaultOptions; + for (var i = 0; i < optionNames.Length; ++i) + optionNames[i] = string.IsNullOrEmpty(optionNames[i]) ? defaultOptions[i] : optionNames[i]; + } + } + + [InitializeOnLoad] + static class EditorGUIControls + { + static EditorGUIControls() + { + if (k_SoftSlider == null) + Debug.LogException(new MissingMemberException("Could not find expected signature of EditorGUI.Slider() for soft slider.")); + } + + static class Styles + { + public const float CreateAssetButtonWidth = 24; + + public static readonly string CompatibilityWarning = L10n.Tr("Not compatible with {0}."); + public static readonly string CreateAssetButtonTooltip = + L10n.Tr("Click to create a {0} asset and define category names."); + public static readonly string MultipleAssetsTooltip = + L10n.Tr("Multiple {0} assets found. UI will display labels defined in {1}."); + + public static readonly GUIContent CreateAssetLabel = new GUIContent { text = "+" }; + public static readonly GUIContent MultipleAssetsWarning = + new GUIContent { image = EditorGUIUtility.Load("console.warnicon") as Texture }; + } + + public static void DisplayCompatibilityWarning(Rect position, GUIContent label, string incompatibleType) + { + EditorGUI.HelpBox( + EditorGUI.PrefixLabel(position, label), + string.Format(Styles.CompatibilityWarning, incompatibleType), + MessageType.Error + ); + } + + public static void DoCustomNamesPopup( + Rect position, SerializedProperty property, GUIContent label, ICustomOptionNamesProvider optionsProvider + ) where T : ScriptableObject + { + if (optionsProvider.NameAssets?.Count == 0) + position.xMax -= Styles.CreateAssetButtonWidth + EditorGUIUtility.standardVerticalSpacing; + else if (optionsProvider.NameAssets?.Count > 1) + position.xMax -= EditorGUIUtility.singleLineHeight + EditorGUIUtility.standardVerticalSpacing; + + EditorGUI.BeginProperty(position, label, property); + + var controlPosition = EditorGUI.PrefixLabel(position, label); + + var indent = EditorGUI.indentLevel; + EditorGUI.indentLevel = 0; + var showMixed = EditorGUI.showMixedValue; + + var value = 0; + var everything = 0; + for (int i = 0, count = property.arraySize; i < count; ++i) + { + var sp = property.GetArrayElementAtIndex(i); + EditorGUI.showMixedValue |= sp.hasMultipleDifferentValues; + value |= sp.boolValue ? 1 << i : 0; + everything |= 1 << i; + } + // in case size is smaller than 32 + if (value == everything) + value = ~0; + + EditorGUI.BeginChangeCheck(); + value = EditorGUI.MaskField(controlPosition, GUIContent.none, value, optionsProvider.GetOptions()); + if (EditorGUI.EndChangeCheck()) + { + for (int i = 0, count = property.arraySize; i < count; ++i) + property.GetArrayElementAtIndex(i).boolValue = (value & (1 << i)) != 0; + } + + EditorGUI.showMixedValue = showMixed; + EditorGUI.indentLevel = indent; + + EditorGUI.EndProperty(); + + if (optionsProvider.NameAssets?.Count == 0) + { + position.width = Styles.CreateAssetButtonWidth; + position.x = controlPosition.xMax + EditorGUIUtility.standardVerticalSpacing; + Styles.CreateAssetLabel.tooltip = + string.Format(Styles.CreateAssetButtonTooltip, ObjectNames.NicifyVariableName(typeof(T).Name)); + if (GUI.Button(position, Styles.CreateAssetLabel, EditorStyles.miniButton)) + { + var assetPath = + AssetDatabase.GenerateUniqueAssetPath($"Assets/{typeof(T).Name}.asset"); + AssetDatabase.CreateAsset(ScriptableObject.CreateInstance(), assetPath); + Selection.activeObject = AssetDatabase.LoadAssetAtPath(assetPath); + optionsProvider.Update(); + } + } + else if (optionsProvider.NameAssets.Count > 1) + { + var id = GUIUtility.GetControlID(FocusType.Passive); + if (Event.current.type == EventType.Repaint) + { + position.width = EditorGUIUtility.singleLineHeight; + position.x = controlPosition.xMax + EditorGUIUtility.standardVerticalSpacing; + Styles.MultipleAssetsWarning.tooltip = string.Format( + Styles.MultipleAssetsTooltip, + ObjectNames.NicifyVariableName(typeof(T).Name), + optionsProvider.NameAssets.FirstOrDefault(n => n != null)?.name + ); + GUIStyle.none.Draw(position, Styles.MultipleAssetsWarning, id); + } + } + } + + static readonly MethodInfo k_SoftSlider = typeof(EditorGUI).GetMethod( + "Slider", + BindingFlags.Static | BindingFlags.NonPublic, + null, + new[] + { + typeof(Rect), // position + typeof(GUIContent), // label + typeof(float), // value + typeof(float), // sliderMin + typeof(float), // sliderMax + typeof(float), // textFieldMin + typeof(float) // textFieldMax + }, + Array.Empty() + ); + + static readonly object[] k_SoftSliderArgs = new object[7]; + + public static void SoftSlider( + Rect position, GUIContent label, SerializedProperty property, + float sliderMin, float sliderMax, + float textFieldMin, float textFieldMax + ) + { + if (property.propertyType != SerializedPropertyType.Float) + { + DisplayCompatibilityWarning(position, label, property.propertyType.ToString()); + } + else if (k_SoftSlider == null) + { + EditorGUI.BeginChangeCheck(); + EditorGUI.PropertyField(position, property, label); + if (EditorGUI.EndChangeCheck()) + property.floatValue = math.clamp(property.floatValue, textFieldMin, textFieldMax); + } + else + { + k_SoftSliderArgs[0] = position; + k_SoftSliderArgs[1] = label; + k_SoftSliderArgs[2] = property.floatValue; + k_SoftSliderArgs[3] = sliderMin; + k_SoftSliderArgs[4] = sliderMax; + k_SoftSliderArgs[5] = textFieldMin; + k_SoftSliderArgs[6] = textFieldMax; + EditorGUI.BeginProperty(position, label, property); + EditorGUI.BeginChangeCheck(); + var result = k_SoftSlider.Invoke(null, k_SoftSliderArgs); + if (EditorGUI.EndChangeCheck()) + property.floatValue = (float)result; + EditorGUI.EndProperty(); + } + } + } +} \ No newline at end of file diff --git a/package/Unity.Physics.Editor/Utilities/EditorGUIControls.cs.meta b/package/Unity.Physics.Editor/Utilities/EditorGUIControls.cs.meta new file mode 100755 index 000000000..31c4c9db5 --- /dev/null +++ b/package/Unity.Physics.Editor/Utilities/EditorGUIControls.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: abf92f81058974f0982c3044a98a9714 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Unity.Physics.Editor/Utilities/ManipulatorUtility.cs b/package/Unity.Physics.Editor/Utilities/ManipulatorUtility.cs new file mode 100755 index 000000000..4810f3d6b --- /dev/null +++ b/package/Unity.Physics.Editor/Utilities/ManipulatorUtility.cs @@ -0,0 +1,38 @@ +using System; +using Unity.Mathematics; +using UnityEngine; + +namespace Unity.Physics.Editor +{ + enum MatrixState + { + UniformScale, + NonUniformScale, + ZeroScale, + NotValidTRS + } + + static class ManipulatorUtility + { + public static MatrixState GetMatrixState(ref float4x4 localToWorld) + { + if ( + localToWorld.c0.w != 0f + || localToWorld.c1.w != 0f + || localToWorld.c2.w != 0f + || localToWorld.c3.w != 1f + ) + return MatrixState.NotValidTRS; + + var m = new float3x3(localToWorld.c0.xyz, localToWorld.c1.xyz, localToWorld.c2.xyz); + var lossyScale = new float3(math.length(m.c0.xyz), math.length(m.c1.xyz), math.length(m.c2.xyz)); + if (math.determinant(m) < 0f) + lossyScale.x *= -1f; + if (math.lengthsq(lossyScale) == 0f) + return MatrixState.ZeroScale; + return math.abs(math.cmax(lossyScale)) - math.abs(math.cmin(lossyScale)) > 0.000001f + ? MatrixState.NonUniformScale + : MatrixState.UniformScale; + } + } +} diff --git a/package/Unity.Physics.Editor/Utilities/ManipulatorUtility.cs.meta b/package/Unity.Physics.Editor/Utilities/ManipulatorUtility.cs.meta new file mode 100755 index 000000000..e55812faa --- /dev/null +++ b/package/Unity.Physics.Editor/Utilities/ManipulatorUtility.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: e7f1e24965b5e4bdca131f81969355bf +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Unity.Physics.Editor/Utilities/MatrixGUIUtility.cs b/package/Unity.Physics.Editor/Utilities/MatrixGUIUtility.cs new file mode 100755 index 000000000..e3819bd70 --- /dev/null +++ b/package/Unity.Physics.Editor/Utilities/MatrixGUIUtility.cs @@ -0,0 +1,44 @@ +using System.Collections.Generic; +using System.Linq; +using UnityEditor; + +namespace Unity.Physics.Editor +{ + static class MatrixGUIUtility + { + public static MessageType GetMatrixStatusMessage( + IReadOnlyList matrixStates, out string statusMessage + ) + { + statusMessage = string.Empty; + if (matrixStates.Contains(MatrixState.NotValidTRS)) + { + statusMessage = L10n.Tr( + matrixStates.Count == 1 + ? "Target's local-to-world matrix is not a valid transformation." + : "One or more targets' local-to-world matrices are not valid transformations." + ); + return MessageType.Error; + } + + if (matrixStates.Contains(MatrixState.ZeroScale)) + { + statusMessage = + L10n.Tr(matrixStates.Count == 1 ? "Target has zero scale." : "One or more targets has zero scale."); + return MessageType.Warning; + } + + if (matrixStates.Contains(MatrixState.NonUniformScale)) + { + statusMessage = L10n.Tr( + matrixStates.Count == 1 + ? "Target has non-uniform scale, which is not supported at run-time." + : "One or more targets has non-uniform scale, which is not supported at run-time." + ); + return MessageType.Warning; + } + + return MessageType.None; + } + } +} diff --git a/package/Unity.Physics.Editor/Utilities/MatrixGUIUtility.cs.meta b/package/Unity.Physics.Editor/Utilities/MatrixGUIUtility.cs.meta new file mode 100755 index 000000000..cfaef5ce9 --- /dev/null +++ b/package/Unity.Physics.Editor/Utilities/MatrixGUIUtility.cs.meta @@ -0,0 +1,3 @@ +fileFormatVersion: 2 +guid: c7772707c1b94ae295e3c02212d90095 +timeCreated: 1548100553 \ No newline at end of file diff --git a/package/Unity.Physics.meta b/package/Unity.Physics.meta new file mode 100755 index 000000000..86feb203d --- /dev/null +++ b/package/Unity.Physics.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: 9e791559801dc2f4a9e9bb8a46108f7a +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Unity.Physics/AssemblyInfo.cs b/package/Unity.Physics/AssemblyInfo.cs new file mode 100755 index 000000000..53752eeeb --- /dev/null +++ b/package/Unity.Physics/AssemblyInfo.cs @@ -0,0 +1,4 @@ +using System.Runtime.CompilerServices; + +[assembly: InternalsVisibleTo("Unity.Physics.Editor")] +[assembly: InternalsVisibleTo("Unity.Physics.PlayModeTests")] \ No newline at end of file diff --git a/package/Unity.Physics/AssemblyInfo.cs.meta b/package/Unity.Physics/AssemblyInfo.cs.meta new file mode 100755 index 000000000..405000ebb --- /dev/null +++ b/package/Unity.Physics/AssemblyInfo.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 9c385b5fbd362444bbd618a13566348f +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Unity.Physics/Base.meta b/package/Unity.Physics/Base.meta new file mode 100755 index 000000000..1d9e42a10 --- /dev/null +++ b/package/Unity.Physics/Base.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: cd322267fa2826e409316c89d05b8387 +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Unity.Physics/Base/Containers.meta b/package/Unity.Physics/Base/Containers.meta new file mode 100755 index 000000000..168363e87 --- /dev/null +++ b/package/Unity.Physics/Base/Containers.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: 29bc5e9d0c9987647a6932bddc3a6db0 +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Unity.Physics/Base/Containers/BlobArray.cs b/package/Unity.Physics/Base/Containers/BlobArray.cs new file mode 100755 index 000000000..e16a77d65 --- /dev/null +++ b/package/Unity.Physics/Base/Containers/BlobArray.cs @@ -0,0 +1,65 @@ +using System; +using Unity.Collections.LowLevel.Unsafe; + +namespace Unity.Physics +{ + // Non-generic temporary stand-in for Unity BlobArray. + // This is to work around C# wanting to treat any struct containing the generic Unity.BlobArray as a managed struct. + // TODO: Use Unity.Blobs instead + public struct BlobArray + { + internal int Offset; + internal int Length; // number of T, not number of bytes + + // Generic accessor + public unsafe struct Accessor where T : struct + { + private readonly int* m_OffsetPtr; + public int Length { get; private set; } + + public Accessor(ref BlobArray blobArray) + { + fixed (BlobArray* ptr = &blobArray) + { + m_OffsetPtr = &ptr->Offset; + Length = ptr->Length; + } + } + + public ref T this[int index] + { + get + { +#if ENABLE_UNITY_COLLECTIONS_CHECKS + if ((uint)index >= (uint)Length) + throw new IndexOutOfRangeException(string.Format("Index {0} is out of range Length {1}", index, Length)); +#endif + return ref UnsafeUtilityEx.ArrayElementAsRef((byte*)m_OffsetPtr + *m_OffsetPtr, index); + } + } + + public Enumerator GetEnumerator() => new Enumerator(m_OffsetPtr, Length); + + public struct Enumerator + { + private readonly int* m_OffsetPtr; + private readonly int m_Length; + private int m_Index; + + public T Current => UnsafeUtilityEx.ArrayElementAsRef((byte*)m_OffsetPtr + *m_OffsetPtr, m_Index); + + public Enumerator(int* offsetPtr, int length) + { + m_OffsetPtr = offsetPtr; + m_Length = length; + m_Index = -1; + } + + public bool MoveNext() + { + return ++m_Index < m_Length; + } + } + } + } +} diff --git a/package/Unity.Physics/Base/Containers/BlobArray.cs.meta b/package/Unity.Physics/Base/Containers/BlobArray.cs.meta new file mode 100755 index 000000000..d594b813c --- /dev/null +++ b/package/Unity.Physics/Base/Containers/BlobArray.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 49d0dc3a3bc127147b3420421377e499 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Unity.Physics/Base/Containers/BlockStream.cs b/package/Unity.Physics/Base/Containers/BlockStream.cs new file mode 100755 index 000000000..aa61ed167 --- /dev/null +++ b/package/Unity.Physics/Base/Containers/BlockStream.cs @@ -0,0 +1,594 @@ +//#define BLOCK_STREAM_DEBUG + +using System; +using System.Diagnostics; +using Unity.Burst; +using Unity.Collections.LowLevel.Unsafe; +using Unity.Jobs; +using Unity.Jobs.LowLevel.Unsafe; +using UnityEngine.Assertions; + +namespace Unity.Collections +{ + // A linked list of fixed size memory blocks for writing arbitrary data to. + [NativeContainer] + public unsafe struct BlockStream : IDisposable + { + [NativeDisableUnsafePtrRestriction] BlockStreamData* m_Block; + private readonly Allocator m_AllocatorLabel; + + // Unique "guid" style int used to identify instance of BlockStream for debugging purposes. + private readonly uint m_UniqueBlockStreamId; + +#if ENABLE_UNITY_COLLECTIONS_CHECKS + private AtomicSafetyHandle m_Safety; + [NativeSetClassTypeToNullOnSchedule] + private DisposeSentinel m_DisposeSentinel; +#endif + + public BlockStream(int foreachCount, uint uniqueBlockStreamId, Allocator allocator = Allocator.TempJob) + { +#if ENABLE_UNITY_COLLECTIONS_CHECKS + if (foreachCount <= 0) + throw new ArgumentException("foreachCount must be > 0", "foreachCount"); + if (allocator <= Allocator.None) + throw new ArgumentException("Allocator must be Temp, TempJob or Persistent", "allocator"); +#endif + + m_Block = null; + m_AllocatorLabel = allocator; + m_UniqueBlockStreamId = uniqueBlockStreamId; + + int blockCount = JobsUtility.MaxJobThreadCount; + + int allocationSize = sizeof(BlockStreamData) + sizeof(Block*) * blockCount + sizeof(Range) * foreachCount; + byte* buffer = (byte*)UnsafeUtility.Malloc(allocationSize, 16, m_AllocatorLabel); + UnsafeUtility.MemClear(buffer, allocationSize); + + m_Block = (BlockStreamData*)buffer; + m_Block->Allocator = m_AllocatorLabel; + m_Block->BlockCount = blockCount; + m_Block->Blocks = (Block**)(buffer + sizeof(BlockStreamData)); + + m_Block->Ranges = (Range*)((byte*)m_Block->Blocks + sizeof(Block*) * blockCount); + m_Block->RangeCount = foreachCount; + +#if ENABLE_UNITY_COLLECTIONS_CHECKS + DisposeSentinel.Create(out m_Safety, out m_DisposeSentinel, 0, m_AllocatorLabel); +#endif + } + + public bool IsCreated => m_Block != null; + + public int ForEachCount + { + get + { + CheckReadAccess(); + return m_Block->RangeCount; + } + } + + [Conditional("BLOCK_STREAM_DEBUG")] + void CheckReadAccess() + { +#if ENABLE_UNITY_COLLECTIONS_CHECKS + AtomicSafetyHandle.CheckReadAndThrow(m_Safety); +#endif + } + + [Conditional("BLOCK_STREAM_DEBUG")] + void CheckWriteAccess() + { +#if ENABLE_UNITY_COLLECTIONS_CHECKS + AtomicSafetyHandle.CheckWriteAndThrow(m_Safety); +#endif + } + + public int ComputeItemCount() + { + CheckReadAccess(); + + int itemCount = 0; + + for (int i = 0; i != m_Block->RangeCount; i++) + { + itemCount += m_Block->Ranges[i].Count; + } + + return itemCount; + } + + public NativeArray ToNativeArray(Allocator allocator = Allocator.Temp) where T : struct + { + CheckReadAccess(); + + var array = new NativeArray(ComputeItemCount(), allocator, NativeArrayOptions.UninitializedMemory); + Reader reader = this; + + int offset = 0; + for (int i = 0; i != reader.ForEachCount; i++) + { + reader.BeginForEachIndex(i); + int rangeItemCount = reader.RemainingItemCount; + for (int j = 0; j < rangeItemCount; ++j) + { + array[offset] = reader.Read(); + offset++; + } + } + + return array; + } + + private void _Dispose() + { + if (m_Block == null) + { + return; + } + + for (int i = 0; i != m_Block->BlockCount; i++) + { + Block* block = m_Block->Blocks[i]; + while (block != null) + { + Block* next = block->Next; + UnsafeUtility.Free(block, m_AllocatorLabel); + block = next; + } + } + + UnsafeUtility.Free(m_Block, m_AllocatorLabel); + m_Block = null; + } + + public void Dispose() + { +#if ENABLE_UNITY_COLLECTIONS_CHECKS + DisposeSentinel.Dispose(ref m_Safety, ref m_DisposeSentinel); +#endif + _Dispose(); + } + + public JobHandle ScheduleDispose(JobHandle inputDeps) + { + // [DeallocateOnJobCompletion] is not supported, but we want the deallocation to happen in a thread. + // DisposeSentinel needs to be cleared on main thread. + // AtomicSafetyHandle can be destroyed after the job was scheduled +#if ENABLE_UNITY_COLLECTIONS_CHECKS + DisposeSentinel.Clear(ref m_DisposeSentinel); +#endif + var jobHandle = new DisposeJob { BlockStream = this }.Schedule(inputDeps); + +#if ENABLE_UNITY_COLLECTIONS_CHECKS + AtomicSafetyHandle.Release(m_Safety); +#endif + + m_Block = null; + + return jobHandle; + } + + public struct Range + { + public Block* Block; + public int Offset; + public int Count; + + /// One byte past the end of the last byte written + public int LastOffset; + } + + public Range* GetRangeRW(int forEachIndex) + { + CheckWriteAccess(); + + return m_Block->Ranges + forEachIndex; + } + + public struct BlockStreamData + { + public const int AllocationSize = 4 * 1024; + public Allocator Allocator; + + public Block** Blocks; + public int BlockCount; + + public Range* Ranges; + public int RangeCount; + + public Block* Allocate(Block* oldBlock, int threadIndex) + { + Assert.IsTrue(threadIndex < BlockCount && threadIndex >= 0); + + Block* block = (Block*)UnsafeUtility.Malloc(AllocationSize, 16, Allocator); + block->Next = null; + + if (oldBlock == null) + { + if (Blocks[threadIndex] == null) + Blocks[threadIndex] = block; + else + { + // Walk the linked list and append our new block to the end. + // Otherwise, we leak memory. + Block* head = Blocks[threadIndex]; + while (head->Next != null) + { + head = head->Next; + } + head->Next = block; + } + } + else + { + oldBlock->Next = block; + } + + return block; + } + } + + public struct Block + { + public Block* Next; + + public fixed byte Data[1]; + } + + [NativeContainer] + //@TODO: Try to use min / max check instead... + [NativeContainerSupportsMinMaxWriteRestriction] + public struct Writer + { + [NativeDisableUnsafePtrRestriction] + BlockStreamData* m_BlockStream; + [NativeDisableUnsafePtrRestriction] + Block* m_CurrentBlock; + [NativeDisableUnsafePtrRestriction] + byte* m_CurrentPtr; + [NativeDisableUnsafePtrRestriction] + byte* m_CurrentBlockEnd; + + int m_ForeachIndex; + int m_Count; + + [NativeDisableUnsafePtrRestriction] + Block* m_FirstBlock; + int m_FirstOffset; + +#pragma warning disable CS0649 + [NativeSetThreadIndex] + int m_ThreadIndex; +#pragma warning restore CS0649 + +#if ENABLE_UNITY_COLLECTIONS_CHECKS + private AtomicSafetyHandle m_Safety; + int m_Length; + int m_MinIndex; + int m_MaxIndex; +#endif + + + public Writer(BlockStream stream) + { + this = stream; + } + + public static implicit operator Writer(BlockStream stream) + { + var writer = new Writer(); + writer.m_BlockStream = stream.m_Block; + writer.m_ForeachIndex = -1; + writer.m_Count = -1; +#if ENABLE_UNITY_COLLECTIONS_CHECKS + writer.m_Safety = stream.m_Safety; + writer.m_Length = stream.ForEachCount; + writer.m_MinIndex = 0; + writer.m_MaxIndex = stream.ForEachCount - 1; +#endif + return writer; + } + + public int ForEachCount + { + get + { + CheckAccess(); + + return m_BlockStream->RangeCount; + } + } + + [Conditional("BLOCK_STREAM_DEBUG")] + void CheckAccess() + { + #if ENABLE_UNITY_COLLECTIONS_CHECKS + AtomicSafetyHandle.CheckWriteAndThrow(m_Safety); + #endif + } + + public void BeginForEachIndex(int foreachIndex) + { + +#if BLOCK_STREAM_DEBUG + if (foreachIndex < m_MinIndex || foreachIndex > m_MaxIndex) + throw new ArgumentException($"Index {foreachIndex} is out of restricted IJobParallelFor range [{m_MinIndex}...{m_MaxIndex}] in BlockStream."); + Assert.IsTrue(foreachIndex >= 0 && foreachIndex < m_BlockStream->RangeCount); + Assert.AreEqual(-1, m_ForeachIndex); + Assert.AreEqual(0, m_BlockStream->Ranges[foreachIndex].Count); +#endif + + m_ForeachIndex = foreachIndex; + m_Count = 0; + m_FirstBlock = m_CurrentBlock; + m_FirstOffset = (int)(m_CurrentPtr - (byte*)m_CurrentBlock); + } + + public void AppendForEachIndex(int foreachIndex) + { + CheckAccess(); +#if ENABLE_UNITY_COLLECTIONS_CHECKS + if (foreachIndex < m_MinIndex || foreachIndex > m_MaxIndex) + throw new System.ArgumentException($"forEachIndex {foreachIndex} is not in the allowed range"); +#endif + +#if BLOCK_STREAM_DEBUG + Assert.IsTrue(foreachIndex >= 0 && foreachIndex < m_BlockStream->RangeCount); + Assert.AreEqual(-1, m_ForeachIndex); +#endif + + m_ForeachIndex = foreachIndex; + m_Count = m_BlockStream->Ranges[foreachIndex].Count; + m_FirstOffset = m_BlockStream->Ranges[foreachIndex].Offset; + m_FirstBlock = m_BlockStream->Ranges[foreachIndex].Block; + + if (m_Count > 0) + { + m_CurrentBlock = m_FirstBlock; + while (m_CurrentBlock->Next != null) + { + m_CurrentBlock = m_CurrentBlock->Next; + } + + m_CurrentPtr = (byte*)m_CurrentBlock + m_BlockStream->Ranges[foreachIndex].LastOffset; + m_CurrentBlockEnd = (byte*)m_CurrentBlock + BlockStreamData.AllocationSize; + } + else + { + Block* allocatedBlock = m_BlockStream->Blocks[m_ThreadIndex]; + if (allocatedBlock != null) + { + // This can happen if a job was generated for this work item but didn't produce any + // contacts - the range will be empty, but a block will have been preemptively allocated + m_FirstBlock = allocatedBlock; + m_CurrentBlock = m_FirstBlock; + m_CurrentPtr = m_CurrentBlock->Data; + m_FirstOffset = (int)(m_CurrentPtr - (byte*)m_CurrentBlock); + m_CurrentBlockEnd = (byte*)m_CurrentBlock + BlockStreamData.AllocationSize; + } + } + } + + public void EndForEachIndex() + { +#if BLOCK_STREAM_DEBUG + Assert.AreNotEqual(-1, m_ForeachIndex); +#endif + CheckAccess(); + + m_BlockStream->Ranges[m_ForeachIndex].Count = m_Count; + m_BlockStream->Ranges[m_ForeachIndex].Offset = m_FirstOffset; + m_BlockStream->Ranges[m_ForeachIndex].Block = m_FirstBlock; + + m_BlockStream->Ranges[m_ForeachIndex].LastOffset = (int)(m_CurrentPtr - (byte*)m_CurrentBlock); + +#if BLOCK_STREAM_DEBUG + m_ForeachIndex = -1; +#endif + } + + public void Write(T value) where T : struct + { + ref T dst = ref Allocate(); + dst = value; + } + + //@TODO: Array allocate + + public ref T Allocate() where T : struct + { + int size = UnsafeUtility.SizeOf(); + return ref UnsafeUtilityEx.AsRef(Allocate(size)); + } + + //@TODO: Make private or remove it. + public byte* Allocate(int size) + { + CheckAccess(); +#if BLOCK_STREAM_DEBUG + if( m_ForeachIndex == -1 ) + throw new InvalidOperationException("Allocate must be called within BeginForEachIndex / EndForEachIndex"); + if(size > BlockStreamData.AllocationSize - sizeof(void*)) + throw new InvalidOperationException("Allocation size is too large"); +#endif + + byte* ptr = m_CurrentPtr; + m_CurrentPtr += size; + + if (m_CurrentPtr > m_CurrentBlockEnd) + { + Block* oldBlock = m_CurrentBlock; + + m_CurrentBlock = m_BlockStream->Allocate(oldBlock, m_ThreadIndex); + m_CurrentPtr = m_CurrentBlock->Data; + + if (m_FirstBlock == null) + { + m_FirstOffset = (int)(m_CurrentPtr - (byte*)m_CurrentBlock); + m_FirstBlock = m_CurrentBlock; + } + + m_CurrentBlockEnd = (byte*)m_CurrentBlock + BlockStreamData.AllocationSize; + ptr = m_CurrentPtr; + m_CurrentPtr += size; + } + + m_Count++; + + return ptr; + } + } + + [NativeContainer] + [NativeContainerIsReadOnly] + public struct Reader + { + [NativeDisableUnsafePtrRestriction] + BlockStreamData* m_BlockStream; + [NativeDisableUnsafePtrRestriction] + Block* m_CurrentBlock; + [NativeDisableUnsafePtrRestriction] + byte* m_CurrentPtr; + [NativeDisableUnsafePtrRestriction] + byte* m_CurrentBlockEnd; + [NativeDisableUnsafePtrRestriction] + byte* m_LastPtr; // The memory returned by the previous Read(). Used by Write() // + /// + /// + /// + /// The number of elements at this index + public int BeginForEachIndex(int foreachIndex) + { + CheckAccess(); + + m_CurrentBlock = m_BlockStream->Ranges[foreachIndex].Block; + m_CurrentPtr = (byte*)m_CurrentBlock + m_BlockStream->Ranges[foreachIndex].Offset; + m_CurrentBlockEnd = (byte*)m_CurrentBlock + BlockStreamData.AllocationSize; + + m_RemainingItemCount = m_BlockStream->Ranges[foreachIndex].Count; + + return m_RemainingItemCount; + } + + public int ForEachCount => m_BlockStream->RangeCount; + + public int RemainingItemCount => m_RemainingItemCount; + + public byte* Read(int size) + { + CheckAccess(); + + m_RemainingItemCount--; + + byte* ptr = m_CurrentPtr; + m_CurrentPtr += size; + + if (m_CurrentPtr > m_CurrentBlockEnd) + { + m_CurrentBlock = m_CurrentBlock->Next; + m_CurrentPtr = m_CurrentBlock->Data; + m_CurrentBlockEnd = (byte*)m_CurrentBlock + BlockStreamData.AllocationSize; + + ptr = m_CurrentPtr; + m_CurrentPtr += size; + } + + m_LastPtr = ptr; + return ptr; + } + + public ref T Read() where T : struct + { + int size = UnsafeUtility.SizeOf(); + +#if BLOCK_STREAM_DEBUG + Assert.IsTrue(size <= BlockStreamData.AllocationSize - (sizeof(void*))); + Assert.IsTrue(RemainingItemCount >= 1); +#endif + + return ref UnsafeUtilityEx.AsRef(Read(size)); + } + + public ref T Peek() where T : struct + { + CheckAccess(); + + int size = UnsafeUtility.SizeOf(); + +#if BLOCK_STREAM_DEBUG + size += sizeof(int); + Assert.IsTrue(size <= BlockStreamData.AllocationSize - (sizeof(void*))); + Assert.IsTrue(m_RemainingItemCount >= 1); +#endif + + byte* ptr = m_CurrentPtr; + + if (ptr + size > m_CurrentBlockEnd) + ptr = m_CurrentBlock->Next->Data; + +#if BLOCK_STREAM_DEBUG + ptr += sizeof(int); +#endif + + return ref UnsafeUtilityEx.AsRef(ptr); + } + + + + //LETS FIX THIS!!!!! + + //(T d) where T : struct + { + ref T prev = ref UnsafeUtilityEx.AsRef(m_LastPtr); + prev = d; + } + + } + + [BurstCompile] + private struct DisposeJob : IJob + { + public BlockStream BlockStream; + + public void Execute() + { + BlockStream._Dispose(); + } + } + } +} diff --git a/package/Unity.Physics/Base/Containers/BlockStream.cs.meta b/package/Unity.Physics/Base/Containers/BlockStream.cs.meta new file mode 100755 index 000000000..c41b81dd5 --- /dev/null +++ b/package/Unity.Physics/Base/Containers/BlockStream.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: c85764b393ecf0b4a8648fc31266fd21 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Unity.Physics/Base/Containers/ElementPool.cs b/package/Unity.Physics/Base/Containers/ElementPool.cs new file mode 100755 index 000000000..9f4bcb132 --- /dev/null +++ b/package/Unity.Physics/Base/Containers/ElementPool.cs @@ -0,0 +1,197 @@ +using System; +using System.Runtime.CompilerServices; +using Unity.Collections.LowLevel.Unsafe; +using UnityEngine.Assertions; + +namespace Unity.Collections +{ + public interface IPoolElement + { + bool IsAllocated { get; } + void MarkFree(int nextFree); + int NextFree { get; } + } + + // A fixed capacity array acting as a pool of allocated/free structs referenced by indices + public struct ElementPool : IDisposable where T : struct, IPoolElement + { + private NativeArray m_Elements; // storage for all elements (allocated and free) + private readonly bool m_DisposeArray; // whether to dispose the storage on destruction + private int m_FirstFreeIndex; // the index of the first free element (or -1 if none free) + + public int Capacity => m_Elements.Length; // the maximum number of elements that can be allocated + public int PeakCount { get; private set; } // the maximum number of elements allocated so far + + public ElementPool(int capacity, Allocator allocator) + { + m_Elements = new NativeArray(capacity, allocator); + m_DisposeArray = true; + m_FirstFreeIndex = -1; + PeakCount = 0; + } + + public unsafe ElementPool(void* userBuffer, int capacity) + { + m_Elements = NativeArrayUnsafeUtility.ConvertExistingDataToNativeArray(userBuffer, capacity, Allocator.None); +#if ENABLE_UNITY_COLLECTIONS_CHECKS + NativeArrayUnsafeUtility.SetAtomicSafetyHandle(ref m_Elements, AtomicSafetyHandle.GetTempUnsafePtrSliceHandle()); +#endif + m_DisposeArray = false; + m_FirstFreeIndex = -1; + PeakCount = 0; + } + + // Add an element to the pool + public int Allocate(T element) + { + Assert.IsTrue(element.IsAllocated); + if (m_FirstFreeIndex != -1) + { + int index = m_FirstFreeIndex; + m_FirstFreeIndex = m_Elements[index].NextFree; + m_Elements[index] = element; + return index; + } + + Assert.IsTrue(PeakCount < m_Elements.Length); + m_Elements[PeakCount++] = element; + return PeakCount - 1; + } + + // Remove an element from the pool + public void Release(int index) + { + T element = m_Elements[index]; + element.MarkFree(m_FirstFreeIndex); + m_Elements[index] = element; + m_FirstFreeIndex = index; + } + + // Empty the pool + public void Clear() + { + PeakCount = 0; + m_FirstFreeIndex = -1; + } + + // Get/set an element + public T this[int index] + { + get { T element = m_Elements[index]; Assert.IsTrue(element.IsAllocated); return element; } + set { m_Elements[index] = value; } + } + + // Get the first allocated index + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public int GetFirstIndex() + { + return GetNextIndex(-1); + } + + // Get the next allocated index + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public int GetNextIndex(int index) + { + for (++index; index < PeakCount; ++index) + { + if (m_Elements[index].IsAllocated) + { + return index; + } + } + return -1; + } + + public unsafe void CopyFrom(ElementPool other) + { + Assert.IsTrue(other.m_Elements.Length <= Capacity); + PeakCount = other.PeakCount; + m_FirstFreeIndex = other.m_FirstFreeIndex; + UnsafeUtility.MemCpy(m_Elements.GetUnsafePtr(), other.m_Elements.GetUnsafePtr(), PeakCount * UnsafeUtility.SizeOf()); + } + + public unsafe void CopyFrom(void* buffer, int length) + { + Assert.IsTrue(length <= Capacity); + PeakCount = length; + m_FirstFreeIndex = -1; + UnsafeUtility.MemCpy(m_Elements.GetUnsafePtr(), buffer, PeakCount * UnsafeUtility.SizeOf()); + } + + public void Dispose() + { + if (m_DisposeArray) + { + m_Elements.Dispose(); + } + } + + #region Enumerables + + public IndexEnumerable Indices => new IndexEnumerable { Slice = new NativeSlice(m_Elements, 0, PeakCount) }; + public ElementEnumerable Elements => new ElementEnumerable { Slice = new NativeSlice(m_Elements, 0, PeakCount) }; + + public struct IndexEnumerable + { + internal NativeSlice Slice; + + public IndexEnumerator GetEnumerator() => new IndexEnumerator(ref Slice); + } + + public struct ElementEnumerable + { + internal NativeSlice Slice; + + public ElementEnumerator GetEnumerator() => new ElementEnumerator(ref Slice); + } + + // An enumerator for iterating over the indices + public struct IndexEnumerator + { + internal NativeSlice Slice; + internal int Index; + + public int Current => Index; + + internal IndexEnumerator(ref NativeSlice slice) + { + Slice = slice; + Index = -1; + } + + public bool MoveNext() + { + while (true) + { + if (++Index >= Slice.Length) + { + return false; + } + if (Slice[Index].IsAllocated) + { + return true; + } + } + } + } + + // An enumerator for iterating over the allocated elements + public struct ElementEnumerator + { + internal NativeSlice Slice; + internal IndexEnumerator IndexEnumerator; + + public T Current => Slice[IndexEnumerator.Current]; + + internal ElementEnumerator(ref NativeSlice slice) + { + Slice = slice; + IndexEnumerator = new IndexEnumerator(ref slice); + } + + public bool MoveNext() => IndexEnumerator.MoveNext(); + } + + #endregion + } +} diff --git a/package/Unity.Physics/Base/Containers/ElementPool.cs.meta b/package/Unity.Physics/Base/Containers/ElementPool.cs.meta new file mode 100755 index 000000000..ef9fe189d --- /dev/null +++ b/package/Unity.Physics/Base/Containers/ElementPool.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 5187615741f91bd4b80ca2007f9bc579 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Unity.Physics/Base/Math.meta b/package/Unity.Physics/Base/Math.meta new file mode 100755 index 000000000..d81ec1c7d --- /dev/null +++ b/package/Unity.Physics/Base/Math.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: 9c6f4b21fb3059b40afab58fe2bb229b +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Unity.Physics/Base/Math/Aabb.cs b/package/Unity.Physics/Base/Math/Aabb.cs new file mode 100755 index 000000000..52e3f8d56 --- /dev/null +++ b/package/Unity.Physics/Base/Math/Aabb.cs @@ -0,0 +1,129 @@ +using System; +using System.Diagnostics; +using System.Runtime.CompilerServices; +using Unity.Mathematics; + +namespace Unity.Physics +{ + // An axis aligned bounding box + [DebuggerDisplay("{Min} - {Max}")] + [Serializable] + public struct Aabb + { + public float3 Min; + public float3 Max; + + public float3 Extents => Max - Min; + public float3 Center => (Max + Min) * 0.5f; + public bool IsValid => math.all(Min <= Max); + + // Create an empty, invalid AABB + public static readonly Aabb Empty = new Aabb { Min = Math.Constants.Max3F, Max = Math.Constants.Min3F }; + + public float SurfaceArea + { + get + { + float3 diff = Max - Min; + return 2 * math.dot(diff, diff.yzx); + } + } + + public static Aabb Union(Aabb a, Aabb b) + { + a.Include(b); + return a; + } + + [DebuggerStepThrough] + public void Include(float3 point) + { + Min = math.min(Min, point); + Max = math.max(Max, point); + } + + [DebuggerStepThrough] + public void Include(Aabb aabb) + { + Min = math.min(Min, aabb.Min); + Max = math.max(Max, aabb.Max); + } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public bool Contains(float3 point) => math.all(point >= Min & point <= Max); + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public bool Contains(Aabb aabb) => math.all((Min <= aabb.Min) & (Max >= aabb.Max)); + + public void Expand(float distance) + { + Min -= distance; + Max += distance; + } + + public static Aabb CreateFromPoints(float3x4 points) + { + Aabb aabb; + aabb.Min = points.c0; + aabb.Max = aabb.Min; + + aabb.Min = math.min(aabb.Min, points.c1); + aabb.Max = math.max(aabb.Max, points.c1); + + aabb.Min = math.min(aabb.Min, points.c2); + aabb.Max = math.max(aabb.Max, points.c2); + + aabb.Min = math.min(aabb.Min, points.c3); + aabb.Max = math.max(aabb.Max, points.c3); + + return aabb; + } + + public bool Overlaps(Aabb other) + { + return math.all(Max >= other.Min & Min <= other.Max); + } + } + + // Helper functions + public static partial class Math + { + // Transform an AABB into another space, expanding it as needed. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Aabb TransformAabb(RigidTransform transform, Aabb aabb) + { + float3 halfExtentsInA = aabb.Extents * 0.5f; + float3 x = math.rotate(transform.rot, new float3(halfExtentsInA.x, 0, 0)); + float3 y = math.rotate(transform.rot, new float3(0, halfExtentsInA.y, 0)); + float3 z = math.rotate(transform.rot, new float3(0, 0, halfExtentsInA.z)); + + float3 halfExtentsInB = math.abs(x) + math.abs(y) + math.abs(z); + float3 centerInB = math.transform(transform, aabb.Center); + + return new Aabb + { + Min = centerInB - halfExtentsInB, + Max = centerInB + halfExtentsInB + }; + } + + // Transform an AABB into another space, expanding it as needed. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Aabb TransformAabb(MTransform transform, Aabb aabb) + { + float3 halfExtentsInA = aabb.Extents * 0.5f; + float3 transformedX = math.abs(transform.Rotation.c0 * halfExtentsInA.x); + float3 transformedY = math.abs(transform.Rotation.c1 * halfExtentsInA.y); + float3 transformedZ = math.abs(transform.Rotation.c2 * halfExtentsInA.z); + + float3 halfExtentsInB = transformedX + transformedY + transformedZ; + float3 centerInB = Math.Mul(transform, aabb.Center); + + return new Aabb + { + Min = centerInB - halfExtentsInB, + Max = centerInB + halfExtentsInB + }; + } + } +} diff --git a/package/Unity.Physics/Base/Math/Aabb.cs.meta b/package/Unity.Physics/Base/Math/Aabb.cs.meta new file mode 100755 index 000000000..2f1d172ec --- /dev/null +++ b/package/Unity.Physics/Base/Math/Aabb.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 9208347103400a44ea72e5751c586b5b +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Unity.Physics/Base/Math/FourTransposedAabbs.cs b/package/Unity.Physics/Base/Math/FourTransposedAabbs.cs new file mode 100755 index 000000000..d07e2f711 --- /dev/null +++ b/package/Unity.Physics/Base/Math/FourTransposedAabbs.cs @@ -0,0 +1,143 @@ +using System; +using Unity.Mathematics; + +namespace Unity.Physics +{ + // 4 transposed AABBs + public struct FourTransposedAabbs + { + public float4 Lx, Hx; // Lower and upper bounds along the X axis. + public float4 Ly, Hy; // Lower and upper bounds along the Y axis. + public float4 Lz, Hz; // Lower and upper bounds along the Z axis. + + public static FourTransposedAabbs Empty => new FourTransposedAabbs + { + Lx = new float4(float.MaxValue), + Hx = new float4(float.MinValue), + Ly = new float4(float.MaxValue), + Hy = new float4(float.MinValue), + Lz = new float4(float.MaxValue), + Hz = new float4(float.MinValue) + }; + + public void SetAllAabbs(Aabb aabb) + { + Lx = new float4(aabb.Min.x); + Ly = new float4(aabb.Min.y); + Lz = new float4(aabb.Min.z); + Hx = new float4(aabb.Max.x); + Hy = new float4(aabb.Max.y); + Hz = new float4(aabb.Max.z); + } + + public void SetAabb(int index, Aabb aabb) + { + Lx[index] = aabb.Min.x; + Hx[index] = aabb.Max.x; + + Ly[index] = aabb.Min.y; + Hy[index] = aabb.Max.y; + + Lz[index] = aabb.Min.z; + Hz[index] = aabb.Max.z; + } + + public Aabb GetAabb(int index) => new Aabb + { + Min = new float3(Lx[index], Ly[index], Lz[index]), + Max = new float3(Hx[index], Hy[index], Hz[index]) + }; + + public FourTransposedAabbs GetAabbT(int index) => new FourTransposedAabbs + { + Lx = new float4(Lx[index]), + Ly = new float4(Ly[index]), + Lz = new float4(Lz[index]), + Hx = new float4(Hx[index]), + Hy = new float4(Hy[index]), + Hz = new float4(Hz[index]) + }; + + public Aabb GetCompoundAabb() => new Aabb + { + Min = new float3(math.cmin(Lx), math.cmin(Ly), math.cmin(Lz)), + Max = new float3(math.cmax(Hx), math.cmax(Hy), math.cmax(Hz)) + }; + + public bool4 Overlap1Vs4(ref FourTransposedAabbs aabbT) + { + bool4 lc = (aabbT.Lx <= Hx) & (aabbT.Ly <= Hy) & (aabbT.Lz <= Hz); + bool4 hc = (aabbT.Hx >= Lx) & (aabbT.Hy >= Ly) & (aabbT.Hz >= Lz); + bool4 c = lc & hc; + return c; + } + + public bool4 Overlap1Vs4(ref FourTransposedAabbs other, int index) + { + FourTransposedAabbs aabbT = other.GetAabbT(index); + return Overlap1Vs4(ref aabbT); + } + + public float4 DistanceFromPointSquared(ref Math.FourTransposedPoints tranposedPoint) + { + float4 px = math.max(tranposedPoint.X, Lx); + px = math.min(px, Hx) - tranposedPoint.X; + + float4 py = math.max(tranposedPoint.Y, Ly); + py = math.min(py, Hy) - tranposedPoint.Y; + + float4 pz = math.max(tranposedPoint.Z, Lz); + pz = math.min(pz, Hz) - pz; + + return px * px + py * py + pz * pz; + } + + public float4 DistanceFromAabbSquared(ref FourTransposedAabbs tranposedAabb) + { + float4 px = math.max(float4.zero, tranposedAabb.Lx - Hx); + px = math.min(px, tranposedAabb.Hx - Lx); + + float4 py = math.max(float4.zero, tranposedAabb.Ly - Hy); + py = math.min(py, tranposedAabb.Hy - Ly); + + float4 pz = math.max(float4.zero, tranposedAabb.Lz - Hz); + pz = math.min(pz, tranposedAabb.Hz - Lz); + + return px * px + py * py + pz * pz; + } + + public bool4 Raycast(Ray ray, float maxFraction, out float4 fractions) + { + float4 lx = Lx - new float4(ray.Origin.x); + float4 hx = Hx - new float4(ray.Origin.x); + float4 nearXt = lx * new float4(ray.ReciprocalDirection.x); + float4 farXt = hx * new float4(ray.ReciprocalDirection.x); + + float4 ly = Ly - new float4(ray.Origin.y); + float4 hy = Hy - new float4(ray.Origin.y); + float4 nearYt = ly * new float4(ray.ReciprocalDirection.y); + float4 farYt = hy * new float4(ray.ReciprocalDirection.y); + + float4 lz = Lz - new float4(ray.Origin.z); + float4 hz = Hz - new float4(ray.Origin.z); + float4 nearZt = lz * new float4(ray.ReciprocalDirection.z); + float4 farZt = hz * new float4(ray.ReciprocalDirection.z); + + float4 nearX = math.min(nearXt, farXt); + float4 farX = math.max(nearXt, farXt); + + float4 nearY = math.min(nearYt, farYt); + float4 farY = math.max(nearYt, farYt); + + float4 nearZ = math.min(nearZt, farZt); + float4 farZ = math.max(nearZt, farZt); + + float4 nearMax = math.max(math.max(math.max(nearX, nearY), nearZ), float4.zero); + float4 farMin = math.min(math.min(math.min(farX, farY), farZ), new float4(maxFraction)); + + fractions = nearMax; + + return (nearMax <= farMin) & (lx <= hx); + } + } +} diff --git a/package/Unity.Physics/Base/Math/FourTransposedAabbs.cs.meta b/package/Unity.Physics/Base/Math/FourTransposedAabbs.cs.meta new file mode 100755 index 000000000..0bc915a4a --- /dev/null +++ b/package/Unity.Physics/Base/Math/FourTransposedAabbs.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 0dad76fe03d740b41af1e74b728f3a29 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Unity.Physics/Base/Math/FourTransposedPoints.cs b/package/Unity.Physics/Base/Math/FourTransposedPoints.cs new file mode 100755 index 000000000..833bb4b7e --- /dev/null +++ b/package/Unity.Physics/Base/Math/FourTransposedPoints.cs @@ -0,0 +1,67 @@ +using System; +using Unity.Mathematics; + +namespace Unity.Physics +{ + public static partial class Math + { + // 4 transposed 3D vertices + [Serializable] + public struct FourTransposedPoints + { + private float4x3 m_TransposedPoints; + + public float4 X => m_TransposedPoints.c0; + public float4 Y => m_TransposedPoints.c1; + public float4 Z => m_TransposedPoints.c2; + + public FourTransposedPoints V0000 => new FourTransposedPoints { m_TransposedPoints = new float4x3(X.xxxx, Y.xxxx, Z.xxxx) }; + public FourTransposedPoints V1111 => new FourTransposedPoints { m_TransposedPoints = new float4x3(X.yyyy, Y.yyyy, Z.yyyy) }; + public FourTransposedPoints V2222 => new FourTransposedPoints { m_TransposedPoints = new float4x3(X.zzzz, Y.zzzz, Z.zzzz) }; + public FourTransposedPoints V3333 => new FourTransposedPoints { m_TransposedPoints = new float4x3(X.wwww, Y.wwww, Z.wwww) }; + public FourTransposedPoints V1230 => new FourTransposedPoints { m_TransposedPoints = new float4x3(X.yzwx, Y.yzwx, Z.yzwx) }; + public FourTransposedPoints V3012 => new FourTransposedPoints { m_TransposedPoints = new float4x3(X.wxyz, Y.wxyz, Z.wxyz) }; + public FourTransposedPoints V1203 => new FourTransposedPoints { m_TransposedPoints = new float4x3(X.yzxw, Y.yzxw, Z.yzxw) }; + + public FourTransposedPoints(float3 v) + { + m_TransposedPoints.c0 = v.xxxx; + m_TransposedPoints.c1 = v.yyyy; + m_TransposedPoints.c2 = v.zzzz; + } + + public FourTransposedPoints(float3 v0, float3 v1, float3 v2, float3 v3) + { + m_TransposedPoints.c0 = new float4(v0.x, v1.x, v2.x, v3.x); + m_TransposedPoints.c1 = new float4(v0.y, v1.y, v2.y, v3.y); + m_TransposedPoints.c2 = new float4(v0.z, v1.z, v2.z, v3.z); + } + + public static FourTransposedPoints operator +(FourTransposedPoints lhs, FourTransposedPoints rhs) + { + return new FourTransposedPoints { m_TransposedPoints = lhs.m_TransposedPoints + rhs.m_TransposedPoints }; + } + + public static FourTransposedPoints operator -(FourTransposedPoints lhs, FourTransposedPoints rhs) + { + return new FourTransposedPoints { m_TransposedPoints = lhs.m_TransposedPoints - rhs.m_TransposedPoints }; + } + + public FourTransposedPoints MulT(float4 v) + { + return new FourTransposedPoints { m_TransposedPoints = new float4x3(X * v, Y * v, Z * v) }; + } + + public FourTransposedPoints Cross(FourTransposedPoints a) + { + return new FourTransposedPoints { m_TransposedPoints = new float4x3(Y * a.Z - Z * a.Y, Z * a.X - X * a.Z, X * a.Y - Y * a.X) }; + } + + public float4 Dot(float3 v) => X * v.x + Y * v.y + Z * v.z; + public float4 Dot(FourTransposedPoints a) => X * a.X + Y * a.Y + Z * a.Z; + + public float3 GetPoint(int index) => new float3(X[index], Y[index], Z[index]); + public float4 GetComponent(int index) => m_TransposedPoints[index]; + } + } +} diff --git a/package/Unity.Physics/Base/Math/FourTransposedPoints.cs.meta b/package/Unity.Physics/Base/Math/FourTransposedPoints.cs.meta new file mode 100755 index 000000000..6f629785c --- /dev/null +++ b/package/Unity.Physics/Base/Math/FourTransposedPoints.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: d877f2d6b4313ce4eb845c29b2c31854 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Unity.Physics/Base/Math/Math.cs b/package/Unity.Physics/Base/Math/Math.cs new file mode 100755 index 000000000..8122e6a27 --- /dev/null +++ b/package/Unity.Physics/Base/Math/Math.cs @@ -0,0 +1,226 @@ +using System; +using System.Diagnostics; +using System.Runtime.CompilerServices; +using Unity.Mathematics; +using Debug = UnityEngine.Debug; + +namespace Unity.Physics +{ + // Common math helper functions + [DebuggerStepThrough] + public static partial class Math + { + // Constants + [DebuggerStepThrough] + public static class Constants + { + public static float4 One4F => new float4(1); + public static float4 Min4F => new float4(float.MinValue); + public static float4 Max4F => new float4(float.MaxValue); + public static float3 Min3F => new float3(float.MinValue); + public static float3 Max3F => new float3(float.MaxValue); + + // Smallest float such that 1.0 + eps != 1.0 + // Different from float.Epsilon which is the smallest value greater than zero. + public const float Eps = 1.192092896e-07F; + } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static int NextMultipleOf16(int input) => ((input + 15) >> 4) << 4; + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static ulong NextMultipleOf16(ulong input) => ((input + 15) >> 4) << 4; + + /// Note that alignment must be a power of two for this to work. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static int NextMultipleOf(int input, int alignment) => (input + (alignment - 1)) & (~(alignment - 1)); + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static ulong NextMultipleOf(ulong input, ulong alignment) => (input + (alignment - 1)) & (~(alignment - 1)); + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static int IndexOfMinComponent(float2 v) => v.x < v.y ? 0 : 1; + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static int IndexOfMinComponent(float3 v) => v.x < v.y ? (v.x < v.z ? 0 : 2) : (v.y < v.z ? 1 : 2); + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static int IndexOfMinComponent(float4 v) { int xyz = IndexOfMinComponent(v.xyz); return v[xyz] < v.w ? xyz : 3; } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static int IndexOfMaxComponent(float2 v) => IndexOfMinComponent(-v); + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static int IndexOfMaxComponent(float3 v) => IndexOfMinComponent(-v); + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static int IndexOfMaxComponent(float4 v) => IndexOfMinComponent(-v); + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static float HorizontalMul(float3 v) => v.x * v.y * v.z; + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static float HorizontalMul(float4 v) => (v.x * v.y) * (v.z * v.w); + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static float Dotxyz1(float4 lhs, float3 rhs) => math.dot(lhs, new float4(rhs, 1)); + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static float Det(float3 a, float3 b, float3 c) => math.dot(math.cross(a, b), c); // TODO: use math.determinant()? + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static float RSqrtSafe(float v) => math.select(math.rsqrt(v), 0.0f, math.abs(v) < 1e-10); + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static float NormalizeWithLength(float3 v, out float3 n) + { + float lengthSq = math.lengthsq(v); + float invLength = math.rsqrt(lengthSq); + n = v * invLength; + return lengthSq * invLength; + } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static bool IsNormalized(float3 v) + { + float lenZero = math.lengthsq(v) - 1.0f; + float absLenZero = math.abs(lenZero); + return absLenZero < Constants.Eps; + } + + // Return two normals perpendicular to the input vector + public static void CalculatePerpendicularNormalized(float3 v, out float3 p, out float3 q) + { + float3 vSquared = v * v; + float3 lengthsSquared = vSquared + vSquared.xxx; // y = ||j x v||^2, z = ||k x v||^2 + float3 invLengths = math.rsqrt(lengthsSquared); + + // select first direction, j x v or k x v, whichever has greater magnitude + float3 dir0 = new float3(-v.y, v.x, 0.0f); + float3 dir1 = new float3(-v.z, 0.0f, v.x); + bool cmp = (lengthsSquared.y > lengthsSquared.z); + float3 dir = math.select(dir1, dir0, cmp); + + // normalize and get the other direction + float invLength = math.select(invLengths.z, invLengths.y, cmp); + p = dir * invLength; + float3 cross = math.cross(v, dir); + q = cross * invLength; + } + + // Calculate the eigenvectors and eigenvalues of a symmetric 3x3 matrix + public static void DiagonalizeSymmetricApproximation(float3x3 a, out float3x3 eigenVectors, out float3 eigenValues) + { + float GetMatrixElement(float3x3 m, int row, int col) + { + switch (col) + { + case 0: return m.c0[row]; + case 1: return m.c1[row]; + case 2: return m.c2[row]; + default: UnityEngine.Assertions.Assert.IsTrue(false); return 0.0f; + } + } + + void SetMatrixElement(ref float3x3 m, int row, int col, float x) + { + switch (col) + { + case 0: m.c0[row] = x; break; + case 1: m.c1[row] = x; break; + case 2: m.c2[row] = x; break; + default: UnityEngine.Assertions.Assert.IsTrue(false); break; + } + } + + eigenVectors = float3x3.identity; + float epsSq = 1e-14f * (math.lengthsq(a.c0) + math.lengthsq(a.c1) + math.lengthsq(a.c2)); + const int maxIterations = 10; + for (int iteration = 0; iteration < maxIterations; iteration++) + { + // Find the row (p) and column (q) of the off-diagonal entry with greater magnitude + int p = 0, q = 1; + { + float maxEntry = math.abs(a.c1[0]); + float mag02 = math.abs(a.c2[0]); + float mag12 = math.abs(a.c2[1]); + if (mag02 > maxEntry) + { + maxEntry = mag02; + p = 0; + q = 2; + } + if (mag12 > maxEntry) + { + maxEntry = mag12; + p = 1; + q = 2; + } + + // Terminate if it's small enough + if (maxEntry * maxEntry < epsSq) + { + break; + } + } + + // Calculate jacobia rotation + float3x3 j = float3x3.identity; + { + float apq = GetMatrixElement(a, p, q); + float tau = (GetMatrixElement(a, q, q) - GetMatrixElement(a, p, p)) / (2.0f * apq); + float t = math.sqrt(1.0f + tau * tau); + if (tau > 0.0f) + { + t = 1.0f / (tau + t); + } + else + { + t = 1.0f / (tau - t); + } + float c = math.rsqrt(1.0f + t * t); + float s = t * c; + + SetMatrixElement(ref j, p, p, c); + SetMatrixElement(ref j, q, q, c); + SetMatrixElement(ref j, p, q, s); + SetMatrixElement(ref j, q, p, -s); + } + + // Rotate a + a = math.mul(math.transpose(j), math.mul(a, j)); + eigenVectors = math.mul(eigenVectors, j); + } + eigenValues = new float3(a.c0.x, a.c1.y, a.c2.z); + } + + // Returns the twist angle of the swing-twist decomposition of q about i, j, or k corresponding to index = 0, 1, or 2 respectively. + public static float CalculateTwistAngle(quaternion q, int twistAxisIndex) + { + // q = swing * twist, twist = normalize(twistAxis * twistAxis dot q.xyz, q.w) + float dot = q.value[twistAxisIndex]; + float w = q.value.w; + float lengthSq = dot * dot + w * w; + float invLength = RSqrtSafe(lengthSq); + float sinHalfAngle = dot * invLength; + float cosHalfAngle = w * invLength; + float halfAngle = math.atan2(sinHalfAngle, cosHalfAngle); + return halfAngle + halfAngle; + } + + // Returns a quaternion q with q * from = to + public static quaternion FromToRotation(float3 from, float3 to) + { + Debug.Assert(math.abs(math.lengthsq(from) - 1.0f) < 1e-4f); + Debug.Assert(math.abs(math.lengthsq(to) - 1.0f) < 1e-4f); + float3 cross = math.cross(from, to); + CalculatePerpendicularNormalized(from, out float3 safeAxis, out float3 unused); // for when angle ~= 180 + float dot = math.dot(from, to); + float3 squares = new float3(0.5f - new float2(dot, -dot) * 0.5f, math.lengthsq(cross)); + float3 inverses = math.select(math.rsqrt(squares), 0.0f, squares < 1e-10f); + float2 sinCosHalfAngle = squares.xy * inverses.xy; + float3 axis = math.select(cross * inverses.z, safeAxis, squares.z < 1e-10f); + return new quaternion(new float4(axis * sinCosHalfAngle.x, sinCosHalfAngle.y)); + } + } +} diff --git a/package/Unity.Physics/Base/Math/Math.cs.meta b/package/Unity.Physics/Base/Math/Math.cs.meta new file mode 100755 index 000000000..c87214ec6 --- /dev/null +++ b/package/Unity.Physics/Base/Math/Math.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 33a80ac9dac48424fa0c83610894c5d9 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Unity.Physics/Base/Math/Physics_Transform.cs b/package/Unity.Physics/Base/Math/Physics_Transform.cs new file mode 100755 index 000000000..68efb4ae6 --- /dev/null +++ b/package/Unity.Physics/Base/Math/Physics_Transform.cs @@ -0,0 +1,62 @@ +using System.Diagnostics; +using Unity.Mathematics; + +namespace Unity.Physics +{ + public static partial class Math + { + [DebuggerStepThrough] + public struct MTransform // TODO: Replace with Unity float4x4 ? + { + public float3x3 Rotation; + public float3 Translation; + + public static MTransform Identity => new MTransform { Rotation = float3x3.identity, Translation = float3.zero }; + + public MTransform(RigidTransform transform) + { + Rotation = new float3x3(transform.rot); + Translation = transform.pos; + } + + public MTransform(quaternion rotation, float3 translation) + { + Rotation = new float3x3(rotation); + Translation = translation; + } + + public MTransform(float3x3 rotation, float3 translation) + { + Rotation = rotation; + Translation = translation; + } + + public float3x3 InverseRotation => math.transpose(Rotation); + } + + public static float3 Mul(MTransform a, float3 x) + { + return math.mul(a.Rotation, x) + a.Translation; + } + + // Returns cFromA = cFromB * bFromA + public static MTransform Mul(MTransform cFromB, MTransform bFromA) + { + return new MTransform + { + Rotation = math.mul(cFromB.Rotation, bFromA.Rotation), + Translation = math.mul(cFromB.Rotation, bFromA.Translation) + cFromB.Translation + }; + } + + public static MTransform Inverse(MTransform a) + { + float3x3 inverseRotation = math.transpose(a.Rotation); + return new MTransform + { + Rotation = inverseRotation, + Translation = math.mul(inverseRotation, -a.Translation) + }; + } + } +} diff --git a/package/Unity.Physics/Base/Math/Physics_Transform.cs.meta b/package/Unity.Physics/Base/Math/Physics_Transform.cs.meta new file mode 100755 index 000000000..f5177d38f --- /dev/null +++ b/package/Unity.Physics/Base/Math/Physics_Transform.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: c068a7f887800b6439bdfadebeaf5370 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Unity.Physics/Base/Math/Plane.cs b/package/Unity.Physics/Base/Math/Plane.cs new file mode 100755 index 000000000..62cce0e71 --- /dev/null +++ b/package/Unity.Physics/Base/Math/Plane.cs @@ -0,0 +1,65 @@ +using System.Runtime.CompilerServices; +using Unity.Mathematics; + +namespace Unity.Physics +{ + // A plane described by a normal and a distance from the origin + public struct Plane + { + private float4 m_NormalAndDistance; + + public float3 Normal + { + get => m_NormalAndDistance.xyz; + set => m_NormalAndDistance.xyz = value; + } + + public float Distance + { + get => m_NormalAndDistance.w; + set => m_NormalAndDistance.w = value; + } + + public Plane Flipped => new Plane { m_NormalAndDistance = -m_NormalAndDistance }; + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public Plane(float3 normal, float distance) + { + m_NormalAndDistance = new float4(normal, distance); + } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static implicit operator float4(Plane plane) => plane.m_NormalAndDistance; + } + + // Helper functions + public static partial class Math + { + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Plane PlaneFromDirection(float3 origin, float3 direction) + { + float3 normal = math.normalize(direction); + return new Plane(normal, -math.dot(normal, origin)); + } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Plane PlaneFromTwoEdges(float3 origin, float3 edgeA, float3 edgeB) + { + return PlaneFromDirection(origin, math.cross(edgeA, edgeB)); + } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Plane TransformPlane(quaternion rotation, float3 translation, Plane plane) + { + float3 normal = math.rotate(rotation, plane.Normal); + return new Plane(normal, plane.Distance - math.dot(normal, translation)); + } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Plane TransformPlane(MTransform transform, Plane plane) + { + float3 normal = math.mul(transform.Rotation, plane.Normal); + return new Plane(normal, plane.Distance - math.dot(normal, transform.Translation)); + } + } +} diff --git a/package/Unity.Physics/Base/Math/Plane.cs.meta b/package/Unity.Physics/Base/Math/Plane.cs.meta new file mode 100755 index 000000000..e458930c2 --- /dev/null +++ b/package/Unity.Physics/Base/Math/Plane.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 8eacc3e3090b56d4e946e9ed8a91cd96 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Unity.Physics/Collision.meta b/package/Unity.Physics/Collision.meta new file mode 100755 index 000000000..a76d61eca --- /dev/null +++ b/package/Unity.Physics/Collision.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: 90afa0e9b4d1a8942a560d58ff9ca1a9 +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Unity.Physics/Collision/Colliders.meta b/package/Unity.Physics/Collision/Colliders.meta new file mode 100755 index 000000000..8ce61c0dd --- /dev/null +++ b/package/Unity.Physics/Collision/Colliders.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: 5b5d25f928a3a0345862d29b2fd5267a +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Unity.Physics/Collision/Colliders/CompoundCollider.cs b/package/Unity.Physics/Collision/Colliders/CompoundCollider.cs new file mode 100755 index 000000000..048d6eac4 --- /dev/null +++ b/package/Unity.Physics/Collision/Colliders/CompoundCollider.cs @@ -0,0 +1,357 @@ +using System; +using System.Diagnostics; +using System.Runtime.InteropServices; +using Unity.Collections; +using Unity.Collections.LowLevel.Unsafe; +using Unity.Mathematics; +using Unity.Entities; +using static Unity.Physics.Math; + +namespace Unity.Physics +{ + // A collider containing instances of other colliders + public struct CompoundCollider : ICompositeCollider + { + private ColliderHeader m_Header; + + // A child collider, within the same blob as the compound collider. + // Warning: This references the collider via a relative offset, so must always be passed by reference. + public struct Child + { + public RigidTransform CompoundFromChild; + internal int m_ColliderOffset; + + public unsafe Collider* Collider + { + get + { + fixed (int* offsetPtr = &m_ColliderOffset) + { + return (Collider*)((byte*)offsetPtr + *offsetPtr); + } + } + } + } + + // The array of child colliders + private BlobArray m_ChildrenBlob; + public int NumChildren => m_ChildrenBlob.Length; + public BlobArray.Accessor Children => new BlobArray.Accessor(ref m_ChildrenBlob); + + // The bounding volume hierarchy + // TODO: Store node filters array too, for filtering queries within the BVH + private BlobArray m_BvhNodesBlob; + public unsafe BoundingVolumeHierarchy BoundingVolumeHierarchy + { + get + { + fixed (BlobArray* blob = &m_BvhNodesBlob) + { + var firstNode = (BoundingVolumeHierarchy.Node*)((byte*)&(blob->Offset) + blob->Offset); + return new BoundingVolumeHierarchy(firstNode, nodeFilters: null); + } + } + } + + #region Construction + + // Input to Create() + public struct ColliderBlobInstance + { + public RigidTransform CompoundFromChild; + public BlobAssetReference Collider; + } + + // Create a compound collider containing an array of other colliders. + // The source colliders are copied into the compound, so that it becomes one blob. + public static unsafe BlobAssetReference Create(NativeArray children) + { + if (children.Length == 0) + throw new ArgumentException(); + + // Get the total required memory size for the compound plus all its children, + // and the combined filter of all children + // TODO: Verify that the size is enough + int totalSize = Math.NextMultipleOf16(UnsafeUtility.SizeOf()); + CollisionFilter filter = children[0].Collider.Value.Filter; + foreach (var child in children) + { + totalSize += Math.NextMultipleOf16(child.Collider.Value.MemorySize); + filter = CollisionFilter.CreateUnion(filter, child.Collider.Value.Filter); + } + totalSize += (children.Length + BoundingVolumeHierarchy.Constants.MaxNumTreeBranches) * UnsafeUtility.SizeOf(); + + // Allocate the collider + var compoundCollider = (CompoundCollider*)UnsafeUtility.Malloc(totalSize, 16, Allocator.Temp); + UnsafeUtility.MemClear(compoundCollider, totalSize); + compoundCollider->m_Header.Type = ColliderType.Compound; + compoundCollider->m_Header.CollisionType = CollisionType.Composite; + compoundCollider->m_Header.Version = 0; + compoundCollider->m_Header.Magic = 0xff; + compoundCollider->m_Header.Filter = filter; + + // Initialize children array + Child* childrenPtr = (Child*)((byte*)compoundCollider + UnsafeUtility.SizeOf()); + compoundCollider->m_ChildrenBlob.Offset = (int)((byte*)childrenPtr - (byte*)(&compoundCollider->m_ChildrenBlob.Offset)); + compoundCollider->m_ChildrenBlob.Length = children.Length; + byte* end = (byte*)childrenPtr + UnsafeUtility.SizeOf() * children.Length; + end = (byte*)Math.NextMultipleOf16((ulong)end); + + // Copy children + for (int i = 0; i < children.Length; i++) + { + Collider* collider = (Collider*)children[i].Collider.GetUnsafePtr(); + UnsafeUtility.MemCpy(end, collider, collider->MemorySize); + childrenPtr[i].m_ColliderOffset = (int)(end - (byte*)(&childrenPtr[i].m_ColliderOffset)); + childrenPtr[i].CompoundFromChild = children[i].CompoundFromChild; + end += Math.NextMultipleOf16(collider->MemorySize); + } + + // Build mass properties + compoundCollider->MassProperties = compoundCollider->BuildMassProperties(); + + // Build bounding volume + int numNodes = compoundCollider->BuildBoundingVolume(out NativeArray nodes); + int bvhSize = numNodes * UnsafeUtility.SizeOf(); + compoundCollider->m_BvhNodesBlob.Offset = (int)(end - (byte*)(&compoundCollider->m_BvhNodesBlob.Offset)); + compoundCollider->m_BvhNodesBlob.Length = numNodes; + UnsafeUtility.MemCpy(end, nodes.GetUnsafeReadOnlyPtr(), bvhSize); + end += bvhSize; + nodes.Dispose(); + + // Copy to blob asset + int usedSize = (int)(end - (byte*)compoundCollider); + UnityEngine.Assertions.Assert.IsTrue(usedSize < totalSize); + compoundCollider->MemorySize = usedSize; + byte[] bytes = new byte[usedSize]; + Marshal.Copy((IntPtr)compoundCollider, bytes, 0, usedSize); + var blob = BlobAssetReference.Create(bytes); + UnsafeUtility.Free(compoundCollider, Allocator.Temp); + return blob; + } + + private unsafe int BuildBoundingVolume(out NativeArray nodes) + { + // Create inputs + var points = new NativeArray(NumChildren, Allocator.Temp, NativeArrayOptions.UninitializedMemory); + var aabbs = new NativeArray(NumChildren, Allocator.Temp, NativeArrayOptions.UninitializedMemory); + for (int i = 0; i < NumChildren; ++i) + { + points[i] = new BoundingVolumeHierarchy.PointAndIndex { Position = Children[i].CompoundFromChild.pos, Index = i }; + aabbs[i] = Children[i].Collider->CalculateAabb(Children[i].CompoundFromChild); + } + + // Build BVH + // Todo: cleanup, better size of nodes array + nodes = new NativeArray(2 + NumChildren, Allocator.Temp, NativeArrayOptions.UninitializedMemory) + { + [0] = BoundingVolumeHierarchy.Node.Empty, + [1] = BoundingVolumeHierarchy.Node.Empty + }; + + var bvh = new BoundingVolumeHierarchy(nodes); + bvh.Build(points, aabbs, out int numNodes); + + points.Dispose(); + aabbs.Dispose(); + + return numNodes; + } + + // Build mass properties representing a union of all the child collider mass properties. + // This assumes a uniform density for all children, and returns a mass properties for a compound of unit mass. + private unsafe MassProperties BuildMassProperties() + { + BlobArray.Accessor children = Children; + + // Calculate combined center of mass + float3 combinedCenterOfMass = float3.zero; + float combinedVolume = 0.0f; + for (int i = 0; i < NumChildren; ++i) + { + ref Child child = ref children[i]; + var mp = child.Collider->MassProperties; + + // weight this contribution by its volume (=mass) + combinedCenterOfMass += math.transform(child.CompoundFromChild, mp.MassDistribution.Transform.pos) * mp.Volume; + combinedVolume += mp.Volume; + } + if (combinedVolume > 0.0f) + { + combinedCenterOfMass /= combinedVolume; + } + + // Calculate combined inertia, relative to new center of mass + float3x3 combinedOrientation; + float3 combinedInertiaTensor; + { + float3x3 combinedInertiaMatrix = float3x3.zero; + for (int i = 0; i < NumChildren; ++i) + { + ref Child child = ref children[i]; + var mp = child.Collider->MassProperties; + + // rotate inertia into compound space + float3x3 temp = math.mul(mp.MassDistribution.InertiaMatrix, new float3x3(math.inverse(child.CompoundFromChild.rot))); + float3x3 inertiaMatrix = math.mul(new float3x3(child.CompoundFromChild.rot), temp); + + // shift it to be relative to the new center of mass + float3 shift = math.transform(child.CompoundFromChild, mp.MassDistribution.Transform.pos) - combinedCenterOfMass; + float3 shiftSq = shift * shift; + var diag = new float3(shiftSq.y + shiftSq.z, shiftSq.x + shiftSq.z, shiftSq.x + shiftSq.y); + var offDiag = new float3(shift.x * shift.y, shift.y * shift.z, shift.z * shift.x) * -1.0f; + inertiaMatrix.c0 += new float3(diag.x, offDiag.x, offDiag.z); + inertiaMatrix.c1 += new float3(offDiag.x, diag.y, offDiag.y); + inertiaMatrix.c2 += new float3(offDiag.z, offDiag.y, diag.z); + + // weight by its proportional volume (=mass) + inertiaMatrix *= mp.Volume / (combinedVolume + float.Epsilon); + + combinedInertiaMatrix += inertiaMatrix; + } + + // convert to box inertia + Math.DiagonalizeSymmetricApproximation( + combinedInertiaMatrix, out combinedOrientation, out combinedInertiaTensor); + } + + // Calculate combined angular expansion factor, relative to new center of mass + float combinedAngularExpansionFactor = 0.0f; + for (int i = 0; i < NumChildren; ++i) + { + ref Child child = ref children[i]; + var mp = child.Collider->MassProperties; + + float3 shift = math.transform(child.CompoundFromChild, mp.MassDistribution.Transform.pos) - combinedCenterOfMass; + float expansionFactor = mp.AngularExpansionFactor + math.length(shift); + combinedAngularExpansionFactor = math.max(combinedAngularExpansionFactor, expansionFactor); + } + + return new MassProperties + { + MassDistribution = new MassDistribution + { + Transform = new RigidTransform(combinedOrientation, combinedCenterOfMass), + InertiaTensor = combinedInertiaTensor + }, + Volume = combinedVolume, + AngularExpansionFactor = combinedAngularExpansionFactor + }; + } + + #endregion + + #region ICompositeCollider + + public ColliderType Type => m_Header.Type; + public CollisionType CollisionType => m_Header.CollisionType; + public int MemorySize { get; private set; } + + public CollisionFilter Filter { get => m_Header.Filter; set => m_Header.Filter = value; } + public MassProperties MassProperties { get; private set; } + + public Aabb CalculateAabb() + { + return CalculateAabb(RigidTransform.identity); + } + + public unsafe Aabb CalculateAabb(RigidTransform transform) + { + // TODO: Store a convex hull wrapping all the children, and use that to calculate tighter AABBs? + return Math.TransformAabb(transform, BoundingVolumeHierarchy.Domain); + } + + // Cast a ray against this collider. + public bool CastRay(RaycastInput input) => QueryWrappers.RayCast(ref this, input); + public bool CastRay(RaycastInput input, out RaycastHit closestHit) => QueryWrappers.RayCast(ref this, input, out closestHit); + public bool CastRay(RaycastInput input, ref NativeList allHits) => QueryWrappers.RayCast(ref this, input, ref allHits); + public unsafe bool CastRay(RaycastInput input, ref T collector) where T : struct, ICollector + { + fixed (CompoundCollider* target = &this) + { + return RaycastQueries.RayCollider(input, (Collider*)target, ref collector); + } + } + + // Cast another collider against this one. + public bool CastCollider(ColliderCastInput input) => QueryWrappers.ColliderCast(ref this, input); + public bool CastCollider(ColliderCastInput input, out ColliderCastHit closestHit) => QueryWrappers.ColliderCast(ref this, input, out closestHit); + public bool CastCollider(ColliderCastInput input, ref NativeList allHits) => QueryWrappers.ColliderCast(ref this, input, ref allHits); + public unsafe bool CastCollider(ColliderCastInput input, ref T collector) where T : struct, ICollector + { + fixed (CompoundCollider* target = &this) + { + return ColliderCastQueries.ColliderCollider(input, (Collider*)target, ref collector); + } + } + + // Calculate the distance from a point to this collider. + public bool CalculateDistance(PointDistanceInput input) => QueryWrappers.CalculateDistance(ref this, input); + public bool CalculateDistance(PointDistanceInput input, out DistanceHit closestHit) => QueryWrappers.CalculateDistance(ref this, input, out closestHit); + public bool CalculateDistance(PointDistanceInput input, ref NativeList allHits) => QueryWrappers.CalculateDistance(ref this, input, ref allHits); + public unsafe bool CalculateDistance(PointDistanceInput input, ref T collector) where T : struct, ICollector + { + fixed (CompoundCollider* target = &this) + { + return DistanceQueries.PointCollider(input, (Collider*)target, ref collector); + } + } + + // Calculate the distance from another collider to this one. + public bool CalculateDistance(ColliderDistanceInput input) => QueryWrappers.CalculateDistance(ref this, input); + public bool CalculateDistance(ColliderDistanceInput input, out DistanceHit closestHit) => QueryWrappers.CalculateDistance(ref this, input, out closestHit); + public bool CalculateDistance(ColliderDistanceInput input, ref NativeList allHits) => QueryWrappers.CalculateDistance(ref this, input, ref allHits); + public unsafe bool CalculateDistance(ColliderDistanceInput input, ref T collector) where T : struct, ICollector + { + fixed (CompoundCollider* target = &this) + { + return DistanceQueries.ColliderCollider(input, (Collider*)target, ref collector); + } + } + + public uint NumColliderKeyBits => (uint)(32 - math.lzcnt(NumChildren)); + + public unsafe bool GetChild(ref ColliderKey key, out ChildCollider child) + { + if (key.PopSubKey(NumColliderKeyBits, out uint childIndex)) + { + ref Child c = ref Children[(int)childIndex]; + child = new ChildCollider(c.Collider) { TransformFromChild = c.CompoundFromChild }; + return true; + } + + child = new ChildCollider(); + return false; + } + + public unsafe bool GetLeaf(ColliderKey key, out ChildCollider leaf) + { + fixed (CompoundCollider* root = &this) + { + return Collider.GetLeafCollider((Collider*)root, RigidTransform.identity, key, out leaf); + } + } + + public unsafe void GetLeaves(ref T collector) where T : struct, ILeafColliderCollector + { + for (uint i = 0; i < NumChildren; i++) + { + ref Child c = ref Children[(int)i]; + ColliderKey childKey = new ColliderKey(NumColliderKeyBits, i); + if (c.Collider->CollisionType == CollisionType.Composite) + { + collector.PushCompositeCollider(new ColliderKeyPath(childKey, NumColliderKeyBits), new MTransform(c.CompoundFromChild), out MTransform worldFromCompound); + c.Collider->GetLeaves(ref collector); + collector.PopCompositeCollider(NumColliderKeyBits, worldFromCompound); + } + else + { + var child = new ChildCollider(c.Collider) { TransformFromChild = c.CompoundFromChild }; + collector.AddLeaf(childKey, ref child); + } + } + } + + #endregion + } +} diff --git a/package/Unity.Physics/Collision/Colliders/CompoundCollider.cs.meta b/package/Unity.Physics/Collision/Colliders/CompoundCollider.cs.meta new file mode 100755 index 000000000..8e806d1be --- /dev/null +++ b/package/Unity.Physics/Collision/Colliders/CompoundCollider.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 022919aecaeb0814ca98f7be88471f8d +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Unity.Physics/Collision/Colliders/ConvexCollider.cs b/package/Unity.Physics/Collision/Colliders/ConvexCollider.cs new file mode 100755 index 000000000..9623b89bb --- /dev/null +++ b/package/Unity.Physics/Collision/Colliders/ConvexCollider.cs @@ -0,0 +1,371 @@ +using System; +using System.Collections.Generic; +using System.Runtime.InteropServices; +using Unity.Collections; +using Unity.Collections.LowLevel.Unsafe; +using Unity.Mathematics; +using Unity.Entities; +using UnityEngine.Assertions; + +namespace Unity.Physics +{ + // A collider in the shape of an arbitrary convex hull. + // Warning: This is just the header, it is followed by variable sized data in memory. + // Therefore this struct must always be passed by reference, never by value. + public struct ConvexCollider : IConvexCollider + { + // Header + private ConvexColliderHeader m_Header; + public ConvexHull ConvexHull; + + // followed by variable sized convex hull data + + #region Construction + + // Create a convex collider from the given point cloud. + public static unsafe BlobAssetReference Create( + NativeArray points, float convexRadius, + float3? scale = null, CollisionFilter? filter = null, Material? material = null) + { + if (convexRadius < 0.0f || !math.isfinite(convexRadius)) + { + throw new ArgumentException("Tried to create ConvexCollider with invalid convex radius"); + } + + // Build convex hull + int verticesCapacity = points.Length; + int triangleCapacity = 2 * verticesCapacity; + var vertices = (ConvexHullBuilder.Vertex*)UnsafeUtility.Malloc(verticesCapacity * sizeof(ConvexHullBuilder.Vertex), 16, Allocator.Temp); + var triangles = (ConvexHullBuilder.Triangle*)UnsafeUtility.Malloc(triangleCapacity * sizeof(ConvexHullBuilder.Triangle), 16, Allocator.Temp); + var builder = new ConvexHullBuilder(vertices, verticesCapacity, triangles, triangleCapacity); + float3 s = scale ?? new float3(1); + foreach (float3 point in points) + { + if (math.any(!math.isfinite(point))) + { + throw new ArgumentException("Tried to create ConvexCollider with invalid points"); + } + builder.AddPoint(point * s); + } + + // TODO: shrink by convex radius + + // Build face information + float maxAngle = 0.1f * (float)math.PI / 180.0f; + builder.BuildFaceIndices(maxAngle); + + // Simplify the hull until it fits requirements + // TODO.ma this is just a failsafe. We need to think about user-controlled simplification settings & how to warn the user if their shape is too complex. + { + const int maxVertices = 252; // as per Havok + + float maxSimplificationError = 1e-3f; + int iterations = 0; + while (builder.Vertices.PeakCount > maxVertices) + { + if (iterations++ > 10) // don't loop forever + { + Assert.IsTrue(false); + return new BlobAssetReference(); + } + builder.SimplifyVertices(maxSimplificationError); + builder.BuildFaceIndices(); + maxSimplificationError *= 2.0f; + } + } + + // Convert hull to compact format + var tempHull = new TempHull(ref builder); + + // Allocate collider + int totalSize = UnsafeUtility.SizeOf(); + totalSize += tempHull.Vertices.Count * sizeof(float3); + totalSize = Math.NextMultipleOf16(totalSize); // planes currently must be aligned for Havok + totalSize += tempHull.Planes.Count * sizeof(Plane); + totalSize += tempHull.Faces.Count * sizeof(ConvexHull.Face); + totalSize += tempHull.FaceVertexIndices.Count * sizeof(short); + totalSize += tempHull.VertexEdges.Count * sizeof(ConvexHull.Edge); + totalSize += tempHull.FaceLinks.Count * sizeof(ConvexHull.Edge); + ConvexCollider* collider = (ConvexCollider*)UnsafeUtility.Malloc(totalSize, 16, Allocator.Temp); + + // Initialize it + { + UnsafeUtility.MemClear(collider, totalSize); + collider->MemorySize = totalSize; + + collider->m_Header.Type = ColliderType.Convex; + collider->m_Header.CollisionType = CollisionType.Convex; + collider->m_Header.Version = 0; + collider->m_Header.Magic = 0xff; + collider->m_Header.Filter = filter ?? CollisionFilter.Default; + collider->m_Header.Material = material ?? Material.Default; + + ref var hull = ref collider->ConvexHull; + + hull.ConvexRadius = convexRadius; + + // Initialize blob arrays + { + byte* end = (byte*)collider + UnsafeUtility.SizeOf(); + + hull.VerticesBlob.Offset = (int)(end - (byte*)UnsafeUtility.AddressOf(ref hull.VerticesBlob.Offset)); + hull.VerticesBlob.Length = tempHull.Vertices.Count; + end += sizeof(float3) * tempHull.Vertices.Count; + + end = (byte*)Math.NextMultipleOf16((ulong)end); // planes currently must be aligned for Havok + + hull.FacePlanesBlob.Offset = (int)(end - (byte*)UnsafeUtility.AddressOf(ref hull.FacePlanesBlob.Offset)); + hull.FacePlanesBlob.Length = tempHull.Planes.Count; + end += sizeof(Plane) * tempHull.Planes.Count; + + hull.FacesBlob.Offset = (int)(end - (byte*)UnsafeUtility.AddressOf(ref hull.FacesBlob.Offset)); + hull.FacesBlob.Length = tempHull.Faces.Count; + end += sizeof(ConvexHull.Face) * tempHull.Faces.Count; + + hull.FaceVertexIndicesBlob.Offset = (int)(end - (byte*)UnsafeUtility.AddressOf(ref hull.FaceVertexIndicesBlob.Offset)); + hull.FaceVertexIndicesBlob.Length = tempHull.FaceVertexIndices.Count; + end += sizeof(byte) * tempHull.FaceVertexIndices.Count; + + hull.VertexEdgesBlob.Offset = (int)(end - (byte*)UnsafeUtility.AddressOf(ref hull.VertexEdgesBlob.Offset)); + hull.VertexEdgesBlob.Length = tempHull.VertexEdges.Count; + end += sizeof(ConvexHull.Edge) * tempHull.VertexEdges.Count; + + hull.FaceLinksBlob.Offset = (int)(end - (byte*)UnsafeUtility.AddressOf(ref hull.FaceLinksBlob.Offset)); + hull.FaceLinksBlob.Length = tempHull.FaceLinks.Count; + end += sizeof(ConvexHull.Edge) * tempHull.FaceLinks.Count; + } + + // Fill blob arrays + { + for (int i = 0; i < tempHull.Vertices.Count; i++) + { + hull.Vertices[i] = tempHull.Vertices[i]; + hull.VertexEdges[i] = tempHull.VertexEdges[i]; + } + + for (int i = 0; i < tempHull.Faces.Count; i++) + { + hull.Planes[i] = tempHull.Planes[i]; + hull.Faces[i] = tempHull.Faces[i]; + } + + for (int i = 0; i < tempHull.FaceVertexIndices.Count; i++) + { + hull.FaceVertexIndices[i] = tempHull.FaceVertexIndices[i]; + hull.FaceLinks[i] = tempHull.FaceLinks[i]; + } + } + + // Fill mass properties + { + var massProperties = builder.ComputeMassProperties(); + Math.DiagonalizeSymmetricApproximation(massProperties.InertiaTensor, out float3x3 orientation, out float3 inertia); + + float maxLengthSquared = 0.0f; + foreach (float3 vertex in hull.Vertices) + { + maxLengthSquared = math.max(maxLengthSquared, math.lengthsq(vertex - massProperties.CenterOfMass)); + } + + collider->MassProperties = new MassProperties + { + MassDistribution = new MassDistribution + { + Transform = new RigidTransform(orientation, massProperties.CenterOfMass), + InertiaTensor = inertia + }, + Volume = massProperties.Volume, + AngularExpansionFactor = math.sqrt(maxLengthSquared) + }; + } + } + + // Copy it into blob + // TODO: Allocate it directly into blob instead + byte[] bytes = new byte[totalSize]; + Marshal.Copy((IntPtr)collider, bytes, 0, totalSize); + UnsafeUtility.Free(collider, Allocator.Temp); + var asset = BlobAssetReference.Create(bytes); + + UnsafeUtility.Free(vertices, Allocator.Temp); + UnsafeUtility.Free(triangles, Allocator.Temp); + + return asset; + } + + // Temporary hull of managed arrays, used during construction + private struct TempHull + { + public readonly List Vertices; + public readonly List Planes; + public readonly List Faces; + public readonly List FaceVertexIndices; + public readonly List VertexEdges; + public readonly List FaceLinks; + + public unsafe TempHull(ref ConvexHullBuilder builder) + { + Vertices = new List(builder.Vertices.PeakCount); + Faces = new List(builder.NumFaces); + Planes = new List(builder.NumFaces); + FaceVertexIndices = new List(builder.NumFaceVertices); + VertexEdges = new List(builder.Vertices.PeakCount); + FaceLinks = new List(builder.NumFaceVertices); + + // Copy the vertices + byte* vertexIndexMap = stackalloc byte[builder.Vertices.PeakCount]; + foreach (int i in builder.Vertices.Indices) + { + vertexIndexMap[i] = (byte)Vertices.Count; + Vertices.Add(builder.Vertices[i].Position); + VertexEdges.Add(new ConvexHull.Edge()); // filled below + } + + // Copy the faces + var edgeMap = new NativeHashMap(builder.NumFaceVertices, Allocator.Temp); + for (ConvexHullBuilder.FaceEdge hullFace = builder.GetFirstFace(); hullFace.IsValid; hullFace = builder.GetNextFace(hullFace)) + { + // Store the plane + ConvexHullBuilder.Edge firstEdge = hullFace; + Plane facePlane = builder.ComputePlane(firstEdge.TriangleIndex); + Planes.Add(facePlane); + + // Walk the face's outer vertices & edges + short firstVertexIndex = (short)FaceVertexIndices.Count; + byte numEdges = 0; + float maxCosAngle = -1.0f; + for (ConvexHullBuilder.FaceEdge edge = hullFace; edge.IsValid; edge = builder.GetNextFaceEdge(edge)) + { + byte vertexIndex = vertexIndexMap[builder.StartVertex(edge)]; + FaceVertexIndices.Add(vertexIndex); + + var hullEdge = new ConvexHull.Edge { FaceIndex = (short)edge.Current.TriangleIndex, EdgeIndex = (byte)edge.Current.EdgeIndex }; // will be mapped to the output hull below + edgeMap.TryAdd(hullEdge, new ConvexHull.Edge { FaceIndex = (short)Faces.Count, EdgeIndex = numEdges }); + + VertexEdges[vertexIndex] = hullEdge; + + ConvexHullBuilder.Edge linkedEdge = builder.GetLinkedEdge(edge); + FaceLinks.Add(new ConvexHull.Edge { FaceIndex = (short)linkedEdge.TriangleIndex, EdgeIndex = (byte)linkedEdge.EdgeIndex }); // will be mapped to the output hull below + + Plane linkedPlane = builder.ComputePlane(linkedEdge.TriangleIndex); + maxCosAngle = math.max(maxCosAngle, math.dot(facePlane.Normal, linkedPlane.Normal)); + + numEdges++; + } + Assert.IsTrue(numEdges >= 3); + float minHalfAngle = math.acos(maxCosAngle) * 0.5f; + byte halfAngleCompressed = (byte)(minHalfAngle * 255.0f / (math.PI * 0.5f)); + + // Store the face + Faces.Add(new ConvexHull.Face + { + FirstIndex = firstVertexIndex, + NumVertices = numEdges, + MinHalfAngle = halfAngleCompressed + }); + } + + // Remap the edges + { + for (int i = 0; i < VertexEdges.Count; i++) + { + edgeMap.TryGetValue(VertexEdges[i], out ConvexHull.Edge vertexEdge); + VertexEdges[i] = vertexEdge; + } + + for (int i = 0; i < FaceLinks.Count; i++) + { + edgeMap.TryGetValue(FaceLinks[i], out ConvexHull.Edge faceLink); + FaceLinks[i] = faceLink; + } + } + edgeMap.Dispose(); + } + } + + #endregion + + #region IConvexCollider + + public ColliderType Type => m_Header.Type; + public CollisionType CollisionType => m_Header.CollisionType; + public int MemorySize { get; private set; } + + public CollisionFilter Filter { get => m_Header.Filter; set => m_Header.Filter = value; } + public Material Material { get => m_Header.Material; set => m_Header.Material = value; } + public MassProperties MassProperties { get; private set; } + + public Aabb CalculateAabb() + { + return CalculateAabb(RigidTransform.identity); + } + + public Aabb CalculateAabb(RigidTransform transform) + { + BlobArray.Accessor vertices = ConvexHull.Vertices; + float3 min = math.rotate(transform, vertices[0]); + float3 max = min; + for (int i = 1; i < vertices.Length; ++i) + { + float3 v = math.rotate(transform, vertices[i]); + min = math.min(min, v); + max = math.max(max, v); + } + return new Aabb + { + Min = min + transform.pos - new float3(ConvexHull.ConvexRadius), + Max = max + transform.pos + new float3(ConvexHull.ConvexRadius) + }; + } + + // Cast a ray against this collider. + public bool CastRay(RaycastInput input) => QueryWrappers.RayCast(ref this, input); + public bool CastRay(RaycastInput input, out RaycastHit closestHit) => QueryWrappers.RayCast(ref this, input, out closestHit); + public bool CastRay(RaycastInput input, ref NativeList allHits) => QueryWrappers.RayCast(ref this, input, ref allHits); + public unsafe bool CastRay(RaycastInput input, ref T collector) where T : struct, ICollector + { + fixed (ConvexCollider* target = &this) + { + return RaycastQueries.RayCollider(input, (Collider*)target, ref collector); + } + } + + // Cast another collider against this one. + public bool CastCollider(ColliderCastInput input) => QueryWrappers.ColliderCast(ref this, input); + public bool CastCollider(ColliderCastInput input, out ColliderCastHit closestHit) => QueryWrappers.ColliderCast(ref this, input, out closestHit); + public bool CastCollider(ColliderCastInput input, ref NativeList allHits) => QueryWrappers.ColliderCast(ref this, input, ref allHits); + public unsafe bool CastCollider(ColliderCastInput input, ref T collector) where T : struct, ICollector + { + fixed (ConvexCollider* target = &this) + { + return ColliderCastQueries.ColliderCollider(input, (Collider*)target, ref collector); + } + } + + // Calculate the distance from a point to this collider. + public bool CalculateDistance(PointDistanceInput input) => QueryWrappers.CalculateDistance(ref this, input); + public bool CalculateDistance(PointDistanceInput input, out DistanceHit closestHit) => QueryWrappers.CalculateDistance(ref this, input, out closestHit); + public bool CalculateDistance(PointDistanceInput input, ref NativeList allHits) => QueryWrappers.CalculateDistance(ref this, input, ref allHits); + public unsafe bool CalculateDistance(PointDistanceInput input, ref T collector) where T : struct, ICollector + { + fixed (ConvexCollider* target = &this) + { + return DistanceQueries.PointCollider(input, (Collider*)target, ref collector); + } + } + + // Calculate the distance from another collider to this one. + public bool CalculateDistance(ColliderDistanceInput input) => QueryWrappers.CalculateDistance(ref this, input); + public bool CalculateDistance(ColliderDistanceInput input, out DistanceHit closestHit) => QueryWrappers.CalculateDistance(ref this, input, out closestHit); + public bool CalculateDistance(ColliderDistanceInput input, ref NativeList allHits) => QueryWrappers.CalculateDistance(ref this, input, ref allHits); + public unsafe bool CalculateDistance(ColliderDistanceInput input, ref T collector) where T : struct, ICollector + { + fixed (ConvexCollider* target = &this) + { + return DistanceQueries.ColliderCollider(input, (Collider*)target, ref collector); + } + } + + #endregion + } +} diff --git a/package/Unity.Physics/Collision/Colliders/ConvexCollider.cs.meta b/package/Unity.Physics/Collision/Colliders/ConvexCollider.cs.meta new file mode 100755 index 000000000..81d62afb1 --- /dev/null +++ b/package/Unity.Physics/Collision/Colliders/ConvexCollider.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: e26fa93df59e9204193fa06106f41a50 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Unity.Physics/Collision/Colliders/CylinderCollider.cs b/package/Unity.Physics/Collision/Colliders/CylinderCollider.cs new file mode 100755 index 000000000..52160b3bd --- /dev/null +++ b/package/Unity.Physics/Collision/Colliders/CylinderCollider.cs @@ -0,0 +1,33 @@ +using Unity.Collections; +using Unity.Entities; +using Unity.Mathematics; + +namespace Unity.Physics +{ + public static class CylinderCollider + { + // Create a cylindrical convex hull with its height axis oriented along z-axis. + public static unsafe BlobAssetReference Create( + float3 center, float height, float radius, quaternion orientation, float convexRadius, + float3 scale, CollisionFilter filter, Material material + ) + { + // resolution of shape is limited by max number of coplanar contact points on each end of the shape + const int k_NumPoints = ConvexConvexManifoldQueries.Manifold.k_MaxNumContacts * 2; + var pointCloud = new NativeArray(k_NumPoints, Allocator.Temp); + var arcStep = (float)(2f * math.PI / (k_NumPoints / 2)); + var halfHeight = height * 0.5f; + for (var i = 0; i < k_NumPoints; i += 2) + { + var x = math.cos(arcStep * i) * radius; + var y = math.sin(arcStep * i) * radius; + pointCloud[i] = center + math.mul(orientation, new float3(x, y, -halfHeight)); + pointCloud[i + 1] = center + math.mul(orientation, new float3(x, y, halfHeight)); + } + + var collider = ConvexCollider.Create(pointCloud, convexRadius, scale, filter, material); + pointCloud.Dispose(); + return collider; + } + } +} diff --git a/package/Unity.Physics/Collision/Colliders/CylinderCollider.cs.meta b/package/Unity.Physics/Collision/Colliders/CylinderCollider.cs.meta new file mode 100755 index 000000000..4608f04b8 --- /dev/null +++ b/package/Unity.Physics/Collision/Colliders/CylinderCollider.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: ce7d3a97fe8af400fbfd020418a33b6e +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Unity.Physics/Collision/Colliders/Physics_BoxCollider.cs b/package/Unity.Physics/Collision/Colliders/Physics_BoxCollider.cs new file mode 100755 index 000000000..302b2661c --- /dev/null +++ b/package/Unity.Physics/Collision/Colliders/Physics_BoxCollider.cs @@ -0,0 +1,262 @@ +using Unity.Collections; +using Unity.Collections.LowLevel.Unsafe; +using Unity.Entities; +using Unity.Mathematics; + +namespace Unity.Physics +{ + // A collider in the shape of a box + public struct BoxCollider : IConvexCollider + { + // Header + private ConvexColliderHeader m_Header; + internal ConvexHull ConvexHull; + + // Convex hull data + // Todo: would be nice to use the actual types here but C# only likes fixed arrays of builtin types.. + private unsafe fixed byte m_Vertices[sizeof(float) * 3 * 8]; // float3[8] + private unsafe fixed byte m_FacePlanes[sizeof(float) * 4 * 6]; // Plane[6] + private unsafe fixed byte m_Faces[4 * 6]; // ConvexHull.Face[6] + private unsafe fixed byte m_FaceVertexIndices[sizeof(byte) * 24]; // byte[24] + private unsafe fixed byte m_VertexEdges[4 * 8]; // ConvexHull.Edge[8] + private unsafe fixed byte m_FaceLinks[4 * 24]; // ConvexHull.Edge[24] + + // Box parameters + private float3 m_Center; + private quaternion m_Orientation; + private float3 m_Size; + + public float3 Center { get => m_Center; set { m_Center = value; Update(); } } + public quaternion Orientation { get => m_Orientation; set { m_Orientation = value; Update(); } } + public float3 Size { get => m_Size; set { m_Size = value; Update(); } } + public float ConvexRadius { get => ConvexHull.ConvexRadius; set { ConvexHull.ConvexRadius = value; Update(); } } + + #region Construction + + public static BlobAssetReference Create(float3 center, quaternion orientation, float3 size, float convexRadius, CollisionFilter? filter = null, Material? material = null) + { + if (math.any(!math.isfinite(center))) + { + throw new System.ArgumentException("Tried to create BoxCollider with inf/nan center"); + } + if (math.any(size <= 0) || math.any(!math.isfinite(size))) + { + throw new System.ArgumentException("Tried to create BoxCollider with a negative, zero, inf or nan size component"); + } + if (convexRadius < 0 || !math.isfinite(convexRadius)) + { + throw new System.ArgumentException("Tried to create BoxCollider with negative, zero, inf or nan convex radius"); + } + using (var allocator = new BlobAllocator(-1)) + { + ref BoxCollider collider = ref allocator.ConstructRoot(); + collider.Init(center, orientation, size, convexRadius, filter ?? CollisionFilter.Default, material ?? Material.Default); + return allocator.CreateBlobAssetReference(Allocator.Persistent); + } + } + + internal unsafe void Init(float3 center, quaternion orientation, float3 size, float convexRadius, CollisionFilter filter, Material material) + { + m_Header.Type = ColliderType.Box; + m_Header.CollisionType = CollisionType.Convex; + m_Header.Version += 1; + m_Header.Magic = 0xff; + m_Header.Filter = filter; + m_Header.Material = material; + + // Build immutable convex data + fixed (BoxCollider* collider = &this) + { + ConvexHull.VerticesBlob.Offset = (int)((byte*)UnsafeUtility.AddressOf(ref collider->m_Vertices[0]) - (byte*)UnsafeUtility.AddressOf(ref ConvexHull.VerticesBlob.Offset)); + ConvexHull.VerticesBlob.Length = 8; + + ConvexHull.FacePlanesBlob.Offset = (int)((byte*)UnsafeUtility.AddressOf(ref collider->m_FacePlanes[0]) - (byte*)UnsafeUtility.AddressOf(ref ConvexHull.FacePlanesBlob.Offset)); + ConvexHull.FacePlanesBlob.Length = 6; + + ConvexHull.FacesBlob.Offset = (int)((byte*)UnsafeUtility.AddressOf(ref collider->m_Faces[0]) - (byte*)UnsafeUtility.AddressOf(ref ConvexHull.FacesBlob.Offset)); + ConvexHull.FacesBlob.Length = 6; + + ConvexHull.FaceVertexIndicesBlob.Offset = (int)((byte*)UnsafeUtility.AddressOf(ref collider->m_FaceVertexIndices[0]) - (byte*)UnsafeUtility.AddressOf(ref ConvexHull.FaceVertexIndicesBlob.Offset)); + ConvexHull.FaceVertexIndicesBlob.Length = 24; + + ConvexHull.VertexEdgesBlob.Offset = (int)((byte*)UnsafeUtility.AddressOf(ref collider->m_VertexEdges[0]) - (byte*)UnsafeUtility.AddressOf(ref ConvexHull.VertexEdgesBlob.Offset)); + ConvexHull.VertexEdgesBlob.Length = 8; + + ConvexHull.FaceLinksBlob.Offset = (int)((byte*)UnsafeUtility.AddressOf(ref collider->m_FaceLinks[0]) - (byte*)UnsafeUtility.AddressOf(ref ConvexHull.FaceLinksBlob.Offset)); + ConvexHull.FaceLinksBlob.Length = 24; + + ConvexHull.Face* faces = (ConvexHull.Face*)(&collider->m_Faces[0]); + faces[0] = new ConvexHull.Face { FirstIndex = 0, NumVertices = 4, MinHalfAngle = 0x80 }; + faces[1] = new ConvexHull.Face { FirstIndex = 4, NumVertices = 4, MinHalfAngle = 0x80 }; + faces[2] = new ConvexHull.Face { FirstIndex = 8, NumVertices = 4, MinHalfAngle = 0x80 }; + faces[3] = new ConvexHull.Face { FirstIndex = 12, NumVertices = 4, MinHalfAngle = 0x80 }; + faces[4] = new ConvexHull.Face { FirstIndex = 16, NumVertices = 4, MinHalfAngle = 0x80 }; + faces[5] = new ConvexHull.Face { FirstIndex = 20, NumVertices = 4, MinHalfAngle = 0x80 }; + + byte* index = &collider->m_FaceVertexIndices[0]; + byte[] faceVertexIndices = new byte[24] { 2, 6, 4, 0, 1, 5, 7, 3, 1, 0, 4, 5, 7, 6, 2, 3, 3, 2, 0, 1, 7, 5, 4, 6 }; + for (int i = 0; i < 24; i++) + { + *index++ = faceVertexIndices[i]; + } + + ConvexHull.Edge* vertexEdge = (ConvexHull.Edge*)(&collider->m_VertexEdges[0]); + short[] vertexEdgeValuePairs = new short[16] { 4, 2, 2, 0, 4, 1, 4, 0, 5, 2, 5, 1, 0, 1, 5, 0 }; + for (int i = 0; i < 8; i++) + { + *vertexEdge++ = new ConvexHull.Edge + { + FaceIndex = vertexEdgeValuePairs[2 * i], + EdgeIndex = (byte)vertexEdgeValuePairs[2 * i + 1] + }; + } + + ConvexHull.Edge* faceLink = (ConvexHull.Edge*)(&collider->m_FaceLinks[0]); + short[] faceLinkValuePairs = new short[48] { + 3, 1, 5, 2, 2, 1, 4, 1, 2, 3, 5, 0, 3, 3, 4, 3, 4, 2, 0, 2, 5, 1, 1, 0, + 5, 3, 0, 0, 4, 0, 1, 2, 3, 2, 0, 3, 2, 0, 1, 3, 1, 1, 2, 2, 0, 1, 3, 0 + }; + for (int i = 0; i < 24; i++) + { + *faceLink++ = new ConvexHull.Edge + { + FaceIndex = faceLinkValuePairs[2 * i], + EdgeIndex = (byte)faceLinkValuePairs[2 * i + 1] + }; + } + } + + // Build mutable convex data + m_Center = center; + m_Orientation = orientation; + m_Size = size; + ConvexHull.ConvexRadius = convexRadius; + Update(); + } + + // Update the vertices and planes to match the current box properties + private unsafe void Update() + { + fixed (BoxCollider* collider = &this) + { + // TODO: clamp to avoid extents <= 0 + float3 he = m_Size * 0.5f - ConvexHull.ConvexRadius; // half extents + + float3* vertices = (float3*)(&collider->m_Vertices[0]); + vertices[0] = m_Center + math.rotate(m_Orientation, new float3(he.x, he.y, he.z)); + vertices[1] = m_Center + math.rotate(m_Orientation, new float3(-he.x, he.y, he.z)); + vertices[2] = m_Center + math.rotate(m_Orientation, new float3(he.x, -he.y, he.z)); + vertices[3] = m_Center + math.rotate(m_Orientation, new float3(-he.x, -he.y, he.z)); + vertices[4] = m_Center + math.rotate(m_Orientation, new float3(he.x, he.y, -he.z)); + vertices[5] = m_Center + math.rotate(m_Orientation, new float3(-he.x, he.y, -he.z)); + vertices[6] = m_Center + math.rotate(m_Orientation, new float3(he.x, -he.y, -he.z)); + vertices[7] = m_Center + math.rotate(m_Orientation, new float3(-he.x, -he.y, -he.z)); + + Plane* planes = (Plane*)(&collider->m_FacePlanes[0]); + planes[0] = Math.TransformPlane(m_Orientation, m_Center, new Plane(new float3(1, 0, 0), -he.x)); + planes[1] = Math.TransformPlane(m_Orientation, m_Center, new Plane(new float3(-1, 0, 0), -he.x)); + planes[2] = Math.TransformPlane(m_Orientation, m_Center, new Plane(new float3(0, 1, 0), -he.y)); + planes[3] = Math.TransformPlane(m_Orientation, m_Center, new Plane(new float3(0, -1, 0), -he.y)); + planes[4] = Math.TransformPlane(m_Orientation, m_Center, new Plane(new float3(0, 0, 1), -he.z)); + planes[5] = Math.TransformPlane(m_Orientation, m_Center, new Plane(new float3(0, 0, -1), -he.z)); + } + } + + #endregion + + #region IConvexCollider + + public ColliderType Type => m_Header.Type; + public CollisionType CollisionType => m_Header.CollisionType; + public int MemorySize => UnsafeUtility.SizeOf(); + + public CollisionFilter Filter { get => m_Header.Filter; set { m_Header.Filter = value; } } + public Material Material { get => m_Header.Material; set { m_Header.Material = value; } } + + public MassProperties MassProperties => new MassProperties + { + MassDistribution = new MassDistribution + { + Transform = new RigidTransform(m_Orientation, m_Center), + InertiaTensor = new float3( + (m_Size.y * m_Size.y + m_Size.z * m_Size.z) / 12.0f, + (m_Size.x * m_Size.x + m_Size.z * m_Size.z) / 12.0f, + (m_Size.x * m_Size.x + m_Size.y * m_Size.y) / 12.0f) + }, + Volume = m_Size.x * m_Size.y * m_Size.z, + AngularExpansionFactor = math.length(m_Size * 0.5f - ConvexRadius) + }; + + public Aabb CalculateAabb() + { + return CalculateAabb(RigidTransform.identity); + } + + public Aabb CalculateAabb(RigidTransform transform) + { + float3 centerInB = math.transform(transform, m_Center); + + quaternion worldFromBox = math.mul(transform.rot, m_Orientation); + float3 x = math.mul(worldFromBox, new float3(m_Size.x * 0.5f, 0, 0)); + float3 y = math.mul(worldFromBox, new float3(0, m_Size.y * 0.5f, 0)); + float3 z = math.mul(worldFromBox, new float3(0, 0, m_Size.z * 0.5f)); + float3 halfExtentsInB = math.abs(x) + math.abs(y) + math.abs(z); + + return new Aabb + { + Min = centerInB - halfExtentsInB, + Max = centerInB + halfExtentsInB + }; + } + + // Cast a ray against this collider. + public bool CastRay(RaycastInput input) => QueryWrappers.RayCast(ref this, input); + public bool CastRay(RaycastInput input, out RaycastHit closestHit) => QueryWrappers.RayCast(ref this, input, out closestHit); + public bool CastRay(RaycastInput input, ref NativeList allHits) => QueryWrappers.RayCast(ref this, input, ref allHits); + public unsafe bool CastRay(RaycastInput input, ref T collector) where T : struct, ICollector + { + fixed (BoxCollider* target = &this) + { + return RaycastQueries.RayCollider(input, (Collider*)target, ref collector); + } + } + + // Cast another collider against this one. + public bool CastCollider(ColliderCastInput input) => QueryWrappers.ColliderCast(ref this, input); + public bool CastCollider(ColliderCastInput input, out ColliderCastHit closestHit) => QueryWrappers.ColliderCast(ref this, input, out closestHit); + public bool CastCollider(ColliderCastInput input, ref NativeList allHits) => QueryWrappers.ColliderCast(ref this, input, ref allHits); + public unsafe bool CastCollider(ColliderCastInput input, ref T collector) where T : struct, ICollector + { + fixed (BoxCollider* target = &this) + { + return ColliderCastQueries.ColliderCollider(input, (Collider*)target, ref collector); + } + } + + // Calculate the distance from a point to this collider. + public bool CalculateDistance(PointDistanceInput input) => QueryWrappers.CalculateDistance(ref this, input); + public bool CalculateDistance(PointDistanceInput input, out DistanceHit closestHit) => QueryWrappers.CalculateDistance(ref this, input, out closestHit); + public bool CalculateDistance(PointDistanceInput input, ref NativeList allHits) => QueryWrappers.CalculateDistance(ref this, input, ref allHits); + public unsafe bool CalculateDistance(PointDistanceInput input, ref T collector) where T : struct, ICollector + { + fixed (BoxCollider* target = &this) + { + return DistanceQueries.PointCollider(input, (Collider*)target, ref collector); + } + } + + // Calculate the distance from another collider to this one. + public bool CalculateDistance(ColliderDistanceInput input) => QueryWrappers.CalculateDistance(ref this, input); + public bool CalculateDistance(ColliderDistanceInput input, out DistanceHit closestHit) => QueryWrappers.CalculateDistance(ref this, input, out closestHit); + public bool CalculateDistance(ColliderDistanceInput input, ref NativeList allHits) => QueryWrappers.CalculateDistance(ref this, input, ref allHits); + public unsafe bool CalculateDistance(ColliderDistanceInput input, ref T collector) where T : struct, ICollector + { + fixed (BoxCollider* target = &this) + { + return DistanceQueries.ColliderCollider(input, (Collider*)target, ref collector); + } + } + + #endregion + } +} diff --git a/package/Unity.Physics/Collision/Colliders/Physics_BoxCollider.cs.meta b/package/Unity.Physics/Collision/Colliders/Physics_BoxCollider.cs.meta new file mode 100755 index 000000000..ac31b202d --- /dev/null +++ b/package/Unity.Physics/Collision/Colliders/Physics_BoxCollider.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 590f28f9b75ed8b49a21ffde288bc964 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Unity.Physics/Collision/Colliders/Physics_CapsuleCollider.cs b/package/Unity.Physics/Collision/Colliders/Physics_CapsuleCollider.cs new file mode 100755 index 000000000..df39a8179 --- /dev/null +++ b/package/Unity.Physics/Collision/Colliders/Physics_CapsuleCollider.cs @@ -0,0 +1,186 @@ +using Unity.Collections; +using Unity.Collections.LowLevel.Unsafe; +using Unity.Mathematics; +using Unity.Entities; + +namespace Unity.Physics +{ + // A collider in the shape of a capsule + public struct CapsuleCollider : IConvexCollider + { + // Header + private ConvexColliderHeader m_Header; + internal ConvexHull ConvexHull; + + private float3 m_Vertex0; + private float3 m_Vertex1; + + public float3 Vertex0 { get => m_Vertex0; set { m_Vertex0 = value; Update(); } } + public float3 Vertex1 { get => m_Vertex1; set { m_Vertex1 = value; Update(); } } + public float Radius { get => ConvexHull.ConvexRadius; set => ConvexHull.ConvexRadius = value; } + + #region Construction + + public static BlobAssetReference Create(float3 point0, float3 point1, float radius, CollisionFilter? filter = null, Material? material = null) + { + if (math.any(!math.isfinite(point0))) + { + throw new System.ArgumentException("Tried to create capsule collider with inf/nan point0"); + } + if (math.any(!math.isfinite(point1))) + { + throw new System.ArgumentException("Tried to create capsule collider with inf/nan point1"); + } + if (!math.isfinite(radius) || radius < 0.0f) + { + throw new System.ArgumentException("Tried to create capsule collider with zero/negative/inf/nan radius"); + } + + using (var allocator = new BlobAllocator(-1)) + { + ref CapsuleCollider collider = ref allocator.ConstructRoot(); + collider.Init(point0, point1, radius, filter ?? CollisionFilter.Default, material ?? Material.Default); + return allocator.CreateBlobAssetReference(Allocator.Persistent); + } + } + + internal unsafe void Init(float3 vertex0, float3 vertex1, float radius, CollisionFilter filter, Material material) + { + m_Header.Type = ColliderType.Capsule; + m_Header.CollisionType = CollisionType.Convex; + m_Header.Version += 1; + m_Header.Magic = 0xff; + m_Header.Filter = filter; + m_Header.Material = material; + + ConvexHull.ConvexRadius = radius; + ConvexHull.VerticesBlob.Offset = (int)((byte*)UnsafeUtility.AddressOf(ref m_Vertex0) - (byte*)UnsafeUtility.AddressOf(ref ConvexHull.VerticesBlob.Offset)); + ConvexHull.VerticesBlob.Length = 2; + // note: no faces + + m_Vertex0 = vertex0; + m_Vertex1 = vertex1; + Update(); + } + + private void Update() + { + // Treat as sphere if degenerate + ConvexHull.VerticesBlob.Length = m_Vertex0.Equals(m_Vertex1) ? 1 : 2; + } + + #endregion + + #region IConvexCollider + + public ColliderType Type => m_Header.Type; + public CollisionType CollisionType => m_Header.CollisionType; + public int MemorySize => UnsafeUtility.SizeOf(); + + public CollisionFilter Filter { get => m_Header.Filter; set { m_Header.Filter = value; } } + public Material Material { get => m_Header.Material; set { m_Header.Material = value; } } + + public MassProperties MassProperties + { + get + { + float3 axis = m_Vertex1 - m_Vertex0; + float length = math.length(axis); + float cylinderMass = (float)math.PI * length * Radius * Radius; + float sphereMass = (float)math.PI * (4.0f / 3.0f) * Radius * Radius * Radius; + float totalMass = cylinderMass + sphereMass; + cylinderMass /= totalMass; + sphereMass /= totalMass; + float onAxisInertia = (cylinderMass * 0.5f + sphereMass * 0.4f) * Radius * Radius; + float offAxisInertia = + cylinderMass * (1.0f / 4.0f * Radius * Radius + 1.0f / 12.0f * length * length) + + sphereMass * (2.0f / 5.0f * Radius * Radius + 3.0f / 8.0f * Radius * length + 1.0f / 4.0f * length * length); + + float3 axisInMotion = new float3(0, 1, 0); + quaternion orientation = length == 0 ? quaternion.identity : + Math.FromToRotation(axisInMotion, math.normalizesafe(Vertex1 - Vertex0, axisInMotion)); + + return new MassProperties + { + MassDistribution = new MassDistribution + { + Transform = new RigidTransform(orientation, (Vertex0 + Vertex1) * 0.5f), + InertiaTensor = new float3(offAxisInertia, onAxisInertia, offAxisInertia) + }, + Volume = (float)math.PI * Radius * Radius * ((4.0f / 3.0f) * Radius + math.length(Vertex1-Vertex0)), + AngularExpansionFactor = math.length(m_Vertex1 - m_Vertex0) * 0.5f + }; + } + } + + public Aabb CalculateAabb() + { + return new Aabb + { + Min = math.min(m_Vertex0, m_Vertex1) - new float3(Radius), + Max = math.max(m_Vertex0, m_Vertex1) + new float3(Radius) + }; + } + + public Aabb CalculateAabb(RigidTransform transform) + { + float3 v0 = math.transform(transform, m_Vertex0); + float3 v1 = math.transform(transform, m_Vertex1); + return new Aabb + { + Min = math.min(v0, v1) - new float3(Radius), + Max = math.max(v0, v1) + new float3(Radius) + }; + } + + // Cast a ray against this collider. + public bool CastRay(RaycastInput input) => QueryWrappers.RayCast(ref this, input); + public bool CastRay(RaycastInput input, out RaycastHit closestHit) => QueryWrappers.RayCast(ref this, input, out closestHit); + public bool CastRay(RaycastInput input, ref NativeList allHits) => QueryWrappers.RayCast(ref this, input, ref allHits); + public unsafe bool CastRay(RaycastInput input, ref T collector) where T : struct, ICollector + { + fixed (CapsuleCollider* target = &this) + { + return RaycastQueries.RayCollider(input, (Collider*)target, ref collector); + } + } + + // Cast another collider against this one. + public bool CastCollider(ColliderCastInput input) => QueryWrappers.ColliderCast(ref this, input); + public bool CastCollider(ColliderCastInput input, out ColliderCastHit closestHit) => QueryWrappers.ColliderCast(ref this, input, out closestHit); + public bool CastCollider(ColliderCastInput input, ref NativeList allHits) => QueryWrappers.ColliderCast(ref this, input, ref allHits); + public unsafe bool CastCollider(ColliderCastInput input, ref T collector) where T : struct, ICollector + { + fixed (CapsuleCollider* target = &this) + { + return ColliderCastQueries.ColliderCollider(input, (Collider*)target, ref collector); + } + } + + // Calculate the distance from a point to this collider. + public bool CalculateDistance(PointDistanceInput input) => QueryWrappers.CalculateDistance(ref this, input); + public bool CalculateDistance(PointDistanceInput input, out DistanceHit closestHit) => QueryWrappers.CalculateDistance(ref this, input, out closestHit); + public bool CalculateDistance(PointDistanceInput input, ref NativeList allHits) => QueryWrappers.CalculateDistance(ref this, input, ref allHits); + public unsafe bool CalculateDistance(PointDistanceInput input, ref T collector) where T : struct, ICollector + { + fixed (CapsuleCollider* target = &this) + { + return DistanceQueries.PointCollider(input, (Collider*)target, ref collector); + } + } + + // Calculate the distance from another collider to this one. + public bool CalculateDistance(ColliderDistanceInput input) => QueryWrappers.CalculateDistance(ref this, input); + public bool CalculateDistance(ColliderDistanceInput input, out DistanceHit closestHit) => QueryWrappers.CalculateDistance(ref this, input, out closestHit); + public bool CalculateDistance(ColliderDistanceInput input, ref NativeList allHits) => QueryWrappers.CalculateDistance(ref this, input, ref allHits); + public unsafe bool CalculateDistance(ColliderDistanceInput input, ref T collector) where T : struct, ICollector + { + fixed (CapsuleCollider* target = &this) + { + return DistanceQueries.ColliderCollider(input, (Collider*)target, ref collector); + } + } + + #endregion + } +} diff --git a/package/Unity.Physics/Collision/Colliders/Physics_CapsuleCollider.cs.meta b/package/Unity.Physics/Collision/Colliders/Physics_CapsuleCollider.cs.meta new file mode 100755 index 000000000..3e2f63fee --- /dev/null +++ b/package/Unity.Physics/Collision/Colliders/Physics_CapsuleCollider.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: f519d9beb59076e4586312e6003761b7 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Unity.Physics/Collision/Colliders/Physics_Collider.cs b/package/Unity.Physics/Collision/Colliders/Physics_Collider.cs new file mode 100755 index 000000000..0c75f5e22 --- /dev/null +++ b/package/Unity.Physics/Collision/Colliders/Physics_Collider.cs @@ -0,0 +1,520 @@ +using System; +using Unity.Collections; +using Unity.Mathematics; +using Unity.Entities; +using static Unity.Physics.Math; + +namespace Unity.Physics +{ + // The concrete type of a collider + public enum ColliderType : byte + { + // Convex types + Convex = 0, + Sphere = 1, + Capsule = 2, + Triangle = 3, + Quad = 4, + Box = 5, + + // Composite types + Mesh = 6, + Compound = 7 + } + + // The base type of a collider + public enum CollisionType : byte + { + Convex = 0, + Composite = 1 + } + + // Interface for colliders + internal interface ICollider : ICollidable + { + ColliderType Type { get; } + CollisionType CollisionType { get; } + MassProperties MassProperties { get; } + + // The total size of the collider in memory + int MemorySize { get; } + } + + // Interface for convex colliders + internal interface IConvexCollider : ICollider + { + CollisionFilter Filter { get; set; } + Material Material { get; set; } + } + + // Interface for composite colliders + internal interface ICompositeCollider : ICollider + { + // The combined filter of all the child colliders. + CollisionFilter Filter { get; } + + // The maximum number of bits needed to identify a child of this collider. + uint NumColliderKeyBits { get; } + + // Get a child of this collider. + // Return false if the key is not valid. + bool GetChild(ref ColliderKey key, out ChildCollider child); + + // Get a leaf of this collider. + // Return false if the key is not valid. + bool GetLeaf(ColliderKey key, out ChildCollider leaf); + + // Get all the leaves of this collider. + void GetLeaves(ref T collector) where T : struct, ILeafColliderCollector; + } + + // Interface for collecting leaf colliders + public interface ILeafColliderCollector + { + void AddLeaf(ColliderKey key, ref ChildCollider leaf); + + void PushCompositeCollider(ColliderKeyPath compositeKey, MTransform parentFromComposite, out MTransform worldFromParent); + + void PopCompositeCollider(uint numCompositeKeyBits, MTransform worldFromParent); + } + + // Base struct common to all colliders. + // Dispatches the interface methods to appropriate implementations for the collider type. + public struct Collider : ICompositeCollider + { + private ColliderHeader m_Header; + + #region ICollider + + public ColliderType Type => m_Header.Type; + public CollisionType CollisionType => m_Header.CollisionType; + + public unsafe int MemorySize + { + get + { + fixed (Collider* collider = &this) + { + switch (collider->Type) + { + case ColliderType.Convex: + return ((ConvexCollider*)collider)->MemorySize; + case ColliderType.Sphere: + return ((SphereCollider*)collider)->MemorySize; + case ColliderType.Capsule: + return ((CapsuleCollider*)collider)->MemorySize; + case ColliderType.Triangle: + case ColliderType.Quad: + return ((PolygonCollider*)collider)->MemorySize; + case ColliderType.Box: + return ((BoxCollider*)collider)->MemorySize; + case ColliderType.Mesh: + return ((MeshCollider*)collider)->MemorySize; + case ColliderType.Compound: + return ((CompoundCollider*)collider)->MemorySize; + default: + //Assert.IsTrue(Enum.IsDefined(typeof(ColliderType), collider->Type)); + return 0; + } + } + } + } + + public CollisionFilter Filter + { + get => m_Header.Filter; + set + { + // Disallow changing the filter of composite types directly, since that is a combination of its children + if(m_Header.CollisionType == CollisionType.Convex) + { + m_Header.Filter = value; + } + } + } + + public unsafe MassProperties MassProperties + { + get + { + fixed (Collider* collider = &this) + { + switch (collider->Type) + { + case ColliderType.Convex: + return ((ConvexCollider*)collider)->MassProperties; + case ColliderType.Sphere: + return ((SphereCollider*)collider)->MassProperties; + case ColliderType.Capsule: + return ((CapsuleCollider*)collider)->MassProperties; + case ColliderType.Triangle: + case ColliderType.Quad: + return ((PolygonCollider*)collider)->MassProperties; + case ColliderType.Box: + return ((BoxCollider*)collider)->MassProperties; + case ColliderType.Mesh: + return ((MeshCollider*)collider)->MassProperties; + case ColliderType.Compound: + return ((CompoundCollider*)collider)->MassProperties; + default: + //Assert.IsTrue(Enum.IsDefined(typeof(ColliderType), collider->Type)); + return MassProperties.UnitSphere; + } + } + } + } + + #endregion + + #region ICompositeCollider + + public unsafe uint NumColliderKeyBits + { + get + { + fixed (Collider* collider = &this) + { + switch (collider->Type) + { + case ColliderType.Mesh: + return ((MeshCollider*)collider)->NumColliderKeyBits; + case ColliderType.Compound: + return ((CompoundCollider*)collider)->NumColliderKeyBits; + default: + //Assert.IsTrue(Enum.IsDefined(typeof(ColliderType), collider->Type)); + return 0; + } + } + } + } + + public unsafe bool GetChild(ref ColliderKey key, out ChildCollider child) + { + fixed (Collider* collider = &this) + { + switch (collider->Type) + { + case ColliderType.Mesh: + return ((MeshCollider*)collider)->GetChild(ref key, out child); + case ColliderType.Compound: + return ((CompoundCollider*)collider)->GetChild(ref key, out child); + default: + //Assert.IsTrue(Enum.IsDefined(typeof(ColliderType), collider->Type)); + child = new ChildCollider(); + return false; + } + } + } + + public unsafe bool GetLeaf(ColliderKey key, out ChildCollider leaf) + { + fixed (Collider* collider = &this) + { + return GetLeafCollider(collider, RigidTransform.identity, key, out leaf); + } + } + + public unsafe void GetLeaves(ref T collector) where T : struct, ILeafColliderCollector + { + fixed (Collider* collider = &this) + { + switch (collider->Type) + { + case ColliderType.Mesh: + ((MeshCollider*)collider)->GetLeaves(ref collector); + break; + case ColliderType.Compound: + ((CompoundCollider*)collider)->GetLeaves(ref collector); + break; + } + } + } + + // Get a leaf of a collider hierarchy. + // Return false if the key is not valid for the collider. + public static unsafe bool GetLeafCollider(Collider* root, RigidTransform rootTransform, ColliderKey key, out ChildCollider leaf) + { + leaf = new ChildCollider(root, rootTransform); + while (leaf.Collider != null) + { + if (!leaf.Collider->GetChild(ref key, out ChildCollider child)) + { + break; + } + leaf = new ChildCollider(leaf, child); + } + return (leaf.Collider == null || leaf.Collider->CollisionType == CollisionType.Convex); + } + + #endregion + + #region ICollidable + + // Calculate a bounding box around this collider. + public Aabb CalculateAabb() + { + return CalculateAabb(RigidTransform.identity); + } + + // Calculate a bounding box around this collider, at the given transform. + public unsafe Aabb CalculateAabb(RigidTransform transform) + { + fixed (Collider* collider = &this) + { + switch (collider->Type) + { + case ColliderType.Convex: + return ((ConvexCollider*)collider)->CalculateAabb(transform); + case ColliderType.Sphere: + return ((SphereCollider*)collider)->CalculateAabb(transform); + case ColliderType.Capsule: + return ((CapsuleCollider*)collider)->CalculateAabb(transform); + case ColliderType.Triangle: + case ColliderType.Quad: + return ((PolygonCollider*)collider)->CalculateAabb(transform); + case ColliderType.Box: + return ((BoxCollider*)collider)->CalculateAabb(transform); + case ColliderType.Mesh: + return ((MeshCollider*)collider)->CalculateAabb(transform); + case ColliderType.Compound: + return ((CompoundCollider*)collider)->CalculateAabb(transform); + default: + //Assert.IsTrue(Enum.IsDefined(typeof(ColliderType), collider->Type)); + return Aabb.Empty; + } + } + } + + // Cast a ray against this collider. + public bool CastRay(RaycastInput input) => QueryWrappers.RayCast(ref this, input); + public bool CastRay(RaycastInput input, out RaycastHit closestHit) => QueryWrappers.RayCast(ref this, input, out closestHit); + public bool CastRay(RaycastInput input, ref NativeList allHits) => QueryWrappers.RayCast(ref this, input, ref allHits); + public unsafe bool CastRay(RaycastInput input, ref T collector) where T : struct, ICollector + { + fixed (Collider* target = &this) + { + return RaycastQueries.RayCollider(input, target, ref collector); + } + } + + // Cast another collider against this one. + public bool CastCollider(ColliderCastInput input) => QueryWrappers.ColliderCast(ref this, input); + public bool CastCollider(ColliderCastInput input, out ColliderCastHit closestHit) => QueryWrappers.ColliderCast(ref this, input, out closestHit); + public bool CastCollider(ColliderCastInput input, ref NativeList allHits) => QueryWrappers.ColliderCast(ref this, input, ref allHits); + public unsafe bool CastCollider(ColliderCastInput input, ref T collector) where T : struct, ICollector + { + fixed (Collider* target = &this) + { + return ColliderCastQueries.ColliderCollider(input, target, ref collector); + } + } + + // Calculate the distance from a point to this collider. + public bool CalculateDistance(PointDistanceInput input) => QueryWrappers.CalculateDistance(ref this, input); + public bool CalculateDistance(PointDistanceInput input, out DistanceHit closestHit) => QueryWrappers.CalculateDistance(ref this, input, out closestHit); + public bool CalculateDistance(PointDistanceInput input, ref NativeList allHits) => QueryWrappers.CalculateDistance(ref this, input, ref allHits); + public unsafe bool CalculateDistance(PointDistanceInput input, ref T collector) where T : struct, ICollector + { + fixed (Collider* target = &this) + { + return DistanceQueries.PointCollider(input, target, ref collector); + } + } + + // Calculate the distance from another collider to this one. + public bool CalculateDistance(ColliderDistanceInput input) => QueryWrappers.CalculateDistance(ref this, input); + public bool CalculateDistance(ColliderDistanceInput input, out DistanceHit closestHit) => QueryWrappers.CalculateDistance(ref this, input, out closestHit); + public bool CalculateDistance(ColliderDistanceInput input, ref NativeList allHits) => QueryWrappers.CalculateDistance(ref this, input, ref allHits); + public unsafe bool CalculateDistance(ColliderDistanceInput input, ref T collector) where T : struct, ICollector + { + fixed (Collider* target = &this) + { + return DistanceQueries.ColliderCollider(input, target, ref collector); + } + } + + #endregion + } + + // Header common to all colliders + public struct ColliderHeader + { + public ColliderType Type; + public CollisionType CollisionType; + public byte Version; // increment whenever the collider data has changed + public byte Magic; // always = 0xff (for validation) + + public CollisionFilter Filter; + } + + // Header common to all convex colliders + public struct ConvexColliderHeader + { + public ColliderType Type; + public CollisionType CollisionType; + public byte Version; + public byte Magic; + + public CollisionFilter Filter; + public Material Material; + } + + // An opaque key which packs a path to a specific leaf of a collider hierarchy into a single integer. + public struct ColliderKey : IEquatable + { + public uint Value { get; internal set; } + + public static readonly ColliderKey Empty = new ColliderKey { Value = uint.MaxValue }; + + internal ColliderKey(uint numSubKeyBits, uint subKey) + { + Value = uint.MaxValue; + PushSubKey(numSubKeyBits, subKey); + } + + public bool Equals(ColliderKey other) + { + return Value == other.Value; + } + + // Append a sub key to the front of the path + // "numSubKeyBits" is the maximum number of bits required to store any value for this sub key. + // Returns false if the key is empty. + public void PushSubKey(uint numSubKeyBits, uint subKey) + { + uint parentPart = (uint)((ulong)subKey << 32 - (int)numSubKeyBits); + uint childPart = Value >> (int)numSubKeyBits; + Value = parentPart | childPart; + } + + // Extract a sub key from the front of the path. + // "numSubKeyBits" is the maximum number of bits required to store any value for this sub key. + // Returns false if the key is empty. + public bool PopSubKey(uint numSubKeyBits, out uint subKey) + { + if (Value != uint.MaxValue) + { + subKey = Value >> (32 - (int)numSubKeyBits); + Value = ((1 + Value) << (int)numSubKeyBits) - 1; + return true; + } + + subKey = uint.MaxValue; + return false; + } + } + + // Stores a ColliderKey along with the number of bits in it that are used. + // This is useful for building keys from root to leaf, the bit count shows where to place the child key bits + public struct ColliderKeyPath + { + private ColliderKey m_Key; + private uint m_NumKeyBits; + + public ColliderKey Key => m_Key; + + public static ColliderKeyPath Empty => new ColliderKeyPath(ColliderKey.Empty, 0); + + public ColliderKeyPath(ColliderKey key, uint numKeyBits) + { + m_Key = key; + m_NumKeyBits = numKeyBits; + } + + // Append the local key for a child of the shape referenced by this path + public void PushChildKey(ColliderKeyPath child) + { + m_Key.Value &= (uint)(child.m_Key.Value >> (int)m_NumKeyBits | (ulong)0xffffffff << (int)(32 - m_NumKeyBits)); + m_NumKeyBits += child.m_NumKeyBits; + } + + // Remove the most leafward shape's key from this path + public void PopChildKey(uint numChildKeyBits) + { + m_NumKeyBits -= numChildKeyBits; + m_Key.Value |= (uint)((ulong)0xffffffff >> (int)m_NumKeyBits); + } + + // Get the collider key for a leaf shape that is a child of the shape referenced by this path + public ColliderKey GetLeafKey(ColliderKey leafKeyLocal) + { + ColliderKeyPath leafPath = this; + leafPath.PushChildKey(new ColliderKeyPath(leafKeyLocal, 0)); + return leafPath.Key; + } + } + + // A pair of collider keys. + public struct ColliderKeyPair + { + // B before A for consistency with other pairs + public ColliderKey ColliderKeyB; + public ColliderKey ColliderKeyA; + + public static readonly ColliderKeyPair Empty = new ColliderKeyPair { ColliderKeyB = ColliderKey.Empty, ColliderKeyA = ColliderKey.Empty }; + } + + // A child/leaf collider. + public unsafe struct ChildCollider + { + private readonly Collider* m_Collider; // if null, the result is in "Polygon" instead + private PolygonCollider m_Polygon; + + // The transform of the child collider in whatever space it was queried from + public RigidTransform TransformFromChild; + + public unsafe Collider* Collider + { + get + { + //Assert.IsTrue(m_Collider != null || m_Polygon.Vertices.Length > 0, "Accessing uninitialized Collider"); + fixed (ChildCollider* self = &this) + { + return (self->m_Collider != null) ? (Collider*)self->m_Collider : (Collider*)&self->m_Polygon; + } + } + } + + // Create from collider + public ChildCollider(Collider* collider) + { + m_Collider = collider; + m_Polygon = new PolygonCollider(); + TransformFromChild = new RigidTransform(quaternion.identity, float3.zero); + } + + // Create from body + public ChildCollider(Collider* collider, RigidTransform transform) + { + m_Collider = collider; + m_Polygon = new PolygonCollider(); + TransformFromChild = transform; + } + + // Create as triangle, from 3 vertices + public ChildCollider(float3 a, float3 b, float3 c, CollisionFilter filter, Material material) + { + m_Collider = null; + m_Polygon = new PolygonCollider(); + m_Polygon.InitAsTriangle(a, b, c, filter, material); + TransformFromChild = new RigidTransform(quaternion.identity, float3.zero); + } + + // Create as quad, from 4 coplanar vertices + public ChildCollider(float3 a, float3 b, float3 c, float3 d, CollisionFilter filter, Material material) + { + m_Collider = null; + m_Polygon = new PolygonCollider(); + m_Polygon.InitAsQuad(a, b, c, d, filter, material); + TransformFromChild = new RigidTransform(quaternion.identity, float3.zero); + } + + // Combine a parent ChildCollider with another ChildCollider describing one of its children + public ChildCollider(ChildCollider parent, ChildCollider child) + { + m_Collider = child.m_Collider; + m_Polygon = child.m_Polygon; + TransformFromChild = math.mul(parent.TransformFromChild, child.TransformFromChild); + } + } +} diff --git a/package/Unity.Physics/Collision/Colliders/Physics_Collider.cs.meta b/package/Unity.Physics/Collision/Colliders/Physics_Collider.cs.meta new file mode 100755 index 000000000..1a3aca18b --- /dev/null +++ b/package/Unity.Physics/Collision/Colliders/Physics_Collider.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 9352fc7b395c33348bbc0ba7168c141b +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Unity.Physics/Collision/Colliders/Physics_MeshCollider.cs b/package/Unity.Physics/Collision/Colliders/Physics_MeshCollider.cs new file mode 100755 index 000000000..0a39646d8 --- /dev/null +++ b/package/Unity.Physics/Collision/Colliders/Physics_MeshCollider.cs @@ -0,0 +1,275 @@ +using System; +using System.Collections.Generic; +using System.Runtime.InteropServices; +using Unity.Collections; +using Unity.Collections.LowLevel.Unsafe; +using Unity.Mathematics; +using Unity.Entities; + +namespace Unity.Physics +{ + // A collider representing a mesh comprised of triangles and quads. + // Warning: This is just the header, it is followed by variable sized data in memory. + // Therefore this struct must always be passed by reference, never by value. + public struct MeshCollider : ICompositeCollider + { + private ColliderHeader m_Header; + private Aabb m_Aabb; + public Mesh Mesh; + + // followed by variable sized mesh data + + #region Construction + + // Create a mesh collider asset from a set of triangles + public static unsafe BlobAssetReference Create(float3[] vertices, int[] indices, CollisionFilter? filter = null, Material? material = null) + { + int numVertices = vertices.Length; + int numIndices = indices.Length; + int numTriangles = numIndices / 3; + + // Copy vertices + float3[] tempVertices = new float3[numVertices]; + Array.Copy(vertices, tempVertices, numVertices); + + // Copy indices + int[] tempIndices = new int[numIndices]; + for (int iTriangle = 0; iTriangle < numTriangles; iTriangle++) + { + int iIndex0 = iTriangle * 3; + int iIndex1 = iIndex0 + 1; + int iIndex2 = iIndex0 + 2; + tempIndices[iIndex0] = indices[iIndex0]; + tempIndices[iIndex1] = indices[iIndex1]; + tempIndices[iIndex2] = indices[iIndex2]; + } + + // Build connectivity and primitives + List primitives = null; + { + MeshConnectivityBuilder.WeldVertices(tempIndices, ref tempVertices); + var connectivity = new MeshConnectivityBuilder(tempIndices, tempVertices); + primitives = connectivity.EnumerateQuadDominantGeometry(tempIndices, tempVertices); + } + + // Build bounding volume hierarchy + var nodes = new NativeArray(primitives.Count * 2 + 1, Allocator.Temp); + int numNodes = 0; + { + // Prepare data for BVH + var points = new NativeArray(primitives.Count, Allocator.Temp); + var aabbs = new NativeArray(primitives.Count, Allocator.Temp); + + for (int i = 0; i < primitives.Count; i++) + { + MeshConnectivityBuilder.Primitive p = primitives[i]; + + // Skip degenerate triangles + if (MeshConnectivityBuilder.IsTriangleDegenerate(p.Vertices[0], p.Vertices[1], p.Vertices[2])) + { + continue; + } + + aabbs[i] = Aabb.CreateFromPoints(p.Vertices); + points[i] = new BoundingVolumeHierarchy.PointAndIndex + { + Position = aabbs[i].Center, + Index = i + }; + } + + var bvh = new BoundingVolumeHierarchy(nodes); + bvh.Build(points, aabbs, out numNodes, useSah: true); + + points.Dispose(); + aabbs.Dispose(); + } + + // Build mesh sections + BoundingVolumeHierarchy.Node* nodesPtr = (BoundingVolumeHierarchy.Node*)nodes.GetUnsafePtr(); + List sections = MeshBuilder.BuildSections(nodesPtr, numNodes, primitives); + + // Allocate collider + int meshDataSize = Mesh.CalculateMeshDataSize(numNodes, sections); + int totalColliderSize = Math.NextMultipleOf(sizeof(MeshCollider), 16) + meshDataSize; + MeshCollider* meshCollider = (MeshCollider*)UnsafeUtility.Malloc(totalColliderSize, 16, Allocator.Temp); + + // Initialize it + { + UnsafeUtility.MemClear(meshCollider, totalColliderSize); + meshCollider->MemorySize = totalColliderSize; + + meshCollider->m_Header.Type = ColliderType.Mesh; + meshCollider->m_Header.CollisionType = CollisionType.Composite; + meshCollider->m_Header.Version += 1; + meshCollider->m_Header.Magic = 0xff; + + ref var mesh = ref meshCollider->Mesh; + + mesh.Init(nodesPtr, numNodes, sections, filter ?? CollisionFilter.Default, material ?? Material.Default); + + // Calculate combined filter + meshCollider->m_Header.Filter = mesh.Sections[0].Filters[0]; + for (int i = 0; i < mesh.Sections.Length; ++i) + { + foreach (CollisionFilter f in mesh.Sections[i].Filters) + { + meshCollider->m_Header.Filter = CollisionFilter.CreateUnion(meshCollider->m_Header.Filter, f); + } + } + + meshCollider->m_Aabb = meshCollider->Mesh.BoundingVolumeHierarchy.Domain; + meshCollider->NumColliderKeyBits = meshCollider->Mesh.NumColliderKeyBits; + } + + nodes.Dispose(); + + // Copy collider into blob + // TODO: Allocate it directly into blob instead + byte[] bytes = new byte[totalColliderSize]; + Marshal.Copy((IntPtr)meshCollider, bytes, 0, totalColliderSize); + UnsafeUtility.Free(meshCollider, Allocator.Temp); + return BlobAssetReference.Create(bytes); + } + + #endregion + + #region ICompositeCollider + + public ColliderType Type => m_Header.Type; + public CollisionType CollisionType => m_Header.CollisionType; + public int MemorySize { get; private set; } + + public CollisionFilter Filter => m_Header.Filter; + + public MassProperties MassProperties + { + get + { + // Rough approximation based on AABB + float3 size = m_Aabb.Extents; + return new MassProperties + { + MassDistribution = new MassDistribution + { + Transform = new RigidTransform(quaternion.identity, m_Aabb.Center), + InertiaTensor = new float3( + (size.y * size.y + size.z * size.z) / 12.0f, + (size.x * size.x + size.z * size.z) / 12.0f, + (size.x * size.x + size.y * size.y) / 12.0f) + }, + Volume = 0, + AngularExpansionFactor = math.length(m_Aabb.Extents) * 0.5f + }; + } + } + + public Aabb CalculateAabb() + { + return m_Aabb; + } + + public Aabb CalculateAabb(RigidTransform transform) + { + // TODO: Store a convex hull wrapping the mesh, and use that to calculate tighter AABBs? + return Math.TransformAabb(transform, m_Aabb); + } + + // Cast a ray against this collider. + public bool CastRay(RaycastInput input) => QueryWrappers.RayCast(ref this, input); + public bool CastRay(RaycastInput input, out RaycastHit closestHit) => QueryWrappers.RayCast(ref this, input, out closestHit); + public bool CastRay(RaycastInput input, ref NativeList allHits) => QueryWrappers.RayCast(ref this, input, ref allHits); + public unsafe bool CastRay(RaycastInput input, ref T collector) where T : struct, ICollector + { + fixed (MeshCollider* target = &this) + { + return RaycastQueries.RayCollider(input, (Collider*)target, ref collector); + } + } + + // Cast another collider against this one. + public bool CastCollider(ColliderCastInput input) => QueryWrappers.ColliderCast(ref this, input); + public bool CastCollider(ColliderCastInput input, out ColliderCastHit closestHit) => QueryWrappers.ColliderCast(ref this, input, out closestHit); + public bool CastCollider(ColliderCastInput input, ref NativeList allHits) => QueryWrappers.ColliderCast(ref this, input, ref allHits); + public unsafe bool CastCollider(ColliderCastInput input, ref T collector) where T : struct, ICollector + { + fixed (MeshCollider* target = &this) + { + return ColliderCastQueries.ColliderCollider(input, (Collider*)target, ref collector); + } + } + + // Calculate the distance from a point to this collider. + public bool CalculateDistance(PointDistanceInput input) => QueryWrappers.CalculateDistance(ref this, input); + public bool CalculateDistance(PointDistanceInput input, out DistanceHit closestHit) => QueryWrappers.CalculateDistance(ref this, input, out closestHit); + public bool CalculateDistance(PointDistanceInput input, ref NativeList allHits) => QueryWrappers.CalculateDistance(ref this, input, ref allHits); + public unsafe bool CalculateDistance(PointDistanceInput input, ref T collector) where T : struct, ICollector + { + fixed (MeshCollider* target = &this) + { + return DistanceQueries.PointCollider(input, (Collider*)target, ref collector); + } + } + + // Calculate the distance from another collider to this one. + public bool CalculateDistance(ColliderDistanceInput input) => QueryWrappers.CalculateDistance(ref this, input); + public bool CalculateDistance(ColliderDistanceInput input, out DistanceHit closestHit) => QueryWrappers.CalculateDistance(ref this, input, out closestHit); + public bool CalculateDistance(ColliderDistanceInput input, ref NativeList allHits) => QueryWrappers.CalculateDistance(ref this, input, ref allHits); + public unsafe bool CalculateDistance(ColliderDistanceInput input, ref T collector) where T : struct, ICollector + { + fixed (MeshCollider* target = &this) + { + return DistanceQueries.ColliderCollider(input, (Collider*)target, ref collector); + } + } + + public uint NumColliderKeyBits { get; private set; } + + public bool GetChild(ref ColliderKey key, out ChildCollider child) + { + if (key.PopSubKey(NumColliderKeyBits, out uint subKey)) + { + int primitiveKey = (int)(subKey >> 1); + int polygonIndex = (int)(subKey & 1); + + Mesh.GetPrimitive(primitiveKey, out float3x4 vertices, out Mesh.PrimitiveFlags flags, out CollisionFilter filter, out Material material); + + if (Mesh.IsPrimitveFlagSet(flags, Mesh.PrimitiveFlags.IsQuad)) + { + child = new ChildCollider(vertices[0], vertices[1], vertices[2], vertices[3], filter, material); + } + else + { + child = new ChildCollider(vertices[0], vertices[1 + polygonIndex], vertices[2 + polygonIndex], filter, material); + } + + return true; + } + + child = new ChildCollider(); + return false; + } + + public bool GetLeaf(ColliderKey key, out ChildCollider leaf) + { + return GetChild(ref key, out leaf); + } + + public unsafe void GetLeaves(ref T collector) where T : struct, ILeafColliderCollector + { + var polygon = new PolygonCollider(); + polygon.InitEmpty(); + if (Mesh.GetFirstPolygon(out uint meshKey, ref polygon)) + { + do + { + var leaf = new ChildCollider((Collider*)&polygon, RigidTransform.identity); + collector.AddLeaf(new ColliderKey(NumColliderKeyBits, meshKey), ref leaf); + } + while (Mesh.GetNextPolygon(meshKey, out meshKey, ref polygon)); + } + } + + #endregion + } +} diff --git a/package/Unity.Physics/Collision/Colliders/Physics_MeshCollider.cs.meta b/package/Unity.Physics/Collision/Colliders/Physics_MeshCollider.cs.meta new file mode 100755 index 000000000..8fdc042aa --- /dev/null +++ b/package/Unity.Physics/Collision/Colliders/Physics_MeshCollider.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 85fcd7cb4600caf479471441afd9e328 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Unity.Physics/Collision/Colliders/Physics_SphereCollider.cs b/package/Unity.Physics/Collision/Colliders/Physics_SphereCollider.cs new file mode 100755 index 000000000..3b8f5ead9 --- /dev/null +++ b/package/Unity.Physics/Collision/Colliders/Physics_SphereCollider.cs @@ -0,0 +1,149 @@ +using Unity.Collections; +using Unity.Collections.LowLevel.Unsafe; +using Unity.Mathematics; +using Unity.Entities; + +namespace Unity.Physics +{ + // A collider in the shape of a sphere + public struct SphereCollider : IConvexCollider + { + // Header + private ConvexColliderHeader m_Header; + internal ConvexHull ConvexHull; + + private float3 m_Vertex; + + public float3 Center { get => m_Vertex; set => m_Vertex = value; } + public float Radius { get => ConvexHull.ConvexRadius; set => ConvexHull.ConvexRadius = value; } + + #region Construction + + public static BlobAssetReference Create(float3 center, float radius, CollisionFilter? filter = null, Material? material = null) + { + if (math.any(!math.isfinite(center))) + { + throw new System.ArgumentException("Tried to create sphere collider with inf/nan center"); + } + if (!math.isfinite(radius) || radius <= 0.0f) + { + throw new System.ArgumentException("Tried to create sphere collider with negative/inf/nan radius"); + } + + var allocator = new BlobAllocator(-1); + ref SphereCollider collider = ref allocator.ConstructRoot(); + collider.Init(center, radius, filter ?? CollisionFilter.Default, material ?? Material.Default); + var sphereCollider = allocator.CreateBlobAssetReference(Allocator.Persistent); + allocator.Dispose(); + return sphereCollider; + } + + internal unsafe void Init(float3 center, float radius, CollisionFilter filter, Material material) + { + m_Header.Type = ColliderType.Sphere; + m_Header.CollisionType = CollisionType.Convex; + m_Header.Version += 1; + m_Header.Magic = 0xff; + m_Header.Filter = filter; + m_Header.Material = material; + + ConvexHull.ConvexRadius = radius; + ConvexHull.VerticesBlob.Offset = (int)((byte*)UnsafeUtility.AddressOf(ref m_Vertex) - (byte*)UnsafeUtility.AddressOf(ref ConvexHull.VerticesBlob.Offset)); + ConvexHull.VerticesBlob.Length = 1; + // note: no faces + + m_Vertex = center; + } + + #endregion + + #region IConvexCollider + + public ColliderType Type => m_Header.Type; + public CollisionType CollisionType => m_Header.CollisionType; + public int MemorySize => UnsafeUtility.SizeOf(); + + public CollisionFilter Filter { get => m_Header.Filter; set => m_Header.Filter = value; } + public Material Material { get => m_Header.Material; set => m_Header.Material = value; } + + public MassProperties MassProperties => new MassProperties + { + MassDistribution = new MassDistribution + { + Transform = new RigidTransform(quaternion.identity, Center), + InertiaTensor = new float3(2.0f / 5.0f * Radius * Radius) + }, + Volume = (4.0f / 3.0f) * (float)math.PI * Radius * Radius * Radius, + AngularExpansionFactor = 0.0f + }; + + public Aabb CalculateAabb() + { + return new Aabb + { + Min = Center - new float3(Radius), + Max = Center + new float3(Radius) + }; + } + + public Aabb CalculateAabb(RigidTransform transform) + { + float3 centerInWorld = math.transform(transform, Center); + return new Aabb + { + Min = centerInWorld - new float3(Radius), + Max = centerInWorld + new float3(Radius) + }; + } + + // Cast a ray against this collider. + public bool CastRay(RaycastInput input) => QueryWrappers.RayCast(ref this, input); + public bool CastRay(RaycastInput input, out RaycastHit closestHit) => QueryWrappers.RayCast(ref this, input, out closestHit); + public bool CastRay(RaycastInput input, ref NativeList allHits) => QueryWrappers.RayCast(ref this, input, ref allHits); + public unsafe bool CastRay(RaycastInput input, ref T collector) where T : struct, ICollector + { + fixed (SphereCollider* target = &this) + { + return RaycastQueries.RayCollider(input, (Collider*)target, ref collector); + } + } + + // Cast another collider against this one. + public bool CastCollider(ColliderCastInput input) => QueryWrappers.ColliderCast(ref this, input); + public bool CastCollider(ColliderCastInput input, out ColliderCastHit closestHit) => QueryWrappers.ColliderCast(ref this, input, out closestHit); + public bool CastCollider(ColliderCastInput input, ref NativeList allHits) => QueryWrappers.ColliderCast(ref this, input, ref allHits); + public unsafe bool CastCollider(ColliderCastInput input, ref T collector) where T : struct, ICollector + { + fixed (SphereCollider* target = &this) + { + return ColliderCastQueries.ColliderCollider(input, (Collider*)target, ref collector); + } + } + + // Calculate the distance from a point to this collider. + public bool CalculateDistance(PointDistanceInput input) => QueryWrappers.CalculateDistance(ref this, input); + public bool CalculateDistance(PointDistanceInput input, out DistanceHit closestHit) => QueryWrappers.CalculateDistance(ref this, input, out closestHit); + public bool CalculateDistance(PointDistanceInput input, ref NativeList allHits) => QueryWrappers.CalculateDistance(ref this, input, ref allHits); + public unsafe bool CalculateDistance(PointDistanceInput input, ref T collector) where T : struct, ICollector + { + fixed (SphereCollider* target = &this) + { + return DistanceQueries.PointCollider(input, (Collider*)target, ref collector); + } + } + + // Calculate the distance from another collider to this one. + public bool CalculateDistance(ColliderDistanceInput input) => QueryWrappers.CalculateDistance(ref this, input); + public bool CalculateDistance(ColliderDistanceInput input, out DistanceHit closestHit) => QueryWrappers.CalculateDistance(ref this, input, out closestHit); + public bool CalculateDistance(ColliderDistanceInput input, ref NativeList allHits) => QueryWrappers.CalculateDistance(ref this, input, ref allHits); + public unsafe bool CalculateDistance(ColliderDistanceInput input, ref T collector) where T : struct, ICollector + { + fixed (SphereCollider* target = &this) + { + return DistanceQueries.ColliderCollider(input, (Collider*)target, ref collector); + } + } + + #endregion + } +} diff --git a/package/Unity.Physics/Collision/Colliders/Physics_SphereCollider.cs.meta b/package/Unity.Physics/Collision/Colliders/Physics_SphereCollider.cs.meta new file mode 100755 index 000000000..acc88c455 --- /dev/null +++ b/package/Unity.Physics/Collision/Colliders/Physics_SphereCollider.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 2387c719c7b0ce14b98f8299ce776499 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Unity.Physics/Collision/Colliders/PolygonCollider.cs b/package/Unity.Physics/Collision/Colliders/PolygonCollider.cs new file mode 100755 index 000000000..93c19d3e2 --- /dev/null +++ b/package/Unity.Physics/Collision/Colliders/PolygonCollider.cs @@ -0,0 +1,298 @@ +using Unity.Collections; +using Unity.Collections.LowLevel.Unsafe; +using Unity.Mathematics; +using Unity.Entities; + +namespace Unity.Physics +{ + // A flat convex collider with either 3 or 4 coplanar vertices (ie, a triangle or a quad) + public struct PolygonCollider : IConvexCollider + { + // Header + private ConvexColliderHeader m_Header; + internal ConvexHull ConvexHull; + + // Convex hull data + // Todo: would be nice to use the actual types here but C# only likes fixed arrays of builtin types + private unsafe fixed byte m_Vertices[sizeof(float) * 3 * 4]; // float3[4] + private unsafe fixed byte m_FacePlanes[sizeof(float) * 4 * 2]; // Plane[2] + private unsafe fixed byte m_Faces[4 * 2]; // ConvexHull.Face[2] + private unsafe fixed byte m_FaceVertexIndices[sizeof(byte) * 8]; // byte[8] + + public bool IsTriangle => Vertices.Length == 3; + public bool IsQuad => Vertices.Length == 4; + + public BlobArray.Accessor Vertices => ConvexHull.Vertices; + public BlobArray.Accessor Planes => ConvexHull.Planes; + + #region Construction + + public static BlobAssetReference CreateTriangle(float3 vertex0, float3 vertex1, float3 vertex2, CollisionFilter? filter = null, Material? material = null) + { + if (math.any(!math.isfinite(vertex0)) || math.any(!math.isfinite(vertex1)) || math.any(!math.isfinite(vertex2))) + { + throw new System.ArgumentException("Tried to create triangle collider with nan/inf vertex"); + } + using (var allocator = new BlobAllocator(-1)) + { + ref PolygonCollider collider = ref allocator.ConstructRoot(); + collider.InitAsTriangle(vertex0, vertex1, vertex2, filter ?? CollisionFilter.Default, material ?? Material.Default); + return allocator.CreateBlobAssetReference(Allocator.Persistent); + } + } + + // Ray casting assumes vertices are presented in clockwise order + public static BlobAssetReference CreateQuad(float3 vertex0, float3 vertex1, float3 vertex2, float3 vertex3, CollisionFilter? filter = null, Material? material = null) + { + if (math.any(!math.isfinite(vertex0)) || math.any(!math.isfinite(vertex1)) || math.any(!math.isfinite(vertex2)) || math.any(!math.isfinite(vertex3))) + { + throw new System.ArgumentException("Tried to create triangle collider with nan/inf vertex"); + } + + // check if vertices are co-planar + float3 normal = math.normalize(math.cross(vertex1 - vertex0, vertex2 - vertex0)); + if (math.abs(math.dot(normal, vertex3 - vertex0)) > 1e-3f) + { + throw new System.ArgumentException("Vertices for quad creation are not co-planar"); + } + + using (var allocator = new BlobAllocator(-1)) + { + ref PolygonCollider collider = ref allocator.ConstructRoot(); + collider.InitAsQuad(vertex0, vertex1, vertex2, vertex3, filter ?? CollisionFilter.Default, material ?? Material.Default); + return allocator.CreateBlobAssetReference(Allocator.Persistent); + } + } + + internal void InitEmpty() + { + Init(CollisionFilter.Default, Material.Default); + ConvexHull.VerticesBlob.Length = 0; + } + + internal void InitAsTriangle(float3 vertex0, float3 vertex1, float3 vertex2, CollisionFilter filter, Material material) + { + Init(filter, material); + SetAsTriangle(vertex0, vertex1, vertex2); + } + + internal void InitAsQuad(float3 vertex0, float3 vertex1, float3 vertex2, float3 vertex3, CollisionFilter filter, Material material) + { + Init(filter, material); + SetAsQuad(vertex0, vertex1, vertex2, vertex3); + } + + internal unsafe void SetAsTriangle(float3 v0, float3 v1, float3 v2) + { + m_Header.Type = ColliderType.Triangle; + m_Header.Version += 1; + + ConvexHull.VerticesBlob.Length = 3; + ConvexHull.FaceVertexIndicesBlob.Length = 6; + + fixed (PolygonCollider* collider = &this) + { + float3* vertices = (float3*)(&collider->m_Vertices[0]); + vertices[0] = v0; + vertices[1] = v1; + vertices[2] = v2; + + ConvexHull.Face* faces = (ConvexHull.Face*)(&collider->m_Faces[0]); + faces[0] = new ConvexHull.Face { FirstIndex = 0, NumVertices = 3, MinHalfAngle = 0xff }; + faces[1] = new ConvexHull.Face { FirstIndex = 3, NumVertices = 3, MinHalfAngle = 0xff }; + + byte* index = &collider->m_FaceVertexIndices[0]; + *index++ = 0; *index++ = 1; *index++ = 2; + *index++ = 2; *index++ = 1; *index++ = 0; + } + + SetPlanes(); + } + + internal unsafe void SetAsQuad(float3 v0, float3 v1, float3 v2, float3 v3) + { + m_Header.Type = ColliderType.Quad; + m_Header.Version += 1; + + ConvexHull.VerticesBlob.Length = 4; + ConvexHull.FaceVertexIndicesBlob.Length = 8; + + fixed (PolygonCollider* collider = &this) + { + float3* vertices = (float3*)(&collider->m_Vertices[0]); + vertices[0] = v0; + vertices[1] = v1; + vertices[2] = v2; + vertices[3] = v3; + + ConvexHull.Face* faces = (ConvexHull.Face*)(&collider->m_Faces[0]); + faces[0] = new ConvexHull.Face { FirstIndex = 0, NumVertices = 4, MinHalfAngle = 0xff }; + faces[1] = new ConvexHull.Face { FirstIndex = 4, NumVertices = 4, MinHalfAngle = 0xff }; + + byte* index = &collider->m_FaceVertexIndices[0]; + *index++ = 0; *index++ = 1; *index++ = 2; *index++ = 3; + *index++ = 3; *index++ = 2; *index++ = 1; *index++ = 0; + } + + SetPlanes(); + } + + private unsafe void Init(CollisionFilter filter, Material material) + { + m_Header.CollisionType = CollisionType.Convex; + m_Header.Version = 0; + m_Header.Magic = 0xff; + m_Header.Filter = filter; + m_Header.Material = material; + + ConvexHull.ConvexRadius = 0.0f; + + fixed (PolygonCollider* collider = &this) + { + ConvexHull.VerticesBlob.Offset = (int)((byte*)UnsafeUtility.AddressOf(ref collider->m_Vertices[0]) - (byte*)UnsafeUtility.AddressOf(ref ConvexHull.VerticesBlob.Offset)); + ConvexHull.VerticesBlob.Length = 4; + + ConvexHull.FacePlanesBlob.Offset = (int)((byte*)UnsafeUtility.AddressOf(ref collider->m_FacePlanes[0]) - (byte*)UnsafeUtility.AddressOf(ref ConvexHull.FacePlanesBlob.Offset)); + ConvexHull.FacePlanesBlob.Length = 2; + + ConvexHull.FacesBlob.Offset = (int)((byte*)UnsafeUtility.AddressOf(ref collider->m_Faces[0]) - (byte*)UnsafeUtility.AddressOf(ref ConvexHull.FacesBlob.Offset)); + ConvexHull.FacesBlob.Length = 2; + + ConvexHull.FaceVertexIndicesBlob.Offset = (int)((byte*)UnsafeUtility.AddressOf(ref collider->m_FaceVertexIndices[0]) - (byte*)UnsafeUtility.AddressOf(ref ConvexHull.FaceVertexIndicesBlob.Offset)); + ConvexHull.FaceVertexIndicesBlob.Length = 8; + + // No connectivity needed + ConvexHull.VertexEdgesBlob.Offset = 0; + ConvexHull.VertexEdgesBlob.Length = 0; + ConvexHull.FaceLinksBlob.Offset = 0; + ConvexHull.FaceLinksBlob.Length = 0; + } + } + + private void SetPlanes() + { + BlobArray.Accessor hullVertices = ConvexHull.Vertices; + float3 cross = math.cross( + hullVertices[1] - hullVertices[0], + hullVertices[2] - hullVertices[0]); + float dot = math.dot(cross, hullVertices[0]); + float invLengthCross = math.rsqrt(math.lengthsq(cross)); + Plane plane = new Plane(cross * invLengthCross, -dot * invLengthCross); + + ConvexHull.Planes[0] = plane; + ConvexHull.Planes[1] = plane.Flipped; + } + + #endregion + + #region IConvexCollider + + public ColliderType Type => m_Header.Type; + public CollisionType CollisionType => m_Header.CollisionType; + public int MemorySize => UnsafeUtility.SizeOf(); + + public CollisionFilter Filter { get => m_Header.Filter; set { m_Header.Filter = value; } } + public Material Material { get => m_Header.Material; set { m_Header.Material = value; } } + + public MassProperties MassProperties + { + get + { + // TODO - the inertia computed here is incorrect. Computing the correct inertia is expensive, so it probably ought to be cached. + // Note this is only called for top level polygon colliders, not for polygons within a mesh. + float3 center = (ConvexHull.Vertices[0] + ConvexHull.Vertices[1] + ConvexHull.Vertices[2]) / 3.0f; + float radiusSq = math.max(math.max( + math.lengthsq(ConvexHull.Vertices[0] - center), + math.lengthsq(ConvexHull.Vertices[1] - center)), + math.lengthsq(ConvexHull.Vertices[2] - center)); + return new MassProperties + { + MassDistribution = new MassDistribution + { + Transform = new RigidTransform(quaternion.identity, center), + InertiaTensor = new float3(2.0f / 5.0f * radiusSq) + }, + Volume = 0, + AngularExpansionFactor = math.sqrt(radiusSq) + }; + } + } + + public Aabb CalculateAabb() + { + float3 min = math.min(math.min(ConvexHull.Vertices[0], ConvexHull.Vertices[1]), math.min(ConvexHull.Vertices[2], ConvexHull.Vertices[3])); + float3 max = math.max(math.max(ConvexHull.Vertices[0], ConvexHull.Vertices[1]), math.max(ConvexHull.Vertices[2], ConvexHull.Vertices[3])); + return new Aabb + { + Min = min - new float3(ConvexHull.ConvexRadius), + Max = max + new float3(ConvexHull.ConvexRadius) + }; + } + + public Aabb CalculateAabb(RigidTransform transform) + { + float3 v0 = math.rotate(transform, ConvexHull.Vertices[0]); + float3 v1 = math.rotate(transform, ConvexHull.Vertices[1]); + float3 v2 = math.rotate(transform, ConvexHull.Vertices[2]); + float3 v3 = IsQuad ? math.rotate(transform, ConvexHull.Vertices[3]) : v2; + + float3 min = math.min(math.min(v0, v1), math.min(v2, v3)); + float3 max = math.max(math.max(v0, v1), math.max(v2, v3)); + return new Aabb + { + Min = min + transform.pos - new float3(ConvexHull.ConvexRadius), + Max = max + transform.pos + new float3(ConvexHull.ConvexRadius) + }; + } + + // Cast a ray against this collider. + public bool CastRay(RaycastInput input) => QueryWrappers.RayCast(ref this, input); + public bool CastRay(RaycastInput input, out RaycastHit closestHit) => QueryWrappers.RayCast(ref this, input, out closestHit); + public bool CastRay(RaycastInput input, ref NativeList allHits) => QueryWrappers.RayCast(ref this, input, ref allHits); + public unsafe bool CastRay(RaycastInput input, ref T collector) where T : struct, ICollector + { + fixed (PolygonCollider* target = &this) + { + return RaycastQueries.RayCollider(input, (Collider*)target, ref collector); + } + } + + // Cast another collider against this one. + public bool CastCollider(ColliderCastInput input) => QueryWrappers.ColliderCast(ref this, input); + public bool CastCollider(ColliderCastInput input, out ColliderCastHit closestHit) => QueryWrappers.ColliderCast(ref this, input, out closestHit); + public bool CastCollider(ColliderCastInput input, ref NativeList allHits) => QueryWrappers.ColliderCast(ref this, input, ref allHits); + public unsafe bool CastCollider(ColliderCastInput input, ref T collector) where T : struct, ICollector + { + fixed (PolygonCollider* target = &this) + { + return ColliderCastQueries.ColliderCollider(input, (Collider*)target, ref collector); + } + } + + // Calculate the distance from a point to this collider. + public bool CalculateDistance(PointDistanceInput input) => QueryWrappers.CalculateDistance(ref this, input); + public bool CalculateDistance(PointDistanceInput input, out DistanceHit closestHit) => QueryWrappers.CalculateDistance(ref this, input, out closestHit); + public bool CalculateDistance(PointDistanceInput input, ref NativeList allHits) => QueryWrappers.CalculateDistance(ref this, input, ref allHits); + public unsafe bool CalculateDistance(PointDistanceInput input, ref T collector) where T : struct, ICollector + { + fixed (PolygonCollider* target = &this) + { + return DistanceQueries.PointCollider(input, (Collider*)target, ref collector); + } + } + + // Calculate the distance from another collider to this one. + public bool CalculateDistance(ColliderDistanceInput input) => QueryWrappers.CalculateDistance(ref this, input); + public bool CalculateDistance(ColliderDistanceInput input, out DistanceHit closestHit) => QueryWrappers.CalculateDistance(ref this, input, out closestHit); + public bool CalculateDistance(ColliderDistanceInput input, ref NativeList allHits) => QueryWrappers.CalculateDistance(ref this, input, ref allHits); + public unsafe bool CalculateDistance(ColliderDistanceInput input, ref T collector) where T : struct, ICollector + { + fixed (PolygonCollider* target = &this) + { + return DistanceQueries.ColliderCollider(input, (Collider*)target, ref collector); + } + } + + #endregion + } +} diff --git a/package/Unity.Physics/Collision/Colliders/PolygonCollider.cs.meta b/package/Unity.Physics/Collision/Colliders/PolygonCollider.cs.meta new file mode 100755 index 000000000..c520584a6 --- /dev/null +++ b/package/Unity.Physics/Collision/Colliders/PolygonCollider.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 89076eda5a01bef48af3cade362f455b +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Unity.Physics/Collision/Filter.meta b/package/Unity.Physics/Collision/Filter.meta new file mode 100755 index 000000000..e324a2b17 --- /dev/null +++ b/package/Unity.Physics/Collision/Filter.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: b108c05cd8d763248ab6d782c289e0b0 +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Unity.Physics/Collision/Filter/CollisionFilter.cs b/package/Unity.Physics/Collision/Filter/CollisionFilter.cs new file mode 100755 index 000000000..71321f0f7 --- /dev/null +++ b/package/Unity.Physics/Collision/Filter/CollisionFilter.cs @@ -0,0 +1,70 @@ +using System; +using System.Diagnostics; +using System.Runtime.CompilerServices; + +namespace Unity.Physics +{ + // Describes which other objects an object can collide with. + [DebuggerDisplay("Group: {GroupIndex} Category: {CategoryBits} Mask: {MaskBits}")] + public struct CollisionFilter + { + // A bit mask describing which layers this object belongs to. + public uint CategoryBits; // TODO rename? + + // A bit mask describing which layers this object can collide with. + public uint MaskBits; // TODO rename? + + // An optional override for the bit mask checks. + // If the value in both objects is equal and positive, the objects always collide. + // If the value in both objects is equal and negative, the objects never collide. + public int GroupIndex; + + // Return false if the filter cannot collide with anything, + // which likely means it was default constructed but not initialized. + public bool IsValid => CategoryBits > 0 && MaskBits > 0; + + // A collision filter which wants to collide with everything. + public static readonly CollisionFilter Default = new CollisionFilter + { + CategoryBits = 0xffffffff, + MaskBits = 0xffffffff, + GroupIndex = 0 + }; + + // A collision filter which never collides with against anything (including Default). + public static readonly CollisionFilter Zero = new CollisionFilter + { + CategoryBits = 0, + MaskBits = 0, + GroupIndex = 0 + }; + + // Return true if the given pair of filters want to collide with each other. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static bool IsCollisionEnabled(CollisionFilter filterA, CollisionFilter filterB) + { + if (filterA.GroupIndex > 0 && filterA.GroupIndex == filterB.GroupIndex) + { + return true; + } + if (filterA.GroupIndex < 0 && filterA.GroupIndex == filterB.GroupIndex) + { + return false; + } + return + (filterA.CategoryBits & filterB.MaskBits) != 0 && + (filterB.CategoryBits & filterA.MaskBits) != 0; + } + + // Return a union of two filters. + public static CollisionFilter CreateUnion(CollisionFilter filterA, CollisionFilter filterB) + { + return new CollisionFilter + { + CategoryBits = filterA.CategoryBits | filterB.CategoryBits, + MaskBits = filterA.MaskBits | filterB.MaskBits, + GroupIndex = (filterA.GroupIndex == filterB.GroupIndex) ? filterA.GroupIndex : 0 + }; + } + } +} diff --git a/package/Unity.Physics/Collision/Filter/CollisionFilter.cs.meta b/package/Unity.Physics/Collision/Filter/CollisionFilter.cs.meta new file mode 100755 index 000000000..dbdbf8296 --- /dev/null +++ b/package/Unity.Physics/Collision/Filter/CollisionFilter.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: f7ac5aa207b29ff47815e5ba3c81cc04 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Unity.Physics/Collision/Geometry.meta b/package/Unity.Physics/Collision/Geometry.meta new file mode 100755 index 000000000..c3993f1e2 --- /dev/null +++ b/package/Unity.Physics/Collision/Geometry.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: d1708614f77857b4b9d2e1c9cc0c599f +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Unity.Physics/Collision/Geometry/BoundingVolumeHierarchy.cs b/package/Unity.Physics/Collision/Geometry/BoundingVolumeHierarchy.cs new file mode 100755 index 000000000..04b4648b3 --- /dev/null +++ b/package/Unity.Physics/Collision/Geometry/BoundingVolumeHierarchy.cs @@ -0,0 +1,559 @@ +using System.Diagnostics; +using System.Runtime.InteropServices; +using Unity.Collections; +using Unity.Collections.LowLevel.Unsafe; +using Unity.Mathematics; + +namespace Unity.Physics +{ + // A 4-way bounding volume hierarchy + public partial struct BoundingVolumeHierarchy + { + private readonly unsafe Node* m_Nodes; + private readonly unsafe CollisionFilter* m_NodeFilters; + + public unsafe Aabb Domain => m_Nodes[1].Bounds.GetCompoundAabb(); + + public unsafe BoundingVolumeHierarchy(Node* nodes, CollisionFilter* nodeFilters) + { + m_Nodes = nodes; + m_NodeFilters = nodeFilters; + } + + public unsafe BoundingVolumeHierarchy(NativeArray nodes, NativeArray nodeFilters) + { + m_Nodes = (Node*)nodes.GetUnsafeReadOnlyPtr(); + m_NodeFilters = (CollisionFilter*)nodeFilters.GetUnsafeReadOnlyPtr(); + } + + public unsafe BoundingVolumeHierarchy(NativeArray nodes) + { + m_Nodes = (Node*)nodes.GetUnsafeReadOnlyPtr(); + m_NodeFilters = null; + } + + // A node in the hierarchy + [DebuggerDisplay("{IsLeaf?\"Leaf\":\"Internal\"}, [ {Data[0]}, {Data[1]}, {Data[2]}, {Data[3]} ]")] + [StructLayout(LayoutKind.Sequential, Size = 128)] + public struct Node + { + public FourTransposedAabbs Bounds; + public int4 Data; + public int Flags; + + public static Node Empty => new Node + { + Bounds = FourTransposedAabbs.Empty, + Data = int4.zero, + IsLeaf = false + }; + + public bool IsInternal { get => Flags == 0; set => Flags = value ? 0 : 1; } + public bool IsLeaf { get => Flags != 0; set => Flags = value ? 1 : 0; } + + private static readonly int4 minusOne4i = new int4(-1); + + public bool4 AreLeavesValid => (Data != minusOne4i); + public bool4 AreInternalsValid => (Data != int4.zero); + + public bool IsChildValid(int index) + { + if (IsLeaf && Data[index] == -1) return false; + if (!IsLeaf && Data[index] == 0) return false; + return true; + } + + public int NumValidChildren() + { + int cnt = 0; + for (int i = 0; i < 4; i++) + { + cnt += IsChildValid(i) ? 1 : 0; + } + + return cnt; + } + + public bool IsLeafValid(int index) => Data[index] != -1; + public bool IsInternalValid(int index) => Data[index] != 0; + + public void ClearLeafData(int index) => Data[index] = -1; + public void ClearInternalData(int index) => Data[index] = 0; + } + + // Utility function + private static void Swap(ref T a, ref T b) where T : struct { T t = a; a = b; b = t; } + + #region Self overlap query + + public interface ITreeOverlapCollector + { + void AddPairs(int l, int4 r, int countR); + void AddPairs(int4 l, int4 r, int count); + void FlushIfNeeded(); + } + + public unsafe void BvhOverlap(ref T pairWriter, BoundingVolumeHierarchy other, int rootA = 1, int rootB = 1) where T : struct, ITreeOverlapCollector + { + TreeOverlap(ref pairWriter, m_Nodes, other.m_Nodes, m_NodeFilters, other.m_NodeFilters, rootA, rootB); + } + + public unsafe void SelfBvhOverlap(ref T pairWriter, int rootA = 1, int rootB = 1) where T : struct, ITreeOverlapCollector + { + TreeOverlap(ref pairWriter, m_Nodes, m_Nodes, m_NodeFilters, m_NodeFilters, rootA, rootB); + } + + public static unsafe void TreeOverlap( + ref T pairWriter, + Node* treeA, Node* treeB, + CollisionFilter* collisionFilterA = null, CollisionFilter* collisionFilterB = null, + int rootA = 1, int rootB = 1) where T : struct, ITreeOverlapCollector + { + int* binaryStackA = stackalloc int[Constants.BinaryStackSize]; + int* binaryStackB = stackalloc int[Constants.BinaryStackSize]; + int* stackA = binaryStackA; + int* stackB = binaryStackB; + + if (treeA == treeB && rootA == rootB) + { + int* unaryStack = stackalloc int[Constants.UnaryStackSize]; + int* stack = unaryStack; + *stack++ = rootA; + + do + { + int nodeIndex = *(--stack); + if (collisionFilterA == null || CollisionFilter.IsCollisionEnabled(collisionFilterA[nodeIndex], collisionFilterB[nodeIndex])) + { + ProcessAA(ref treeA[nodeIndex], ref stack, ref stackA, ref stackB, ref pairWriter); + } + pairWriter.FlushIfNeeded(); + + while (stackA > binaryStackA) + { + int nodeIndexA = *(--stackA); + int nodeIndexB = *(--stackB); + + if (collisionFilterA == null || CollisionFilter.IsCollisionEnabled(collisionFilterA[nodeIndexA], collisionFilterB[nodeIndexB])) + { + ProcessAB(ref treeA[nodeIndexA], ref treeA[nodeIndexB], treeA, treeA, ref stackA, ref stackB, ref pairWriter); + } + pairWriter.FlushIfNeeded(); + } + } while (stack > unaryStack); + } + else + { + *stackA++ = rootA; + *stackB++ = rootB; + + do + { + int nodeIndexA = *(--stackA); + int nodeIndexB = *(--stackB); + if (collisionFilterA == null || CollisionFilter.IsCollisionEnabled(collisionFilterA[nodeIndexA], collisionFilterB[nodeIndexB])) + { + ProcessAB(ref treeA[nodeIndexA], ref treeB[nodeIndexB], treeA, treeB, ref stackA, ref stackB, ref pairWriter); + } + + pairWriter.FlushIfNeeded(); + } while (stackA > binaryStackA); + } + } + + private static unsafe void ProcessAA(ref Node node, ref int* stack, ref int* stackA, ref int* stackB, ref T pairWriter) where T : struct, ITreeOverlapCollector + { + int4 nodeData = node.Data; + FourTransposedAabbs nodeBounds = node.Bounds; + + FourTransposedAabbs* aabbT = stackalloc FourTransposedAabbs[3]; + aabbT[0] = nodeBounds.GetAabbT(0); + aabbT[1] = nodeBounds.GetAabbT(1); + aabbT[2] = nodeBounds.GetAabbT(2); + + bool4x3 masks = new bool4x3( + new bool4(false, true, true, true), + new bool4(false, false, true, true), + new bool4(false, false, false, true)); + + int4* compressedValues = stackalloc int4[3]; + int* compressedCounts = stackalloc int[3]; + + for (int i = 0; i < 3; i++) + { + bool4 overlap = aabbT[i].Overlap1Vs4(ref nodeBounds) & masks[i]; + compressedCounts[i] = math.compress((int*)(compressedValues + i), 0, nodeData, overlap); + } + + if (node.IsLeaf) + { + for (int i = 0; i < 3; i++) + { + pairWriter.AddPairs(nodeData[i], compressedValues[i], compressedCounts[i]); + } + } + else + { + int4* internalNodes = stackalloc int4[1]; + int numInternals = math.compress((int*)internalNodes, 0, nodeData, node.AreInternalsValid); + *((int4*)stack) = *internalNodes; + stack += numInternals; + + for (int i = 0; i < 3; i++) + { + *((int4*)stackA) = new int4(nodeData[i]); + *((int4*)stackB) = compressedValues[i]; + stackA += compressedCounts[i]; + stackB += compressedCounts[i]; + } + } + } + + private static unsafe void ProcessAB( + ref Node nodeA, ref Node nodeB, + Node* treeA, Node* treeB, + ref int* stackA, ref int* stackB, + ref T pairWriter) where T : struct, ITreeOverlapCollector + { + int4 nodeAdata = nodeA.Data; + int4 nodeBdata = nodeB.Data; + + FourTransposedAabbs nodeABounds = nodeA.Bounds; + FourTransposedAabbs nodeBBounds = nodeB.Bounds; + + bool4x4 overlapMask = new bool4x4( + nodeBBounds.Overlap1Vs4(ref nodeABounds, 0), + nodeBBounds.Overlap1Vs4(ref nodeABounds, 1), + nodeBBounds.Overlap1Vs4(ref nodeABounds, 2), + nodeBBounds.Overlap1Vs4(ref nodeABounds, 3)); + + int4 compressedData; + + if (nodeA.IsLeaf && nodeB.IsLeaf) + { + for (int i = 0; i < 4; i++) + { + int count = math.compress((int*)&compressedData, 0, nodeBdata, overlapMask[i]); + pairWriter.AddPairs(nodeAdata[i], compressedData, count); + } + } + else if (nodeA.IsInternal && nodeB.IsInternal) + { + for (int i = 0; i < 4; i++) + { + int count = math.compress(stackB, 0, nodeBdata, overlapMask[i]); + *((int4*)stackA) = new int4(nodeAdata[i]); + stackA += count; + stackB += count; + } + } + else + { + int* stack = stackB; + if (nodeA.IsInternal) + { + overlapMask = math.transpose(overlapMask); + Swap(ref nodeAdata, ref nodeBdata); + Swap(ref nodeABounds, ref nodeBBounds); + stack = stackA; + treeB = treeA; + } + + for (int i = 0; i < 4; i++) + { + if (math.any(overlapMask[i])) + { + int internalsCount = math.compress(stack, 0, nodeBdata, overlapMask[i]); + int* internalStack = stack + internalsCount; + + FourTransposedAabbs aabbT = nodeABounds.GetAabbT(i); + int4 leafA = new int4(nodeAdata[i]); + + do + { + Node* internalNode = treeB + *(--internalStack); + int compressedCount = math.compress((int*)&compressedData, 0, internalNode->Data, aabbT.Overlap1Vs4(ref internalNode->Bounds)); + + if (internalNode->IsLeaf) + { + pairWriter.AddPairs(leafA, compressedData, compressedCount); + pairWriter.FlushIfNeeded(); + } + else + { + *((int4*)internalStack) = compressedData; + internalStack += compressedCount; + } + } while (internalStack != stack); + } + } + } + } + + #endregion + + #region AABB overlap query + + public interface IAabbOverlapLeafProcessor + { + // Called when the query overlaps a leaf node of the bounding volume hierarchy + void AabbLeaf(OverlapAabbInput input, int leafData, ref T collector) where T : struct, IOverlapCollector; + } + + public unsafe void AabbOverlap(OverlapAabbInput input, ref TProcessor processor, ref TCollector collector, int root = 1) + where TProcessor : struct, IAabbOverlapLeafProcessor + where TCollector : struct, IOverlapCollector + { + int* binaryStack = stackalloc int[Constants.BinaryStackSize]; + int* stack = binaryStack; + *stack++ = root; + + FourTransposedAabbs aabbT; + (&aabbT)->SetAllAabbs(input.Aabb); + do + { + int nodeIndex = *(--stack); + Node* node = m_Nodes + nodeIndex; + bool4 overlap = aabbT.Overlap1Vs4(ref node->Bounds); + int4 compressedValues; + int compressedCount = math.compress((int*)(&compressedValues), 0, node->Data, overlap); + + if (node->IsLeaf) + { + for (int i = 0; i < compressedCount; i++) + { + processor.AabbLeaf(input, compressedValues[i], ref collector); + } + } + else + { + *((int4*)stack) = compressedValues; + stack += compressedCount; + } + } while (stack > binaryStack); + } + + #endregion + + #region Ray cast query + + public interface IRaycastLeafProcessor + { + // Called when the query hits a leaf node of the bounding volume hierarchy + bool RayLeaf(RaycastInput input, int leafData, ref T collector) where T : struct, ICollector; + } + + public unsafe bool Raycast(RaycastInput input, ref TProcessor leafProcessor, ref TCollector collector) + where TProcessor : struct, IRaycastLeafProcessor + where TCollector : struct, ICollector + { + bool hadHit = false; + int* stack = stackalloc int[Constants.UnaryStackSize], top = stack; + *top++ = 1; + do + { + Node* node = m_Nodes + *(--top); + bool4 hitMask = node->Bounds.Raycast(input.Ray, collector.MaxFraction, out float4 hitFractions); + int4 hitData; + int hitCount = math.compress((int*)(&hitData), 0, node->Data, hitMask); + + if (node->IsLeaf) + { + for (int i = 0; i < hitCount; i++) + { + hadHit |= leafProcessor.RayLeaf(input, hitData[i], ref collector); + if (collector.EarlyOutOnFirstHit && hadHit) + { + return true; + } + } + } + else + { + *((int4*)top) = hitData; + top += hitCount; + } + } while (top > stack); + + return hadHit; + } + + #endregion + + #region Collider cast query + + public interface IColliderCastLeafProcessor + { + // Called when the query hits a leaf node of the bounding volume hierarchy + bool ColliderCastLeaf(ColliderCastInput input, int leafData, ref T collector) where T : struct, ICollector; + } + + public unsafe bool ColliderCast(ColliderCastInput input, ref TProcessor leafProcessor, ref TCollector collector) + where TProcessor : struct, IColliderCastLeafProcessor + where TCollector : struct, ICollector + { + float3 aabbExtents; + Ray aabbRay; + { + Aabb aabb = input.Collider->CalculateAabb(new RigidTransform(input.Orientation, input.Position)); + aabbExtents = aabb.Extents; + aabbRay = input.Ray; + aabbRay.Origin = aabb.Min; + } + + bool hadHit = false; + + int* stack = stackalloc int[Constants.UnaryStackSize], top = stack; + *top++ = 1; + do + { + Node* node = m_Nodes + *(--top); + FourTransposedAabbs bounds = node->Bounds; + bounds.Lx -= aabbExtents.x; + bounds.Ly -= aabbExtents.y; + bounds.Lz -= aabbExtents.z; + bool4 hitMask = bounds.Raycast(aabbRay, collector.MaxFraction, out float4 hitFractions); + int4 hitData; + int hitCount = math.compress((int*)(&hitData), 0, node->Data, hitMask); + + if (node->IsLeaf) + { + for (int i = 0; i < hitCount; i++) + { + hadHit |= leafProcessor.ColliderCastLeaf(input, hitData[i], ref collector); + if (collector.EarlyOutOnFirstHit && hadHit) + { + return true; + } + } + } + else + { + *((int4*)top) = hitData; + top += hitCount; + } + } while (top > stack); + + return hadHit; + } + + #endregion + + #region Point distance query + + public interface IPointDistanceLeafProcessor + { + // Called when the query hits a leaf node of the bounding volume hierarchy + bool DistanceLeaf(PointDistanceInput input, int leafData, ref T collector) where T : struct, ICollector; + } + + public unsafe bool Distance(PointDistanceInput input, ref TProcessor leafProcessor, ref TCollector collector) + where TProcessor : struct, IPointDistanceLeafProcessor + where TCollector : struct, ICollector + { + UnityEngine.Assertions.Assert.IsTrue(collector.MaxFraction <= input.MaxDistance); + + bool hadHit = false; + + int* binaryStack = stackalloc int[Constants.BinaryStackSize]; + int* stack = binaryStack; + *stack++ = 1; + + var pointT = new Math.FourTransposedPoints(input.Position); + float4 maxDistanceSquared = new float4(collector.MaxFraction * collector.MaxFraction); + + do + { + int nodeIndex = *(--stack); + Node* node = m_Nodes + nodeIndex; + float4 distanceToNodesSquared = node->Bounds.DistanceFromPointSquared(ref pointT); + bool4 overlap = (node->Bounds.Lx <= node->Bounds.Hx) & (distanceToNodesSquared <= maxDistanceSquared); + int4 hitData; + int hitCount = math.compress((int*)(&hitData), 0, node->Data, overlap); + + if (node->IsLeaf) + { + for (int i = 0; i < hitCount; i++) + { + hadHit |= leafProcessor.DistanceLeaf(input, hitData[i], ref collector); + + if (collector.EarlyOutOnFirstHit && hadHit) + { + return true; + } + } + + maxDistanceSquared = new float4(collector.MaxFraction * collector.MaxFraction); + } + else + { + *((int4*)stack) = hitData; + stack += hitCount; + } + } while (stack > binaryStack); + + return hadHit; + } + + #endregion + + #region Collider distance query + + public interface IColliderDistanceLeafProcessor + { + // Called when the query hits a leaf node of the bounding volume hierarchy + bool DistanceLeaf(ColliderDistanceInput input, int leafData, ref T collector) where T : struct, ICollector; + } + + public unsafe bool Distance(ColliderDistanceInput input, ref TProcessor leafProcessor, ref TCollector collector) + where TProcessor : struct, IColliderDistanceLeafProcessor + where TCollector : struct, ICollector + { + UnityEngine.Assertions.Assert.IsTrue(collector.MaxFraction <= input.MaxDistance); + + bool hadHit = false; + + int* binaryStack = stackalloc int[Constants.BinaryStackSize]; + int* stack = binaryStack; + *stack++ = 1; + + Aabb aabb = input.Collider->CalculateAabb(input.Transform); + FourTransposedAabbs aabbT; + (&aabbT)->SetAllAabbs(aabb); + float4 maxDistanceSquared = new float4(collector.MaxFraction * collector.MaxFraction); + + do + { + int nodeIndex = *(--stack); + Node* node = m_Nodes + nodeIndex; + float4 distanceToNodesSquared = node->Bounds.DistanceFromAabbSquared(ref aabbT); + bool4 overlap = (node->Bounds.Lx <= node->Bounds.Hx) & (distanceToNodesSquared <= maxDistanceSquared); + int4 hitData; + int hitCount = math.compress((int*)(&hitData), 0, node->Data, overlap); + + if (node->IsLeaf) + { + for (int i = 0; i < hitCount; i++) + { + hadHit |= leafProcessor.DistanceLeaf(input, hitData[i], ref collector); + if (collector.EarlyOutOnFirstHit && hadHit) + { + return true; + } + + maxDistanceSquared = new float4(collector.MaxFraction * collector.MaxFraction); + } + } + else + { + *((int4*)stack) = hitData; + stack += hitCount; + } + } while (stack > binaryStack); + + return hadHit; + } + + #endregion + } +} diff --git a/package/Unity.Physics/Collision/Geometry/BoundingVolumeHierarchy.cs.meta b/package/Unity.Physics/Collision/Geometry/BoundingVolumeHierarchy.cs.meta new file mode 100755 index 000000000..588e0c382 --- /dev/null +++ b/package/Unity.Physics/Collision/Geometry/BoundingVolumeHierarchy.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 1c4f1bd7578251443bf35dbf8dadc021 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Unity.Physics/Collision/Geometry/BoundingVolumeHierarchyBuilder.cs b/package/Unity.Physics/Collision/Geometry/BoundingVolumeHierarchyBuilder.cs new file mode 100755 index 000000000..d3c4b45d6 --- /dev/null +++ b/package/Unity.Physics/Collision/Geometry/BoundingVolumeHierarchyBuilder.cs @@ -0,0 +1,863 @@ +using System; +using System.Collections.Generic; +using Unity.Burst; +using Unity.Collections; +using Unity.Collections.LowLevel.Unsafe; +using Unity.Jobs; +using Unity.Mathematics; +using UnityEngine.Assertions; +using static Unity.Physics.Math; + +namespace Unity.Physics +{ + // Utilities for building bounding volume hierarchies + public partial struct BoundingVolumeHierarchy + { + public struct Constants + { + public const int MaxNumTreeBranches = 64; + public const int SmallRangeSize = 32; + public const int UnaryStackSize = 256; + public const int BinaryStackSize = 512; + } + + public struct PointAndIndex + { + public float3 Position; + public int Index; + } + + /// + /// Builder. + /// + public unsafe struct Builder + { + /// + /// Range. + /// + public struct Range + { + public Range(int start, int length, int root, Aabb domain) + { + Start = start; + Length = length; + Root = root; + Domain = domain; + } + + public int Start; + public int Length; + public int Root; + public Aabb Domain; + } + + void SortRange(int axis, ref Range range) + { + for (int i = range.Start; i < range.Length; ++i) + { + PointAndIndex value = Points[i]; + float key = value.Position[axis]; + int j = i; + while (j > range.Start && key < Points[j - 1].Position[axis]) + { + Points[j] = Points[j - 1]; + j--; + } + Points[j] = value; + } + } + + /// + /// Compute axis and pivot of a given range. + /// + /// + /// + /// + static void ComputeAxisAndPivot(ref Range range, out int axis, out float pivot) + { + // Compute axis and pivot. + axis = IndexOfMaxComponent(range.Domain.Extents); + pivot = ((range.Domain.Min + range.Domain.Max) / 2)[axis]; + } + + static void SplitRange(ref Range range, int size, ref Range lRange, ref Range rRange) + { + lRange.Start = range.Start; + lRange.Length = size; + rRange.Start = lRange.Start + lRange.Length; + rRange.Length = range.Length - lRange.Length; + } + + struct CompareVertices : IComparer + { + public int Compare(float4 x, float4 y) + { + return x[SortAxis].CompareTo(y[SortAxis]); + } + + public int SortAxis; + } + + void ProcessAxis(Range range, int axis, NativeArray scores, NativeArray points, ref int bestAxis, ref int pivot, ref float minScore) + { + CompareVertices comparator; + comparator.SortAxis = axis; + points.Sort(comparator); + + PointAndIndex* p = (PointAndIndex*)PointsAsFloat4 + range.Start; + + Aabb runningAabb = Aabb.Empty; + + for (int i = 0; i < points.Length; i++) + { + runningAabb.Include(Aabbs[p[i].Index]); + scores[i] = (i + 1) * runningAabb.SurfaceArea; + } + + runningAabb = Aabb.Empty; + + for (int i = points.Length - 1, j = 1; i > 0; --i, ++j) + { + runningAabb.Include(Aabbs[p[i].Index]); + float sum = scores[i - 1] + j * runningAabb.SurfaceArea; + if (sum < minScore) + { + pivot = i; + bestAxis = axis; + minScore = sum; + } + } + } + + [BurstDiscard] + void SegregateSah3(Range range, int minItems, ref Range lRange, ref Range rRange) + { + NativeArray scores = new NativeArray(range.Length, Allocator.Temp, NativeArrayOptions.UninitializedMemory); + NativeArray pointsX = new NativeArray(range.Length, Allocator.Temp, NativeArrayOptions.UninitializedMemory); + NativeArray pointsY = new NativeArray(range.Length, Allocator.Temp, NativeArrayOptions.UninitializedMemory); + NativeArray pointsZ = new NativeArray(range.Length, Allocator.Temp, NativeArrayOptions.UninitializedMemory); + float4* p = PointsAsFloat4 + range.Start; + + for (int i = 0; i < range.Length; i++) + { + pointsX[i] = p[i]; + } + + pointsY.CopyFrom(pointsX); + pointsZ.CopyFrom(pointsX); + + int bestAxis = -1, pivot = -1; + float minScore = float.MaxValue; + + ProcessAxis(range, 0, scores, pointsX, ref bestAxis, ref pivot, ref minScore); + ProcessAxis(range, 1, scores, pointsY, ref bestAxis, ref pivot, ref minScore); + ProcessAxis(range, 2, scores, pointsZ, ref bestAxis, ref pivot, ref minScore); + + // Build sub-ranges. + int lSize = pivot; + int rSize = range.Length - lSize; + if (lSize < minItems || rSize < minItems) + { + // Make sure sub-ranges contains at least minItems nodes, in these rare cases (i.e. all points at the same position), we just split the set in half regardless of positions. + SplitRange(ref range, range.Length / 2, ref lRange, ref rRange); + } + else + { + SplitRange(ref range, lSize, ref lRange, ref rRange); + } + + float4* sortedPoints; + + if (bestAxis == 0) + { + sortedPoints = (float4*)pointsX.GetUnsafePtr(); + } + else if (bestAxis == 1) + { + sortedPoints = (float4*)pointsY.GetUnsafePtr(); + } + else // bestAxis == 2 + { + sortedPoints = (float4*)pointsZ.GetUnsafePtr(); + } + + // Write back sorted points. + for (int i = 0; i < range.Length; i++) + { + p[i] = sortedPoints[i]; + } + + scores.Dispose(); + pointsX.Dispose(); + pointsY.Dispose(); + pointsZ.Dispose(); + } + + + void Segregate(int axis, float pivot, Range range, int minItems, ref Range lRange, ref Range rRange) + { + Assert.IsTrue(range.Length > 1/*, "Range length must be greater than 1."*/); + + Aabb lDomain = Aabb.Empty; + Aabb rDomain = Aabb.Empty; + + float4* p = PointsAsFloat4; + float4* start = p + range.Start; + float4* end = p + range.Length - 1; + + do + { + // Consume left. + + while (start <= end && (*start)[axis] < pivot) + { + lDomain.Include((*(start++)).xyz); + } + + // Consume right. + while (end > start && (*end)[axis] >= pivot) + { + rDomain.Include((*(end--)).xyz); + } + + if (start >= end) goto FINISHED; + + lDomain.Include((*end).xyz); + rDomain.Include((*start).xyz); + + Swap(ref *(start++), ref *(end--)); + } while (true); + FINISHED: + // Build sub-ranges. + int lSize = (int)(start - p); + int rSize = range.Length - lSize; + if (lSize < minItems || rSize < minItems) + { + // Make sure sub-ranges contains at least minItems nodes, in these rare cases (i.e. all points at the same position), we just split the set in half regardless of positions. + SplitRange(ref range, range.Length / 2, ref lRange, ref rRange); + + SetAabbFromPoints(ref lDomain, PointsAsFloat4 + lRange.Start, lRange.Length); + SetAabbFromPoints(ref rDomain, PointsAsFloat4 + rRange.Start, rRange.Length); + } + else + { + SplitRange(ref range, lSize, ref lRange, ref rRange); + } + + lRange.Domain = lDomain; + rRange.Domain = rDomain; + } + + void CreateChildren(Range* subRanges, int numSubRanges, int parentNodeIndex, ref int freeNodeIndex, Range* rangeStack, ref int stackSize) + { + int4 parentData = int4.zero; + + for (int i = 0; i < numSubRanges; i++) + { + // Add child node. + int childNodeIndex = freeNodeIndex++; + parentData[i] = childNodeIndex; + + if (subRanges[i].Length > 4) + { + // Keep splitting the range, push it on the stack. + rangeStack[stackSize] = subRanges[i]; + rangeStack[stackSize++].Root = childNodeIndex; + } + else + { + Node* childNode = GetNode(childNodeIndex); + childNode->IsLeaf = true; + + for (int pointIndex = 0; pointIndex < subRanges[i].Length; pointIndex++) + { + childNode->Data[pointIndex] = Points[subRanges[i].Start + pointIndex].Index; + } + + for (int j = subRanges[i].Length; j < 4; j++) + { + childNode->ClearLeafData(j); + } + } + } + + Node* parentNode = GetNode(parentNodeIndex); + parentNode->Data = parentData; + parentNode->IsInternal = true; + } + + Node* GetNode(int nodeIndex) => Bvh.m_Nodes + nodeIndex; + + float4* PointsAsFloat4 => (float4*)Points.GetUnsafePtr(); + + void ProcessSmallRange(Range baseRange, ref int freeNodeIndex) + { + Range range = baseRange; + + ComputeAxisAndPivot(ref range, out int axis, out float pivot); + SortRange(axis, ref range); + + Range* subRanges = stackalloc Range[4]; + int hasLeftOvers = 1; + do + { + int numSubRanges = 0; + while (range.Length > 4 && numSubRanges < 3) + { + subRanges[numSubRanges].Start = range.Start; + subRanges[numSubRanges].Length = 4; + numSubRanges++; + + range.Start += 4; + range.Length -= 4; + } + + if (range.Length > 0) + { + subRanges[numSubRanges].Start = range.Start; + subRanges[numSubRanges].Length = range.Length; + + numSubRanges++; + } + + hasLeftOvers = 0; + CreateChildren(subRanges, numSubRanges, range.Root, ref freeNodeIndex, (Range*)UnsafeUtility.AddressOf(ref range), ref hasLeftOvers); + + Assert.IsTrue(hasLeftOvers <= 1/*, "Internal error"*/); + } while (hasLeftOvers > 0); + } + + public void ProcessLargeRange(Range range, Range* subRanges) + { + if (!UseSah) + { + ComputeAxisAndPivot(ref range, out int axis, out float pivot); + + Range* temps = stackalloc Range[2]; + Segregate(axis, pivot, range, 2, ref temps[0], ref temps[1]); + + ComputeAxisAndPivot(ref temps[0], out int lAxis, out float lPivot); + Segregate(lAxis, lPivot, temps[0], 1, ref subRanges[0], ref subRanges[1]); + + ComputeAxisAndPivot(ref temps[1], out int rAxis, out float rPivot); + Segregate(rAxis, rPivot, temps[1], 1, ref subRanges[2], ref subRanges[3]); + } + else + { + Range* temps = stackalloc Range[2]; + SegregateSah3(range, 2, ref temps[0], ref temps[1]); + + SegregateSah3(temps[0], 1, ref subRanges[0], ref subRanges[1]); + SegregateSah3(temps[1], 1, ref subRanges[2], ref subRanges[3]); + } + } + + public void CreateInternalNodes(Range* subRanges, int numSubRanges, int root, Range* rangeStack, ref int stackSize, ref int freeNodeIndex) + { + int4 rootData = int4.zero; + + for (int i = 0; i < numSubRanges; ++i) + { + rootData[i] = freeNodeIndex++; + rangeStack[stackSize] = subRanges[i]; + rangeStack[stackSize++].Root = rootData[i]; + } + + Node* rootNode = GetNode(root); + rootNode->Data = rootData; + rootNode->IsInternal = true; + } + + public void Build(Range baseRange) + { + Range* ranges = stackalloc Range[Constants.UnaryStackSize]; + int rangeStackSize = 1; + ranges[0] = baseRange; + + if (baseRange.Length > 4) + { + do + { + Range range = ranges[--rangeStackSize]; + + if (range.Length <= Constants.SmallRangeSize) + { + ProcessSmallRange(range, ref FreeNodeIndex); + } + else + { + Range* subRanges = stackalloc Range[4]; + ProcessLargeRange(range, subRanges); + CreateChildren(subRanges, 4, range.Root, ref FreeNodeIndex, ranges, ref rangeStackSize); + } + } + while (rangeStackSize > 0); + } + else + { + CreateChildren(ranges, 1, baseRange.Root, ref FreeNodeIndex, ranges, ref rangeStackSize); + } + } + + public BoundingVolumeHierarchy Bvh; + public NativeArray Points; + public NativeArray Aabbs; + public int FreeNodeIndex; + public bool UseSah; + } + + public unsafe JobHandle ScheduleBuildJobs( + NativeArray points, NativeArray aabbs, NativeArray bodyFilters, + int numThreadsHint, JobHandle inputDeps, int numNodes, NativeArray ranges, NativeArray numBranches) + { + JobHandle handle = inputDeps; + + var branchNodeOffsets = new NativeArray(Constants.MaxNumTreeBranches, Allocator.TempJob, NativeArrayOptions.UninitializedMemory); + + // Build initial branches + handle = new BuildFirstNLevelsJob + { + Points = points, + Nodes = m_Nodes, + NodeFilters = m_NodeFilters, + Ranges = ranges, + BranchNodeOffsets = branchNodeOffsets, + BranchCount = numBranches, + ThreadCount = numThreadsHint + }.Schedule(handle); + + // Build branches + handle = new BuildBranchesJob + { + Points = points, + Aabbs = aabbs, + BodyFilters = bodyFilters, + Nodes = m_Nodes, + NodeFilters = m_NodeFilters, + Ranges = ranges, + BranchNodeOffsets = branchNodeOffsets, + BranchCount = numBranches + }.Schedule(Constants.MaxNumTreeBranches, 1, handle); + + // Note: This job also deallocates the aabbs and lookup arrays on completion + handle = new FinalizeTreeJob + { + Aabbs = aabbs, + Nodes = m_Nodes, + NodeFilters = m_NodeFilters, + LeafFilters = bodyFilters, + NumNodes = numNodes, + BranchNodeOffsets = branchNodeOffsets, + BranchCount = numBranches + }.Schedule(handle); + + return handle; + } + + public unsafe void Build(NativeArray points, NativeArray aabbs, out int nodeCount, bool useSah = false) + { + m_Nodes[0] = Node.Empty; + + var builder = new Builder + { + Bvh = this, + Points = points, + Aabbs = aabbs, + FreeNodeIndex = 2, + UseSah = useSah + }; + + Aabb aabb = new Aabb(); + SetAabbFromPoints(ref aabb, (float4*)points.GetUnsafePtr(), points.Length); + builder.Build(new Builder.Range(0, points.Length, 1, aabb)); + nodeCount = builder.FreeNodeIndex; + + Refit(aabbs, 1, builder.FreeNodeIndex - 1); + } + + // For each node between nodeStartIndex and nodeEnd index, set the collision filter info to the combination of the node's childen + public unsafe void BuildCombinedCollisionFilter(NativeArray leafFilterInfo, int nodeStartIndex, int nodeEndIndex) + { + Node* baseNode = m_Nodes; + Node* currentNode = baseNode + nodeEndIndex; + + for (int i = nodeEndIndex; i >= nodeStartIndex; i--, currentNode--) + { + CollisionFilter combinationFilter = new CollisionFilter(); + + if (currentNode->IsLeaf) + { + // We know that at least one child will be valid, so start with that leaf's filter: + combinationFilter = leafFilterInfo[currentNode->Data[0]]; + for (int j = 1; j < 4; ++j) + { + if (currentNode->IsLeafValid(j)) + { + CollisionFilter leafFilter = leafFilterInfo[currentNode->Data[j]]; + combinationFilter = CollisionFilter.CreateUnion(combinationFilter, leafFilter); + } + } + } + else + { + combinationFilter = m_NodeFilters[currentNode->Data[0]]; + for (int j = 1; j < 4; j++) + { + if (currentNode->IsInternalValid(j)) + { + CollisionFilter nodeFilter = m_NodeFilters[currentNode->Data[j]]; + combinationFilter = CollisionFilter.CreateUnion(combinationFilter, nodeFilter); + } + } + } + + m_NodeFilters[i] = combinationFilter; + } + } + + // Set the collision filter on nodeIndex to the combination of all it's child filters. Node must not be a leaf. + unsafe void BuildCombinedCollisionFilter(int nodeIndex) + { + Node* baseNode = m_Nodes; + Node* currentNode = baseNode + nodeIndex; + + Assert.IsTrue(currentNode->IsInternal); + + CollisionFilter combinedFilter = new CollisionFilter(); + for (int j = 0; j < 4; j++) + { + combinedFilter = CollisionFilter.CreateUnion(combinedFilter, m_NodeFilters[currentNode->Data[j]]); + } + + m_NodeFilters[nodeIndex] = combinedFilter; + } + + + public unsafe void Refit(NativeArray aabbs, int nodeStartIndex, int nodeEndIndex) + { + Node* baseNode = m_Nodes; + Node* currentNode = baseNode + nodeEndIndex; + + for (int i = nodeEndIndex; i >= nodeStartIndex; i--, currentNode--) + { + if (currentNode->IsLeaf) + { + for (int j = 0; j < 4; ++j) + { + Aabb aabb; + if (currentNode->IsLeafValid(j)) + { + aabb = aabbs[currentNode->Data[j]]; + } + else + { + aabb = Aabb.Empty; + } + + currentNode->Bounds.SetAabb(j, aabb); + } + } + else + { + for (int j = 0; j < 4; j++) + { + Aabb aabb; + if (currentNode->IsInternalValid(j)) + { + aabb = baseNode[currentNode->Data[j]].Bounds.GetCompoundAabb(); + } + else + { + aabb = Aabb.Empty; + } + + currentNode->Bounds.SetAabb(j, aabb); + } + } + } + } + + unsafe void RefitNode(int nodeIndex) + { + Node* baseNode = m_Nodes; + Node* currentNode = baseNode + nodeIndex; + + Assert.IsTrue(currentNode->IsInternal); + + for (int j = 0; j < 4; j++) + { + Aabb compoundAabb = baseNode[currentNode->Data[j]].Bounds.GetCompoundAabb(); + currentNode->Bounds.SetAabb(j, compoundAabb); + } + } + + private struct RangeSizeAndIndex + { + public int RangeIndex; + public int RangeSize; + public int RangeFirstNodeOffset; + } + + unsafe void SortRangeMap(RangeSizeAndIndex* rangeMap, int numElements) + { + for (int i = 0; i < numElements; i++) + { + RangeSizeAndIndex value = rangeMap[i]; + int key = rangeMap[i].RangeSize; + int j = i; + while (j > 0 && key > rangeMap[j - 1].RangeSize) + { + rangeMap[j] = rangeMap[j - 1]; + j--; + } + + rangeMap[j] = value; + } + } + + public unsafe void BuildFirstNLevels( + NativeArray points, + NativeArray branchRanges, NativeArray branchNodeOffset, + int threadCount, out int branchCount) + { + Builder.Range* level0 = stackalloc Builder.Range[Constants.MaxNumTreeBranches]; + Builder.Range* level1 = stackalloc Builder.Range[Constants.MaxNumTreeBranches]; + int level0Size = 1; + int level1Size = 0; + + Aabb aabb = new Aabb(); + SetAabbFromPoints(ref aabb, (float4*)points.GetUnsafePtr(), points.Length); + level0[0] = new Builder.Range(0, points.Length, 1, aabb); + + int largestAllowedRange = math.max(level0[0].Length / threadCount, Constants.SmallRangeSize); + int smallRangeThreshold = math.max(largestAllowedRange / threadCount, Constants.SmallRangeSize); + int largestRangeInLastLevel; + int maxNumBranchesMinusOneSplit = Constants.MaxNumTreeBranches - 3; + int freeNodeIndex = 2; + + var builder = new Builder { Bvh = this, Points = points, UseSah = false }; + + do + { + largestRangeInLastLevel = 0; + + for (int i = 0; i < level0Size; ++i) + { + if (level0[i].Length > smallRangeThreshold && freeNodeIndex < maxNumBranchesMinusOneSplit) + { + // Split range in up to 4 sub-ranges. + Builder.Range* subRanges = stackalloc Builder.Range[4]; + + builder.ProcessLargeRange(level0[i], subRanges); + + largestRangeInLastLevel = math.max(largestRangeInLastLevel, subRanges[0].Length); + largestRangeInLastLevel = math.max(largestRangeInLastLevel, subRanges[1].Length); + largestRangeInLastLevel = math.max(largestRangeInLastLevel, subRanges[2].Length); + largestRangeInLastLevel = math.max(largestRangeInLastLevel, subRanges[3].Length); + + // Create nodes for the sub-ranges and append level 1 sub-ranges. + builder.CreateInternalNodes(subRanges, 4, level0[i].Root, level1, ref level1Size, ref freeNodeIndex); + } + else + { + // Too small, ignore. + level1[level1Size++] = level0[i]; + } + } + + Builder.Range* tmp = level0; + level0 = level1; + level1 = tmp; + + level0Size = level1Size; + level1Size = 0; + smallRangeThreshold = largestAllowedRange; + } while (level0Size < Constants.MaxNumTreeBranches && largestRangeInLastLevel > largestAllowedRange); + + RangeSizeAndIndex* rangeMapBySize = stackalloc RangeSizeAndIndex[Constants.MaxNumTreeBranches]; + + int nodeOffset = freeNodeIndex; + for (int i = 0; i < level0Size; i++) + { + rangeMapBySize[i] = new RangeSizeAndIndex { RangeIndex = i, RangeSize = level0[i].Length, RangeFirstNodeOffset = nodeOffset }; + nodeOffset += level0[i].Length; + } + + SortRangeMap(rangeMapBySize, level0Size); + + for (int i = 0; i < level0Size; i++) + { + branchRanges[i] = level0[rangeMapBySize[i].RangeIndex]; + branchNodeOffset[i] = rangeMapBySize[i].RangeFirstNodeOffset; + } + + for (int i = level0Size; i < Constants.MaxNumTreeBranches; i++) + { + branchNodeOffset[i] = -1; + } + + branchCount = level0Size; + + m_Nodes[0] = Node.Empty; + } + + // Build the branch for range. Returns the index of the last built node in the range + public int BuildBranch(NativeArray points, NativeArray aabb, Builder.Range range, int firstNodeIndex) + { + var builder = new Builder + { + Bvh = this, + Points = points, + FreeNodeIndex = firstNodeIndex, + UseSah = false + }; + + builder.Build(range); + + Refit(aabb, firstNodeIndex, builder.FreeNodeIndex - 1); + RefitNode(range.Root); + return builder.FreeNodeIndex - 1; + } + + // helper + private static unsafe void SetAabbFromPoints(ref Aabb aabb, float4* points, int length) + { + aabb.Min = Math.Constants.Max3F; + aabb.Max = Math.Constants.Min3F; + for (int i = 0; i < length; i++) + { + aabb.Min = math.min(aabb.Min, points[i].xyz); + aabb.Max = math.max(aabb.Max, points[i].xyz); + } + } + + [BurstCompile] + public unsafe struct BuildFirstNLevelsJob : IJob + { + public NativeArray Points; + [NativeDisableUnsafePtrRestriction] + public Node* Nodes; + [NativeDisableUnsafePtrRestriction] + public CollisionFilter* NodeFilters; + public NativeArray Ranges; + public NativeArray BranchNodeOffsets; + public NativeArray BranchCount; + public int ThreadCount; + + public void Execute() + { + var bvh = new BoundingVolumeHierarchy(Nodes, NodeFilters); + bvh.BuildFirstNLevels(Points, Ranges, BranchNodeOffsets, ThreadCount, out int branchCount); + BranchCount[0] = branchCount; + } + } + + [BurstCompile] + public unsafe struct BuildBranchesJob : IJobParallelFor + { + [ReadOnly] public NativeArray Aabbs; + [ReadOnly] public NativeArray BodyFilters; + [ReadOnly] public NativeArray Ranges; + [ReadOnly] public NativeArray BranchNodeOffsets; + [ReadOnly] public NativeArray BranchCount; + + [NativeDisableUnsafePtrRestriction] + public Node* Nodes; + [NativeDisableUnsafePtrRestriction] + public CollisionFilter* NodeFilters; + + [NativeDisableContainerSafetyRestriction] + [DeallocateOnJobCompletion] public NativeArray Points; + + public void Execute(int index) + { + // This is need since we schedule the job before we know the exact number of + // branches we need to build. + if (index >= BranchCount[0]) + { + return; + } + + Assert.IsTrue(BranchNodeOffsets[index] >= 0); + var bvh = new BoundingVolumeHierarchy(Nodes, NodeFilters); + int lastNode = bvh.BuildBranch(Points, Aabbs, Ranges[index], BranchNodeOffsets[index]); + + if (NodeFilters != null) + { + bvh.BuildCombinedCollisionFilter(BodyFilters, BranchNodeOffsets[index], lastNode); + bvh.BuildCombinedCollisionFilter(Ranges[index].Root); + } + } + } + + [BurstCompile] + public unsafe struct FinalizeTreeJob : IJob + { + [ReadOnly] [DeallocateOnJobCompletion] public NativeArray Aabbs; + [ReadOnly] [DeallocateOnJobCompletion] public NativeArray BranchNodeOffsets; + [ReadOnly] public NativeArray LeafFilters; + [ReadOnly] public NativeArray BranchCount; + [NativeDisableUnsafePtrRestriction] + public Node* Nodes; + [NativeDisableUnsafePtrRestriction] + public CollisionFilter* NodeFilters; + public int NumNodes; + + public void Execute() + { + int minBranchNodeIndex = BranchNodeOffsets[0] - 1; + int branchCount = BranchCount[0]; + for (int i = 1; i < BranchCount[0]; i++) + { + minBranchNodeIndex = math.min(BranchNodeOffsets[i] - 1, minBranchNodeIndex); + } + + var bvh = new BoundingVolumeHierarchy(Nodes, NodeFilters); + bvh.Refit(Aabbs, 1, minBranchNodeIndex); + + if (NodeFilters != null) + { + bvh.BuildCombinedCollisionFilter(LeafFilters, 1, minBranchNodeIndex); + } + } + } + + public unsafe void CheckIntegrity(int nodeIndex = 1, int parentIndex = 0, int childIndex = 0) + { + Node parent = m_Nodes[parentIndex]; + Node node = m_Nodes[nodeIndex]; + Aabb parentAabb = parent.Bounds.GetAabb(childIndex); + + for (int i = 0; i < 4; ++i) + { + int data = node.Data[i]; + Aabb aabb = node.Bounds.GetAabb(i); + + bool validData = node.IsChildValid(i); + + bool validAabb = aabb.IsValid; + + if (validData != validAabb) + { + throw new Exception("Invalid node should have empty AABB."); + } + + if (validData) + { + if (parentIndex != 0) + { + if (!parentAabb.Contains(aabb)) + { + throw new Exception("Parent AABB do not contains child AABB"); + } + } + + if (node.IsInternal) + { + CheckIntegrity(data, nodeIndex, i); + } + } + } + } + } +} diff --git a/package/Unity.Physics/Collision/Geometry/BoundingVolumeHierarchyBuilder.cs.meta b/package/Unity.Physics/Collision/Geometry/BoundingVolumeHierarchyBuilder.cs.meta new file mode 100755 index 000000000..cb22782a1 --- /dev/null +++ b/package/Unity.Physics/Collision/Geometry/BoundingVolumeHierarchyBuilder.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: dabe461abef50cb47a33cb29a4e01f8b +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Unity.Physics/Collision/Geometry/ConvexHull.cs b/package/Unity.Physics/Collision/Geometry/ConvexHull.cs new file mode 100755 index 000000000..8534a020f --- /dev/null +++ b/package/Unity.Physics/Collision/Geometry/ConvexHull.cs @@ -0,0 +1,127 @@ +using System; +using Unity.Collections.LowLevel.Unsafe; +using Unity.Mathematics; + +namespace Unity.Physics +{ + // A convex hull. + // Warning: This is just the header, the hull's variable sized data follows it in memory. + // Therefore this struct must always be passed by reference, never by value. + public struct ConvexHull + { + public struct Face : IEquatable + { + public short FirstIndex; // index into FaceVertexIndices array + public byte NumVertices; // number of vertex indices in the FaceVertexIndices array + public byte MinHalfAngle; // 0-255 = 0-90 degrees + + public bool Equals(Face other) => FirstIndex.Equals(other.FirstIndex) && NumVertices.Equals(other.NumVertices) && MinHalfAngle.Equals(other.MinHalfAngle); + } + + public struct Edge : IEquatable + { + public short FaceIndex; // index into Faces array + public byte EdgeIndex; // edge index within the face + private readonly byte m_Padding; // TODO: can we use/remove this? + + public bool Equals(Edge other) => FaceIndex.Equals(other.FaceIndex) && EdgeIndex.Equals(other.EdgeIndex); + } + + // A distance by which to inflate the surface of the hull for collision detection. + // This helps to keep the actual hulls from overlapping during simulation, which avoids more costly algorithms. + // For spheres and capsules, this is the radius of the primitive. + // For other convex hulls, this is typically a small value. + // For polygons in a static mesh, this is typically zero. + public float ConvexRadius; + + // Relative arrays of convex hull data + internal BlobArray VerticesBlob; + internal BlobArray FacePlanesBlob; + internal BlobArray FacesBlob; + internal BlobArray FaceVertexIndicesBlob; + internal BlobArray FaceLinksBlob; + internal BlobArray VertexEdgesBlob; + + public int NumVertices => VerticesBlob.Length; + public int NumFaces => FacesBlob.Length; + + // Indexers for the data + public BlobArray.Accessor Vertices => new BlobArray.Accessor(ref VerticesBlob); + public BlobArray.Accessor VertexEdges => new BlobArray.Accessor(ref VertexEdgesBlob); + public BlobArray.Accessor Faces => new BlobArray.Accessor(ref FacesBlob); + public BlobArray.Accessor Planes => new BlobArray.Accessor(ref FacePlanesBlob); + public BlobArray.Accessor FaceVertexIndices => new BlobArray.Accessor(ref FaceVertexIndicesBlob); + public BlobArray.Accessor FaceLinks => new BlobArray.Accessor(ref FaceLinksBlob); + + public unsafe float3* VerticesPtr => (float3*)((byte*)UnsafeUtility.AddressOf(ref VerticesBlob.Offset) + VerticesBlob.Offset); + public unsafe byte* FaceVertexIndicesPtr => (byte*)UnsafeUtility.AddressOf(ref FaceVertexIndicesBlob.Offset) + FaceVertexIndicesBlob.Offset; + + // Returns the index of the face with maximum normal dot direction + public int GetSupportingFace(float3 direction) + { + int bestIndex = 0; + Plane plane0 = Planes[0]; + float bestDot = math.dot(direction, plane0.Normal); + for (int i = 1; i < NumFaces; i++) + { + float dot = math.dot(direction, Planes[i].Normal); + if (dot > bestDot) + { + bestDot = dot; + bestIndex = i; + } + } + return bestIndex; + } + + // Returns the index of the best supporting face that contains supportingVertex + public int GetSupportingFace(float3 direction, int supportingVertexIndex) + { + // Special case for polygons, they don't have connectivity information and don't need to search edges + // because both faces contain all vertices + if (Faces.Length == 2) + { + return GetSupportingFace(direction); + } + + // Search the edges that contain supportingVertexIndex for the one that is most perpendicular to direction + int bestEdgeIndex = -1; + { + float bestEdgeDot = float.MaxValue; + float3 supportingVertex = Vertices[supportingVertexIndex]; + Edge edge = VertexEdges[supportingVertexIndex]; + int firstFaceIndex = edge.FaceIndex; + Face face = Faces[firstFaceIndex]; + while (true) + { + // Get the linked edge and test it against the support direction + int linkedEdgeIndex = face.FirstIndex + edge.EdgeIndex; + edge = FaceLinks[linkedEdgeIndex]; + face = Faces[edge.FaceIndex]; + float3 linkedVertex = Vertices[FaceVertexIndices[face.FirstIndex + edge.EdgeIndex]]; + float3 edgeDirection = linkedVertex - supportingVertex; + float dot = math.abs(math.dot(direction, edgeDirection)) * math.rsqrt(math.lengthsq(edgeDirection)); + bestEdgeIndex = math.select(bestEdgeIndex, linkedEdgeIndex, dot < bestEdgeDot); + bestEdgeDot = math.min(bestEdgeDot, dot); + + // Quit after looping back to the first face + if (edge.FaceIndex == firstFaceIndex) + { + break; + } + + // Get the next edge + edge.EdgeIndex = (byte)((edge.EdgeIndex + 1) % face.NumVertices); + } + } + + // Choose the face containing the best edge that is most parallel to the support direction + Edge bestEdge = FaceLinks[bestEdgeIndex]; + int faceIndex0 = bestEdge.FaceIndex; + int faceIndex1 = FaceLinks[Faces[faceIndex0].FirstIndex + bestEdge.EdgeIndex].FaceIndex; + float3 normal0 = Planes[faceIndex0].Normal; + float3 normal1 = Planes[faceIndex1].Normal; + return math.select(faceIndex0, faceIndex1, math.dot(direction, normal1) > math.dot(direction, normal0)); + } + } +} diff --git a/package/Unity.Physics/Collision/Geometry/ConvexHull.cs.meta b/package/Unity.Physics/Collision/Geometry/ConvexHull.cs.meta new file mode 100755 index 000000000..6cdb16cc3 --- /dev/null +++ b/package/Unity.Physics/Collision/Geometry/ConvexHull.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: d99169bd34b3b3044bc330f8bd6666b6 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Unity.Physics/Collision/Geometry/ConvexHullBuilder.cs b/package/Unity.Physics/Collision/Geometry/ConvexHullBuilder.cs new file mode 100755 index 000000000..29571cc41 --- /dev/null +++ b/package/Unity.Physics/Collision/Geometry/ConvexHullBuilder.cs @@ -0,0 +1,1610 @@ +using System; +using System.Collections.Generic; +using System.Diagnostics; +using System.Runtime.CompilerServices; +using Unity.Burst; +using Unity.Collections; +using Unity.Mathematics; +using UnityEngine.Assertions; +using static Unity.Physics.Math; + +namespace Unity.Physics +{ + /// + /// Convex hull builder. + /// + public struct ConvexHullBuilder : IDisposable + { + public ElementPool Vertices; + public ElementPool Triangles; + + public int Dimension { get; private set; } + public int NumFaces { get; private set; } + public int NumFaceVertices { get; private set; } + + private IntegerSpace m_IntegerSpace; + private Aabb m_IntegerSpaceAabb; + private uint m_NextUid; + + public Aabb IntegerSpaceAabb + { + get => m_IntegerSpaceAabb; + set { m_IntegerSpaceAabb = value; UpdateIntSpace(); } + } + + /// + /// Convex hull vertex. + /// + [DebuggerDisplay("{Cardinality}:{Position}")] + public struct Vertex : IPoolElement + { + public float3 Position; + public int3 IntPosition; + public int Cardinality; + public uint UserData; + public readonly uint Uid; + public int NextFree { get; private set; } + + public bool IsAllocated => Cardinality != -1; + + public Vertex(float3 position, uint userData, uint uid) + { + Position = position; + UserData = userData; + Uid = uid; + Cardinality = 0; + IntPosition = new int3(0); + NextFree = -1; + } + + void IPoolElement.MarkFree(int nextFree) + { + Cardinality = -1; + NextFree = nextFree; + } + } + + /// + /// Convex hull triangle. + /// + [DebuggerDisplay("#{FaceIndex}[{Vertex0}, {Vertex1}, {Vertex2}]")] + public struct Triangle : IPoolElement + { + public int Vertex0, Vertex1, Vertex2; + public Edge Link0, Link1, Link2; + public int FaceIndex; + public readonly uint Uid; + public int NextFree { get; private set; } + + public bool IsAllocated => FaceIndex != -2; + + public Triangle(int vertex0, int vertex1, int vertex2, uint uid) + { + FaceIndex = 0; + Vertex0 = vertex0; + Vertex1 = vertex1; + Vertex2 = vertex2; + Link0 = Edge.Invalid; + Link1 = Edge.Invalid; + Link2 = Edge.Invalid; + Uid = uid; + NextFree = -1; + } + + public unsafe int GetVertex(int index) { fixed (int* p = &Vertex0) { return p[index]; } } + public unsafe void SetVertex(int index, int value) { fixed (int* p = &Vertex0) { p[index] = value; } } + + public unsafe Edge GetLink(int index) { fixed (Edge* p = &Link0) { return p[index]; } } + public unsafe void SetLink(int index, Edge handle) { fixed (Edge* p = &Link0) { p[index] = handle; } } + + void IPoolElement.MarkFree(int nextFree) + { + FaceIndex = -2; + NextFree = nextFree; + } + } + + /// + /// An edge of a triangle, used internally to traverse hull topology. + /// + [DebuggerDisplay("[{value>>2}:{value&3}]")] + public struct Edge : IEquatable + { + public readonly int Value; + + public bool IsValid => Value != Invalid.Value; + public int TriangleIndex => Value >> 2; + public int EdgeIndex => Value & 3; + + public static readonly Edge Invalid = new Edge(0x7fffffff); + + public Edge(int value) { Value = value; } + public Edge(int triangleIndex, int edgeIndex) { Value = triangleIndex << 2 | edgeIndex; } + + public Edge Next => IsValid ? new Edge(TriangleIndex, (EdgeIndex + 1) % 3) : Invalid; + public Edge Prev => IsValid ? new Edge(TriangleIndex, (EdgeIndex + 2) % 3) : Invalid; + + public bool Equals(Edge other) => Value == other.Value; + } + + /// + /// An edge of a face (possibly made from multiple triangles). + /// + public struct FaceEdge + { + public Edge Start; // the first edge of the face + public Edge Current; // the current edge of the face + + public bool IsValid => Current.IsValid; + + public static readonly FaceEdge Invalid = new FaceEdge { Start = Edge.Invalid, Current = Edge.Invalid }; + + public static implicit operator Edge(FaceEdge fe) => fe.Current; + } + + /// + /// Convex hull mass properties. + /// + public struct MassProperties + { + public float3 CenterOfMass; + public float3x3 InertiaTensor; + public float SurfaceArea; + public float Volume; + } + + /// + /// Edge collapse information, used internally to simplify convex hull. + /// + public struct EdgeCollapse + { + public int StartVertex; + public int EndVertex; + public float4 Data; + } + + /// + /// A quantized integer space. + /// + private struct IntegerSpace + { + public readonly float3 Offset; + public readonly float3 Scale; + public readonly float3 InvScale; + + public IntegerSpace(Aabb aabb, int resolution, bool uniform, float minExtent) + { + if (uniform) + { + float3 c = aabb.Center; + var m = new float3(math.cmax(aabb.Max - c)); + aabb.Min = c - m; + aabb.Max = c + m; + } + float3 extents = math.max(minExtent, (aabb.Max - aabb.Min)); + Offset = aabb.Min; + Scale = extents / resolution; + InvScale = math.select(resolution / extents, new float3(0), Scale <= 0); + } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public int3 ToIntegerSpace(float3 x) => new int3((x - Offset) * InvScale + 0.5f); + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public float3 ToFloatSpace(int3 x) => x * Scale + Offset; + } + + /// + /// Create using internal storage. + /// + public ConvexHullBuilder(int verticesCapacity, int trianglesCapacity, Allocator allocator = Allocator.Persistent) + { + Vertices = new ElementPool(verticesCapacity, allocator); + Triangles = new ElementPool(trianglesCapacity, allocator); + Dimension = -1; + NumFaces = 0; + NumFaceVertices = 0; + m_NextUid = 1; + m_IntegerSpaceAabb = Aabb.Empty; + m_IntegerSpace = new IntegerSpace(); + } + + /// + /// Create using external storage. + /// + public unsafe ConvexHullBuilder(Vertex* vertices, int verticesCapacity, Triangle* triangles, int triangleCapacity) + { + Vertices = new ElementPool(vertices, verticesCapacity); + Triangles = new ElementPool(triangles, triangleCapacity); + Dimension = -1; + NumFaces = 0; + NumFaceVertices = 0; + m_NextUid = 1; + m_IntegerSpaceAabb = Aabb.Empty; + m_IntegerSpace = new IntegerSpace(); + } + + /// + /// Copy the content of another convex hull into this one. + /// + public void CopyFrom(ConvexHullBuilder other) + { + Vertices.CopyFrom(other.Vertices); + Triangles.CopyFrom(other.Triangles); + Dimension = other.Dimension; + NumFaces = other.NumFaces; + NumFaceVertices = other.NumFaceVertices; + m_IntegerSpaceAabb = other.m_IntegerSpaceAabb; + m_IntegerSpace = other.m_IntegerSpace; + m_NextUid = other.m_NextUid; + } + + /// + /// Dispose internal convex hull storages if any. + /// + [BurstDiscard] + public void Dispose() + { + Vertices.Dispose(); + Triangles.Dispose(); + } + + #region Construction + + /// + /// Reset the convex hull. + /// + public void Reset() + { + Vertices.Clear(); + Triangles.Clear(); + Dimension = -1; + NumFaces = 0; + NumFaceVertices = 0; + m_IntegerSpaceAabb = Aabb.Empty; + m_IntegerSpace = new IntegerSpace(); + } + + /// + /// Add a point the the convex hull. + /// + /// Point to insert. + /// User data attached to the new vertex if insertion succeeds. + /// true if the insertion succeeded, false otherwise. + public unsafe bool AddPoint(float3 point, uint userData = 0, bool project = false) + { + // Acceptance tolerances + const float minDistanceFromPoint = 1e-5f; // for dimension = 1 + const float minVolumeOfTriangle = 1e-6f; // for dimension = 2 + + // Reset faces. + NumFaces = 0; + NumFaceVertices = 0; + + // Update integer space. + bool intSpaceUpdated = false; + if (!m_IntegerSpaceAabb.Contains(point)) + { + m_IntegerSpaceAabb.Include(point); + UpdateIntSpace(); + intSpaceUpdated = true; + } + + int3 intPoint = m_IntegerSpace.ToIntegerSpace(point); + + // Return false if there is not enough room to allocate a vertex. + if (Vertices.PeakCount >= Vertices.Capacity) + { + return false; + } + + // Insert vertex. + switch (Dimension) + { + // Empty hull, just add a vertex. + case -1: + { + AllocateVertex(point, userData); + Dimension = 0; + } + break; + + // 0 dimensional hull, make a line. + case 0: + { + if (project) return false; + if (math.lengthsq(Vertices[0].Position - point) <= minDistanceFromPoint) return false; + AllocateVertex(point, userData); + Dimension = 1; + } + break; + + // 1 dimensional hull, make a triangle. + case 1: + { + float3 delta = Vertices[1].Position - Vertices[0].Position; + if (project || math.lengthsq(math.cross(delta, point - Vertices[0].Position)) < minVolumeOfTriangle) + { + // Extend the line. + float3 diff = point - Vertices[0].Position; + float dot = math.dot(diff, delta); + float solution = dot * math.rcp(math.lengthsq(delta)); + float3 projectedPosition = Vertices[0].Position + solution * delta; + if (solution < 0) + { + Vertex projection = Vertices[0]; + projection.Position = projectedPosition; + projection.IntPosition = m_IntegerSpace.ToIntegerSpace(projectedPosition); + projection.UserData = userData; + Vertices[0] = projection; + } + else if (solution > 1) + { + Vertex projection = Vertices[1]; + projection.Position = projectedPosition; + projection.IntPosition = m_IntegerSpace.ToIntegerSpace(projectedPosition); + projection.UserData = userData; + Vertices[1] = projection; + } + } + else + { + // Extend dimension. + AllocateVertex(point, userData); + Dimension = 2; + } + } + break; + + // 2 dimensional hull, make a volume or expand face. + case 2: + { + long det = project ? 0 : Det64(0, 1, 2, intPoint); + if (det == 0) + { + // Grow. + Plane projectionPlane = ComputeProjectionPlane(); + if (project) + { + point -= projectionPlane.Normal * Dotxyz1(projectionPlane, point); + intPoint = m_IntegerSpace.ToIntegerSpace(point); + } + + bool* isOutside = stackalloc bool[Vertices.PeakCount]; + bool isOutsideAny = false; + for (int i = Vertices.PeakCount - 1, j = 0; j < Vertices.PeakCount; i = j++) + { + float sign = math.dot(projectionPlane.Normal, math.cross(Vertices[j].Position - point, Vertices[i].Position - point)); + isOutsideAny |= isOutside[i] = sign > 0; + } + if (isOutsideAny) + { + Vertex* newVertices = stackalloc Vertex[Vertices.PeakCount + 1]; + int numNewVertices = 1; + newVertices[0] = new Vertex(point, userData, m_NextUid++); + newVertices[0].IntPosition = m_IntegerSpace.ToIntegerSpace(point); + for (int i = Vertices.PeakCount - 1, j = 0; j < Vertices.PeakCount; i = j++) + { + if (isOutside[i] && isOutside[i] != isOutside[j]) + { + newVertices[numNewVertices++] = Vertices[j]; + for (; ; ) + { + if (isOutside[j]) break; + j = (j + 1) % Vertices.PeakCount; + newVertices[numNewVertices++] = Vertices[j]; + } + break; + } + } + + Vertices.CopyFrom(newVertices, numNewVertices); + } + } + else + { + // Extend dimension. + + // Orient tetrahedron. + if (det > 0) + { + Vertex t = Vertices[2]; + Vertices[2] = Vertices[1]; + Vertices[1] = t; + } + + // Allocate vertex. + int nv = Vertices.PeakCount; + int vertexIndex = AllocateVertex(point, userData); + + // Build tetrahedron. + Dimension = 3; + Edge nt0 = AllocateTriangle(0, 1, 2); + Edge nt1 = AllocateTriangle(1, 0, vertexIndex); + Edge nt2 = AllocateTriangle(2, 1, vertexIndex); + Edge nt3 = AllocateTriangle(0, 2, vertexIndex); + BindEdges(nt0, nt1); BindEdges(nt0.Next, nt2); BindEdges(nt0.Prev, nt3); + BindEdges(nt1.Prev, nt2.Next); BindEdges(nt2.Prev, nt3.Next); BindEdges(nt3.Prev, nt1.Next); + + // Re-insert other vertices. + bool success = true; + for (int i = 3; i < nv; ++i) + { + Vertex vertex = Vertices[i]; + Vertices.Release(i); + success = success & AddPoint(vertex.Position, vertex.UserData); + } + return success; + } + } + break; + + // 3 dimensional hull, add vertex. + case 3: + { + if (intSpaceUpdated) + { + // Check if convexity still holds. + bool isFlat = false; + for (Edge edge = GetFirstPrimaryEdge(); edge.IsValid; edge = GetNextPrimaryEdge(edge)) + { + int i = StartVertex(edge), j = EndVertex(edge), k = ApexVertex(edge), l = ApexVertex(GetLinkedEdge(edge)); + long det = Det64(i, j, k, l); + if (det > 0) + { + // Found a concave edge, release triangles, create tetrahedron and reinsert vertices. + for (int t = Triangles.GetFirstIndex(); t != -1;) + { + int n = Triangles.GetNextIndex(t); + ReleaseTriangle(t, false); + t = n; + } + + Edge t0 = AllocateTriangle(j, i, k); + Edge t1 = AllocateTriangle(i, j, l); + Edge t2 = AllocateTriangle(l, k, i); + Edge t3 = AllocateTriangle(k, l, j); + BindEdges(t0, t1); BindEdges(t2, t3); + BindEdges(t0.Next, t2.Next); BindEdges(t0.Prev, t3.Prev); + BindEdges(t1.Next, t3.Next); BindEdges(t1.Prev, t2.Prev); + + for (int v = Vertices.GetFirstIndex(); v != -1;) + { + int n = Vertices.GetNextIndex(v); + if (Vertices[v].Cardinality == 0) + { + Vertex cv = Vertices[v]; + Vertices.Release(v); + AddPoint(cv.Position, cv.UserData); + } + v = n; + } + break; + } + + isFlat &= det == 0; + } + + // Hull became flat following int space update. + Assert.IsFalse(isFlat); + } + + int* nextTriangles = stackalloc int[Triangles.PeakCount]; + for (int i = 0; i < Triangles.PeakCount; i++) + { + nextTriangles[i] = -1; + } + + Edge* newEdges = stackalloc Edge[Vertices.PeakCount]; + for (int i = 0; i < Vertices.PeakCount; i++) + { + newEdges[i] = Edge.Invalid; + } + + // Classify all triangles as either front(faceIndex = 1) or back(faceIndex = -1). + int firstFrontTriangleIndex = -1, numFrontTriangles = 0, numBackTriangles = 0; + float3 floatPoint = m_IntegerSpace.ToFloatSpace(intPoint); + float maxDistance = 0.0f; + foreach (int triangleIndex in Triangles.Indices) + { + Triangle triangle = Triangles[triangleIndex]; + long det = Det64(triangle.Vertex0, triangle.Vertex1, triangle.Vertex2, intPoint); + if (det == 0) + { + // Check for duplicated vertex. + if (math.all(Vertices[triangle.Vertex0].IntPosition == intPoint)) return false; + if (math.all(Vertices[triangle.Vertex1].IntPosition == intPoint)) return false; + if (math.all(Vertices[triangle.Vertex2].IntPosition == intPoint)) return false; + } + if (det > 0) + { + newEdges[triangle.Vertex0] = Edge.Invalid; + newEdges[triangle.Vertex1] = Edge.Invalid; + newEdges[triangle.Vertex2] = Edge.Invalid; + + nextTriangles[triangleIndex] = firstFrontTriangleIndex; + firstFrontTriangleIndex = triangleIndex; + + triangle.FaceIndex = 1; + numFrontTriangles++; + + Plane plane = ComputePlane(triangleIndex, true); + float distance = math.dot(plane.Normal, floatPoint) + plane.Distance; + maxDistance = math.max(distance, maxDistance); + } + else + { + triangle.FaceIndex = -1; + numBackTriangles++; + } + Triangles[triangleIndex] = triangle; + } + + // Return false if the vertex is inside the hull + if (numFrontTriangles == 0 || numBackTriangles == 0) + { + return false; + } + + // Return false is the vertex is too close to the surface to be inserted + if (maxDistance <= minDistanceFromPoint) + { + return false; + } + + // Link boundary loop. + Edge loopEdge = Edge.Invalid; + int loopCount = 0; + for (int frontTriangle = firstFrontTriangleIndex; frontTriangle != -1; frontTriangle = nextTriangles[frontTriangle]) + { + for (int j = 0; j < 3; ++j) + { + var edge = new Edge(frontTriangle, j); + Edge linkEdge = GetLinkedEdge(edge); + if (Triangles[linkEdge.TriangleIndex].FaceIndex == -1) + { + int vertexIndex = StartVertex(linkEdge); + + // Vertex already bound. + Assert.IsTrue(newEdges[vertexIndex].Equals(Edge.Invalid)); + + // Link. + newEdges[vertexIndex] = linkEdge; + loopEdge = linkEdge; + loopCount++; + } + } + } + + // Return false if there is not enough room to allocate new triangles. + if ((Triangles.PeakCount + loopCount - numFrontTriangles) > Triangles.Capacity) + { + return false; + } + + // Release front triangles. + do + { + int next = nextTriangles[firstFrontTriangleIndex]; + ReleaseTriangle(firstFrontTriangleIndex); + firstFrontTriangleIndex = next; + } while (firstFrontTriangleIndex != -1); + + // Add vertex. + int newVertex = AllocateVertex(point, userData); + + // Add fan of triangles. + Edge firstFanEdge = Edge.Invalid, lastFanEdge = Edge.Invalid; + for (int i = 0; i < loopCount; ++i) + { + int v0 = StartVertex(loopEdge), v1 = EndVertex(loopEdge); + Edge t = AllocateTriangle(v1, v0, newVertex); + BindEdges(loopEdge, t); + if (lastFanEdge.IsValid) + BindEdges(t.Next, lastFanEdge.Prev); + else + firstFanEdge = t; + + lastFanEdge = t; + loopEdge = newEdges[v1]; + } + BindEdges(lastFanEdge.Prev, firstFanEdge.Next); + } + break; + } + return true; + } + + /// + /// Set the face index for each triangle. + /// Triangles lying in the same plane will have the same face index. + /// + [BurstDiscard] + public void BuildFaceIndices(float maxAngle = (float)(1 * System.Math.PI / 180)) + { + float maxCosAngle = math.cos(maxAngle); + const float convexEps = 1e-5f; + + NumFaces = 0; + NumFaceVertices = 0; + + switch (Dimension) + { + case 2: + NumFaces = 1; + NumFaceVertices = Vertices.PeakCount; + break; + + case 3: + { + var sortedTriangles = new List(); + var triangleAreas = new float[Triangles.PeakCount]; + foreach (int triangleIndex in Triangles.Indices) + { + Triangle t = Triangles[triangleIndex]; + float3 o = Vertices[t.Vertex0].Position; + float3 a = Vertices[t.Vertex1].Position - o; + float3 b = Vertices[t.Vertex2].Position - o; + triangleAreas[triangleIndex] = math.lengthsq(math.cross(a, b)); + t.FaceIndex = -1; + Triangles[triangleIndex] = t; + sortedTriangles.Add(triangleIndex); + } + + sortedTriangles.Sort((a, b) => triangleAreas[b].CompareTo(triangleAreas[a])); + + var boundaryEdges = new List(); + foreach (int triangleIndex in sortedTriangles) + { + if (Triangles[triangleIndex].FaceIndex != -1) + { + continue; + } + int newFaceIndex = NumFaces++; + float3 normal = ComputePlane(triangleIndex).Normal; + Triangle t = Triangles[triangleIndex]; t.FaceIndex = newFaceIndex; Triangles[triangleIndex] = t; + + boundaryEdges.Clear(); + boundaryEdges.Add(new Edge(triangleIndex, 0)); + boundaryEdges.Add(new Edge(triangleIndex, 1)); + boundaryEdges.Add(new Edge(triangleIndex, 2)); + + while (true) + { + int openBoundaryEdgeIndex = -1; + float maxArea = -1; + + for (int i = 0; i < boundaryEdges.Count; ++i) + { + Edge edge = boundaryEdges[i]; + Edge linkedEdge = GetLinkedEdge(edge); + + int linkedTriangleIndex = linkedEdge.TriangleIndex; + + if (Triangles[linkedTriangleIndex].FaceIndex != -1) continue; + if (triangleAreas[linkedTriangleIndex] <= maxArea) continue; + if (math.dot(normal, ComputePlane(linkedTriangleIndex).Normal) < maxCosAngle) continue; + + int apex = ApexVertex(linkedEdge); + Plane p0 = PlaneFromTwoEdges(Vertices[apex].Position, Vertices[apex].Position - Vertices[StartVertex(edge)].Position, normal); + Plane p1 = PlaneFromTwoEdges(Vertices[apex].Position, Vertices[EndVertex(edge)].Position - Vertices[apex].Position, normal); + + var accept = true; + for (int j = 1; accept && j < (boundaryEdges.Count - 1); ++j) + { + float3 x = Vertices[EndVertex(boundaryEdges[(i + j) % boundaryEdges.Count])].Position; + float d = math.max(Dotxyz1(p0, x), Dotxyz1(p1, x)); + accept &= d < convexEps; + } + + if (accept) + { + openBoundaryEdgeIndex = i; + maxArea = triangleAreas[linkedTriangleIndex]; + } + } + + if (openBoundaryEdgeIndex != -1) + { + Edge linkedEdge = GetLinkedEdge(boundaryEdges[openBoundaryEdgeIndex]); + boundaryEdges[openBoundaryEdgeIndex] = linkedEdge.Prev; + boundaryEdges.Insert(openBoundaryEdgeIndex, linkedEdge.Next); + + Triangle tri = Triangles[linkedEdge.TriangleIndex]; + tri.FaceIndex = newFaceIndex; + Triangles[linkedEdge.TriangleIndex] = tri; + } + else + { + break; + } + } + NumFaceVertices += boundaryEdges.Count; + } + } + break; + } + } + + /// + /// Simplify convex hull. + /// + /// + /// + /// + [BurstDiscard] + public unsafe int SimplifyVertices(float maxError, int minVertices = 0, float volumeConservation = 1) + { + // Ensure that parameters are valid. + maxError = math.max(0, maxError); + minVertices = math.max(0, minVertices); + volumeConservation = math.clamp(volumeConservation, 0, 1); + + // Allocate new points. + var totalVerticesRemoved = 0; + int numVertices = Vertices.PeakCount; + var newPoints = new List(); + + // Keep simplifying until no changes are possible. + while (true) + { + // 2D hull. + if (Dimension == 2 && Vertices.PeakCount > 3) + { + Plane projectionPlane = ComputeProjectionPlane(); + + Plane* edgePlanes = stackalloc Plane[Vertices.PeakCount]; + for (int n = Vertices.PeakCount, i = n - 1, j = 0; j < n; i = j++) + { + edgePlanes[i] = PlaneFromTwoEdges(Vertices[i].Position, Vertices[j].Position - Vertices[i].Position, projectionPlane.Normal); + } + + float4x4 m; + m.c2 = projectionPlane; + m.c3 = new float4(0, 0, 0, 1); + + for (int n = Vertices.PeakCount, j = 0; j < n; ++j) + { + int i = (j + n - 1) % n, k = (j + 1) % n; + m.c0 = edgePlanes[i]; + m.c1 = edgePlanes[k]; + float3 position = math.inverse(math.transpose(m)).c3.xyz; + var vertex = new float4(position, Dotxyz1(edgePlanes[j], position)); + newPoints.Add(new EdgeCollapse { StartVertex = j, EndVertex = k, Data = vertex }); + } + } + // 3D hull. + else if (Dimension == 3) + { + var perVertexPlanes = new float4[Vertices.PeakCount]; + + foreach (int triangleIndex in Triangles.Indices) + { + Triangle t = Triangles[triangleIndex]; + var p = (float4)ComputePlane(triangleIndex); + float3 v0 = Vertices[t.Vertex0].Position; + float3 v1 = Vertices[t.Vertex1].Position; + float3 v2 = Vertices[t.Vertex2].Position; + perVertexPlanes[t.Vertex0] += p * math.acos(math.dot(math.normalize(v1 - v0), math.normalize(v2 - v0))); + perVertexPlanes[t.Vertex1] += p * math.acos(math.dot(math.normalize(v0 - v1), math.normalize(v2 - v1))); + perVertexPlanes[t.Vertex2] += p * math.acos(math.dot(math.normalize(v0 - v2), math.normalize(v1 - v2))); + } + + for (int vertexIndex = 0; vertexIndex < Vertices.PeakCount; ++vertexIndex) + { + if (!Vertices[vertexIndex].IsAllocated) + { + numVertices--; + perVertexPlanes[vertexIndex] = new Plane(); + } + else + { + perVertexPlanes[vertexIndex] = PlaneFromDirection(Vertices[vertexIndex].Position, perVertexPlanes[vertexIndex].xyz); + } + } + + foreach (int triangleIndex in Triangles.Indices) + { + for (int sideIndex = 0; sideIndex < 3; ++sideIndex) + { + var edge = new Edge(triangleIndex, sideIndex); + if (!IsPrimaryEdge(edge)) continue; + + float3 midPoint = (Vertices[StartVertex(edge)].Position + Vertices[EndVertex(edge)].Position) / 2; + Plane midPlane = PlaneFromDirection(midPoint, perVertexPlanes[StartVertex(edge)].xyz + perVertexPlanes[EndVertex(edge)].xyz); + + float4x4 m; + m.c0 = perVertexPlanes[StartVertex(edge)]; + m.c1 = perVertexPlanes[EndVertex(edge)]; + m.c2 = PlaneFromTwoEdges(midPoint, perVertexPlanes[StartVertex(edge)].xyz, perVertexPlanes[EndVertex(edge)].xyz); + m.c3 = new float4(0, 0, 0, 1); + + float det = Det(m.c0.xyz, m.c1.xyz, m.c2.xyz); + if (det > 1e-6f) + { + float4 vertex = math.inverse(math.transpose(m)).c3; + vertex.w = Dotxyz1(midPlane, vertex.xyz); + newPoints.Add(new EdgeCollapse { StartVertex = StartVertex(edge), EndVertex = EndVertex(edge), Data = vertex }); + } + } + } + } + + int verticesRemoved = 0; + + if (newPoints.Count > 0 || numVertices > minVertices) + { + // Sort new points. + newPoints.Sort((a, b) => a.Data.w < b.Data.w ? -1 : (a.Data.w > b.Data.w ? 1 : 0)); + + // Filter out new points. + bool* exclude = stackalloc bool[Vertices.PeakCount]; + for (int i = 0; i < Vertices.PeakCount; ++i) + { + exclude[i] = false; + } + + for (int i = 0; i < newPoints.Count; ++i) + { + EdgeCollapse p = newPoints[i]; + if (numVertices <= minVertices || exclude[p.StartVertex] || exclude[p.EndVertex] || p.Data.w <= 0 || p.Data.w > maxError) + { + p.Data.w = -1; + newPoints[i] = p; + } + else + { + if (volumeConservation < 1) + { + float3 midPoint = (Vertices[p.StartVertex].Position + Vertices[p.EndVertex].Position) / 2; + p.Data.xyz = midPoint + (midPoint - p.Data.xyz) * volumeConservation; + newPoints[i] = p; + } + exclude[p.StartVertex] = exclude[p.EndVertex] = true; + numVertices--; + verticesRemoved++; + } + } + + // Add existing vertices not referenced by edge collapse. + for (int i = 0; i < Vertices.PeakCount; i++) + { + if (!exclude[i]) + { + newPoints.Add(new EdgeCollapse { Data = new float4(Vertices[i].Position, 1) }); + } + } + + // Rebuild convex hull. + Reset(); + for (int i = 0; i < newPoints.Count; ++i) + { + if (newPoints[i].Data.w > 0) + { + AddPoint(newPoints[i].Data.xyz, (uint)i); + } + } + } + + if (verticesRemoved > 0) + { + newPoints.Clear(); + numVertices = Vertices.PeakCount; + totalVerticesRemoved += verticesRemoved; + } + else + { + break; + } + } + + return totalVerticesRemoved; + } + + private int AllocateVertex(float3 point, uint userData) + { + Assert.IsTrue(m_IntegerSpaceAabb.Contains(point)); + var vertex = new Vertex(point, userData, m_NextUid++) { IntPosition = m_IntegerSpace.ToIntegerSpace(point) }; + return Vertices.Allocate(vertex); + } + + private Edge AllocateTriangle(int vertex0, int vertex1, int vertex2) + { + Triangle triangle = new Triangle(vertex0, vertex1, vertex2, m_NextUid++); + int triangleIndex = Triangles.Allocate(triangle); + + Vertex v; + v = Vertices[vertex0]; v.Cardinality++; Vertices[vertex0] = v; + v = Vertices[vertex1]; v.Cardinality++; Vertices[vertex1] = v; + v = Vertices[vertex2]; v.Cardinality++; Vertices[vertex2] = v; + + return new Edge(triangleIndex, 0); + } + + private void ReleaseTriangle(int triangle, bool releaseOrphanVertices = true) + { + for (int i = 0; i < 3; ++i) + { + int j = Triangles[triangle].GetVertex(i); + Vertex v = Vertices[j]; + v.Cardinality--; + Vertices[j] = v; + if (v.Cardinality == 0 && releaseOrphanVertices) + { + Vertices.Release(j); + } + } + + Triangles.Release(triangle); + } + + private void BindEdges(Edge lhs, Edge rhs) + { + // Incompatible edges. + Assert.IsTrue(EndVertex(lhs) == StartVertex(rhs) && StartVertex(lhs) == EndVertex(rhs)); + + Triangle lf = Triangles[lhs.TriangleIndex]; + Triangle rf = Triangles[rhs.TriangleIndex]; + lf.SetLink(lhs.EdgeIndex, rhs); + rf.SetLink(rhs.EdgeIndex, lhs); + Triangles[lhs.TriangleIndex] = lf; + Triangles[rhs.TriangleIndex] = rf; + } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + private void UpdateIntSpace() + { + const int quantizationBits = 16; + const float minExtent = 1e-5f; + m_IntegerSpace = new IntegerSpace(m_IntegerSpaceAabb, (1 << quantizationBits) - 1, true, minExtent); + foreach (int i in Vertices.Indices) + { + Vertex v = Vertices[i]; + v.IntPosition = m_IntegerSpace.ToIntegerSpace(Vertices[i].Position); + Vertices[i] = v; + } + } + + #endregion + + #region Edge methods + + /// + /// Returns one of the triangle edges starting from a given vertex. + /// Note: May be one of the inner edges of a face. + /// + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public Edge GetVertexEdge(int vertexIndex) + { + Assert.IsTrue(Dimension == 3, "This method is only working on 3D convex hulls."); + foreach (int triangleIndex in Triangles.Indices) + { + Triangle triangle = Triangles[triangleIndex]; + if (triangle.Vertex0 == vertexIndex) return new Edge(triangleIndex, 0); + if (triangle.Vertex1 == vertexIndex) return new Edge(triangleIndex, 1); + if (triangle.Vertex2 == vertexIndex) return new Edge(triangleIndex, 2); + } + return Edge.Invalid; + } + + /// + /// Returns an edge's linked edge on the neighboring triangle. + /// + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public Edge GetLinkedEdge(Edge edge) => edge.IsValid ? Triangles[edge.TriangleIndex].GetLink(edge.EdgeIndex) : edge; + + /// + /// Returns true if the edge's triangle has a higher index than its linked triangle. + /// + [MethodImpl(MethodImplOptions.AggressiveInlining)] + private bool IsPrimaryEdge(Edge edge) => edge.TriangleIndex > GetLinkedEdge(edge).TriangleIndex; + + public Edge GetFirstPrimaryEdge() + { + foreach (int triangleIndex in Triangles.Indices) + { + for (int vi = 0; vi < 3; ++vi) + { + var edge = new Edge(triangleIndex, vi); + if (IsPrimaryEdge(edge)) + { + return edge; + } + } + } + return Edge.Invalid; + } + + public Edge GetNextPrimaryEdge(Edge edge) + { + while (edge.EdgeIndex < 2) + { + edge = edge.Next; + if (IsPrimaryEdge(edge)) + { + return edge; + } + } + for (int triangleIndex = Triangles.GetNextIndex(edge.TriangleIndex); triangleIndex != -1; triangleIndex = Triangles.GetNextIndex(triangleIndex)) + { + for (int vi = 0; vi < 3; ++vi) + { + edge = new Edge(triangleIndex, vi); + if (IsPrimaryEdge(edge)) + { + return edge; + } + } + } + return Edge.Invalid; + } + + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public int StartVertex(Edge edge) => Triangles[edge.TriangleIndex].GetVertex(edge.EdgeIndex); + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public int EndVertex(Edge edge) => StartVertex(edge.Next); + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public int ApexVertex(Edge edge) => StartVertex(edge.Prev); + + + /// + /// Returns (the first edge of) the first face. + /// + public FaceEdge GetFirstFace() + { + return NumFaces > 0 ? GetFirstFace(0) : FaceEdge.Invalid; + } + + /// + /// Returns the first face edge from a given face index. + /// + public FaceEdge GetFirstFace(int faceIndex) + { + foreach (int triangleIndex in Triangles.Indices) + { + if (Triangles[triangleIndex].FaceIndex != faceIndex) + { + continue; + } + for (int i = 0; i < 3; ++i) + { + var edge = new Edge(triangleIndex, i); + if (Triangles[GetLinkedEdge(edge).TriangleIndex].FaceIndex != faceIndex) + { + return new FaceEdge { Start = edge, Current = edge }; + } + } + } + return FaceEdge.Invalid; + } + + /// + /// Returns (the first edge of) the next face. + /// + public FaceEdge GetNextFace(FaceEdge fe) + { + int faceIndex = fe.IsValid ? Triangles[fe.Start.TriangleIndex].FaceIndex + 1 : 0; + if (faceIndex < NumFaces) + return GetFirstFace(faceIndex); + return FaceEdge.Invalid; + } + + /// + /// Returns the next edge within a face. + /// + public FaceEdge GetNextFaceEdge(FaceEdge fe) + { + int faceIndex = Triangles[fe.Start.TriangleIndex].FaceIndex; + bool found = false; + fe.Current = fe.Current.Next; + for (int n = Vertices[StartVertex(fe.Current)].Cardinality; n > 0; --n) + { + if (Triangles[GetLinkedEdge(fe.Current).TriangleIndex].FaceIndex == faceIndex) + { + fe.Current = GetLinkedEdge(fe.Current).Next; + } + else + { + found = true; + break; + } + } + + if (!found || fe.Current.Equals(fe.Start)) + return FaceEdge.Invalid; + return fe; + } + + #endregion + + #region Queries + + /// + /// Compute vertex normal. + /// + [BurstDiscard] + public float3 ComputeVertexNormal(int vertex) + { + return ComputeVertexNormal(GetVertexEdge(vertex)); + } + + /// + /// Compute vertex normal. + /// + [BurstDiscard] + public float3 ComputeVertexNormal(Edge edge) + { + float3 n = new float3(0); + switch (Dimension) + { + case 2: + n = ComputeProjectionPlane().Normal; + break; + + case 3: + float3 origin = Vertices[StartVertex(edge)].Position; + float3 dir0 = math.normalize(Vertices[EndVertex(edge)].Position - origin); + for (int c = Vertices[StartVertex(edge)].Cardinality, i = 0; i < c; ++i) + { + edge = GetLinkedEdge(edge).Next; + float3 dir1 = math.normalize(Vertices[EndVertex(edge)].Position - origin); + n += math.acos(math.dot(dir0, dir1)) * ComputePlane(edge.TriangleIndex).Normal; + dir0 = dir1; + } + n = math.normalize(n); + break; + } + return n; + } + + /// + /// Returns the AABB of the convex hull. + /// + public Aabb ComputeAabb() + { + Aabb aabb = Aabb.Empty; + foreach (Vertex vertex in Vertices.Elements) + { + aabb.Include(vertex.Position); + } + return aabb; + } + + /// + /// Returns the supporting vertex index in a given direction. + /// + public int ComputeSupportingVertex(float3 direction) + { + int bi = 0; + float maxDot = float.MinValue; + foreach (int i in Vertices.Indices) + { + float d = math.dot(direction, Vertices[i].Position); + if (d > maxDot) + { + bi = i; + maxDot = d; + } + } + return bi; + } + + /// + /// Returns the centroid of the convex hull. + /// + public float3 ComputeCentroid() + { + float4 sum = new float4(0); + foreach (Vertex vertex in Vertices.Elements) + { + sum += new float4(vertex.Position, 1); + } + + if (sum.w > 0) + return sum.xyz / sum.w; + return new float3(0); + } + + /// + /// Compute the mass properties of the convex hull. + /// Note: Inertia computation adapted from S. Melax, http://www.melax.com/volint. + /// + public unsafe MassProperties ComputeMassProperties() + { + var mp = new MassProperties(); + switch (Dimension) + { + case 0: + mp.CenterOfMass = Vertices[0].Position; + break; + case 1: + mp.CenterOfMass = (Vertices[0].Position + Vertices[1].Position) * 0.5f; + break; + case 2: + { + float3 offset = ComputeCentroid(); + for (int n = Vertices.PeakCount, i = n - 1, j = 0; j < n; i = j++) + { + float w = math.length(math.cross(Vertices[i].Position - offset, Vertices[j].Position - offset)); + mp.CenterOfMass += (Vertices[i].Position + Vertices[j].Position + offset) * w; + mp.SurfaceArea += w; + } + mp.CenterOfMass /= mp.SurfaceArea * 3; + mp.InertiaTensor = float3x3.identity; // + mp.SurfaceArea *= 0.5f; + } + break; + case 3: + { + float3 offset = ComputeCentroid(); + int numTriangles = 0; + float* dets = stackalloc float[Triangles.Capacity]; + foreach (int i in Triangles.Indices) + { + float3 v0 = Vertices[Triangles[i].Vertex0].Position - offset; + float3 v1 = Vertices[Triangles[i].Vertex1].Position - offset; + float3 v2 = Vertices[Triangles[i].Vertex2].Position - offset; + float w = Det(v0, v1, v2); + mp.CenterOfMass += (v0 + v1 + v2) * w; + mp.Volume += w; + mp.SurfaceArea += math.length(math.cross(v1 - v0, v2 - v0)); + dets[i] = w; + numTriangles++; + } + + mp.CenterOfMass = mp.CenterOfMass / (mp.Volume * 4) + offset; + + var diag = new float3(0); + var offd = new float3(0); + + foreach (int i in Triangles.Indices) + { + float3 v0 = Vertices[Triangles[i].Vertex0].Position - mp.CenterOfMass; + float3 v1 = Vertices[Triangles[i].Vertex1].Position - mp.CenterOfMass; + float3 v2 = Vertices[Triangles[i].Vertex2].Position - mp.CenterOfMass; + diag += (v0 * v1 + v1 * v2 + v2 * v0 + v0 * v0 + v1 * v1 + v2 * v2) * dets[i]; + offd += (v0.yzx * v1.zxy + v1.yzx * v2.zxy + v2.yzx * v0.zxy + + v0.yzx * v2.zxy + v1.yzx * v0.zxy + v2.yzx * v1.zxy + + (v0.yzx * v0.zxy + v1.yzx * v1.zxy + v2.yzx * v2.zxy) * 2) * dets[i]; + numTriangles++; + } + + diag /= mp.Volume * (60 / 6); + offd /= mp.Volume * (120 / 6); + + mp.InertiaTensor.c0 = new float3(diag.y + diag.z, -offd.z, -offd.y); + mp.InertiaTensor.c1 = new float3(-offd.z, diag.x + diag.z, -offd.x); + mp.InertiaTensor.c2 = new float3(-offd.y, -offd.x, diag.x + diag.y); + + mp.SurfaceArea /= 2; + mp.Volume /= 6; + } + break; + } + + return mp; + } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + private Plane ComputePlane(int vertex0, int vertex1, int vertex2, bool fromIntCoordinates) + { + float3 o, a, b; + if (fromIntCoordinates) + { + o = m_IntegerSpace.ToFloatSpace(Vertices[vertex0].IntPosition); + a = m_IntegerSpace.ToFloatSpace(Vertices[vertex1].IntPosition) - o; + b = m_IntegerSpace.ToFloatSpace(Vertices[vertex2].IntPosition) - o; + } + else + { + o = Vertices[vertex0].Position; + a = Vertices[vertex1].Position - o; + b = Vertices[vertex2].Position - o; + } + float3 n = math.normalize(math.cross(a, b)); + return new Plane(n, -math.dot(n, o)); + } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public Plane ComputePlane(int triangleIndex, bool fromIntCoordinates = true) + { + return ComputePlane(Triangles[triangleIndex].Vertex0, Triangles[triangleIndex].Vertex1, Triangles[triangleIndex].Vertex2, fromIntCoordinates); + } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public Plane ComputeProjectionPlane() + { + if (Dimension == 2) + return ComputePlane(0, 1, 2, true); + return new Plane(); + } + + #endregion + + #region Helpers + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + private static long Det64(int3 a, int3 b, int3 c, int3 d) + { + long ax = b.x - a.x, ay = b.y - a.y, az = b.z - a.z; + long bx = c.x - a.x, by = c.y - a.y, bz = c.z - a.z; + long cx = d.x - a.x, cy = d.y - a.y, cz = d.z - a.z; + long kx = ay * bz - az * by, ky = az * bx - ax * bz, kz = ax * by - ay * bx; + return kx * cx + ky * cy + kz * cz; + } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + private long Det64(int a, int b, int c, int d) + { + return Det64(Vertices[a].IntPosition, Vertices[b].IntPosition, Vertices[c].IntPosition, Vertices[d].IntPosition); + } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + private long Det64(int a, int b, int c, int3 d) + { + return Det64(Vertices[a].IntPosition, Vertices[b].IntPosition, Vertices[c].IntPosition, d); + } + + #endregion + } + + + // Extensions, not part of the core functionality (yet) + public static class ConvexHullBuilderExtensions + { + /// + /// Simplify faces. + /// + [BurstDiscard] + public static int SimplifyFaces(this ConvexHullBuilder builder, int maxFaces, float minError, float minAngle) + { + // This method is only working on 3D convex hulls. + Assert.IsTrue(builder.Dimension == 3); + + builder.BuildFaceIndices(); + + maxFaces = math.max(6, maxFaces < 0 ? builder.NumFaces : maxFaces); + minError = math.max(0, minError); + + float cosAngle = math.clamp(math.cos(minAngle), 0, 1); + var planes = new List(); + + foreach (int triangleIndex in builder.Triangles.Indices) + { + planes.Add(builder.ComputePlane(triangleIndex)); + } + + Aabb actualAabb = builder.ComputeAabb(); + + builder.Reset(); + builder.AddPoint(math.select(actualAabb.Min, actualAabb.Max, new bool3(false, false, false))); + builder.AddPoint(math.select(actualAabb.Min, actualAabb.Max, new bool3(true, false, false))); + builder.AddPoint(math.select(actualAabb.Min, actualAabb.Max, new bool3(true, true, false))); + builder.AddPoint(math.select(actualAabb.Min, actualAabb.Max, new bool3(false, true, false))); + builder.AddPoint(math.select(actualAabb.Min, actualAabb.Max, new bool3(false, false, true))); + builder.AddPoint(math.select(actualAabb.Min, actualAabb.Max, new bool3(true, false, true))); + builder.AddPoint(math.select(actualAabb.Min, actualAabb.Max, new bool3(true, true, true))); + builder.AddPoint(math.select(actualAabb.Min, actualAabb.Max, new bool3(false, true, true))); + + builder.BuildFaceIndices(); + + while (builder.NumFaces < maxFaces && planes.Count > 0) + { + float maxd = 0; + int bestPlane = -1; + + float3[] normals = null; + if (cosAngle < 1) + { + normals = new float3[builder.Triangles.PeakCount]; + foreach (int triangleIndex in builder.Triangles.Indices) + { + normals[triangleIndex] = builder.ComputePlane(triangleIndex).Normal; + } + } + + for (int i = 0; i < planes.Count; ++i) + { + float d = Dotxyz1(planes[i], builder.Vertices[builder.ComputeSupportingVertex(planes[i].Normal)].Position); + if (d > maxd) + { + if (normals != null) + { + bool isAngleOk = true; + foreach (int triangleIndex in builder.Triangles.Indices) + { + if (math.dot(normals[triangleIndex], planes[i].Normal) > cosAngle) + { + isAngleOk = false; + break; + } + } + if (!isAngleOk) + { + continue; + } + } + bestPlane = i; + maxd = d; + } + } + + if (bestPlane == -1) break; + if (maxd <= minError) break; + + ConvexHullBuilder negHull, posHull; + SplitByPlane(builder, planes[bestPlane], out negHull, out posHull); + planes.RemoveAt(bestPlane); + if (negHull.Dimension == 3) + { + negHull.BuildFaceIndices(); + if (negHull.NumFaces <= maxFaces) + { + builder.CopyFrom(negHull); + } + } + posHull.Dispose(); + negHull.Dispose(); + } + + return builder.NumFaces; + } + + /// + /// Split this convex hull by a plane. + /// + [BurstDiscard] + public static void SplitByPlane(this ConvexHullBuilder builder, Plane plane, out ConvexHullBuilder negHull, out ConvexHullBuilder posHull, uint isecUserData = 0) + { + negHull = new ConvexHullBuilder(builder.Vertices.PeakCount * 2, builder.Vertices.PeakCount * 4); + posHull = new ConvexHullBuilder(builder.Vertices.PeakCount * 2, builder.Vertices.PeakCount * 4); + negHull.IntegerSpaceAabb = builder.IntegerSpaceAabb; + posHull.IntegerSpaceAabb = builder.IntegerSpaceAabb; + + // Compute vertices signed distance to plane and add them to the hull they belong too. + const float minD2P = 1e-6f; + var perVertexD2P = new float[builder.Vertices.PeakCount]; + foreach (int i in builder.Vertices.Indices) + { + float3 position = builder.Vertices[i].Position; + perVertexD2P[i] = Dotxyz1(plane, position); + if (math.abs(perVertexD2P[i]) <= minD2P) perVertexD2P[i] = 0; + if (perVertexD2P[i] <= 0) negHull.AddPoint(position, builder.Vertices[i].UserData); + if (perVertexD2P[i] >= 0) posHull.AddPoint(position, builder.Vertices[i].UserData); + } + + // Add intersecting vertices. + switch (builder.Dimension) + { + case 1: + { + if ((perVertexD2P[0] * perVertexD2P[1]) < 0) + { + float sol = perVertexD2P[0] / (perVertexD2P[0] - perVertexD2P[1]); + float3 isec = builder.Vertices[0].Position + (builder.Vertices[1].Position - builder.Vertices[0].Position) * sol; + negHull.AddPoint(isec, isecUserData); + posHull.AddPoint(isec, isecUserData); + } + } + break; + case 2: + { + for (int n = builder.Vertices.PeakCount, i = n - 1, j = 0; j < n; i = j++) + { + if ((perVertexD2P[i] * perVertexD2P[j]) < 0) + { + float sol = perVertexD2P[i] / (perVertexD2P[i] - perVertexD2P[j]); + float3 isec = builder.Vertices[i].Position + (builder.Vertices[j].Position - builder.Vertices[i].Position) * sol; + negHull.AddPoint(isec, isecUserData); + posHull.AddPoint(isec, isecUserData); + } + } + } + break; + case 3: + { + var edges = new List(); + for (ConvexHullBuilder.Edge edge = builder.GetFirstPrimaryEdge(); edge.IsValid; edge = builder.GetNextPrimaryEdge(edge)) + { + float d = math.dot(builder.ComputePlane(edge.TriangleIndex).Normal, builder.ComputePlane(builder.GetLinkedEdge(edge).TriangleIndex).Normal); + edges.Add(new ConvexHullBuilder.EdgeCollapse { StartVertex = builder.StartVertex(edge), EndVertex = builder.EndVertex(edge), Data = new float4(d) }); + } + + // Insert sharpest edge intersection first to improve quality. + edges.Sort((a, b) => a.Data.x.CompareTo(b.Data.x)); + + foreach (ConvexHullBuilder.EdgeCollapse edge in edges) + { + int i = edge.StartVertex, j = edge.EndVertex; + if ((perVertexD2P[i] * perVertexD2P[j]) < 0) + { + float sol = perVertexD2P[i] / (perVertexD2P[i] - perVertexD2P[j]); + float3 isec = builder.Vertices[i].Position + (builder.Vertices[j].Position - builder.Vertices[i].Position) * sol; + negHull.AddPoint(isec, isecUserData); + posHull.AddPoint(isec, isecUserData); + } + } + } + break; + } + } + + /// + /// For 3D convex hulls, vertex indices are not always continuous, this methods compact them. + /// + [BurstDiscard] + public static unsafe void CompactVertices(this ConvexHullBuilder builder) + { + if (builder.Dimension == 3) + { + int* newIndices = stackalloc int[builder.Vertices.PeakCount]; + ConvexHullBuilder.Vertex* newVertices = stackalloc ConvexHullBuilder.Vertex[builder.Vertices.PeakCount]; + int numNewVertices = 0; + bool needsCompacting = false; + foreach (int vertexIndex in builder.Vertices.Indices) + { + needsCompacting |= vertexIndex != numNewVertices; + newIndices[vertexIndex] = numNewVertices; + newVertices[numNewVertices++] = builder.Vertices[vertexIndex]; + } + + if (needsCompacting) + { + builder.Vertices.CopyFrom(newVertices, numNewVertices); + + foreach (int triangleIndex in builder.Triangles.Indices) + { + ConvexHullBuilder.Triangle t = builder.Triangles[triangleIndex]; + t.Vertex0 = newIndices[t.Vertex0]; + t.Vertex1 = newIndices[t.Vertex1]; + t.Vertex2 = newIndices[t.Vertex2]; + builder.Triangles[triangleIndex] = t; + } + } + } + } + + /// + /// Offset vertices with respect to the surface by a given amount while minimizing distortions. + /// + /// a positive value will move vertices outward while a negative value will push them toward the inside + /// Prevent vertices to be shrunk too much + [BurstDiscard] + public static void OffsetVertices(this ConvexHullBuilder builder, float surfaceOffset, float minRadius = 0) + { + // This method is only working on 3D convex hulls. + Assert.IsTrue(builder.Dimension == 3); + + float3 com = builder.ComputeMassProperties().CenterOfMass; + ConvexHullBuilder.Vertex[] newVertices = new ConvexHullBuilder.Vertex[builder.Vertices.PeakCount]; + int numVertices = 0; + foreach (int vertexIndex in builder.Vertices.Indices) + { + ConvexHullBuilder.Edge edge = builder.GetVertexEdge(vertexIndex); + float3 normal = builder.ComputeVertexNormal(edge); + float offset = surfaceOffset; + float3 position = builder.Vertices[vertexIndex].Position; + if (surfaceOffset < 0) + { + // Clip offset to prevent flipping. + var plane = new float4(normal, -math.dot(com, normal) + minRadius); + offset = math.max(-Dotxyz1(plane, position), offset); + } + newVertices[numVertices++] = new ConvexHullBuilder.Vertex + { + Position = position + normal * offset, + UserData = builder.Vertices[vertexIndex].UserData + }; + } + + builder.Reset(); + for (int i = 0; i < numVertices; ++i) + { + builder.AddPoint(newVertices[i].Position, newVertices[i].UserData); + } + } + } +} diff --git a/package/Unity.Physics/Collision/Geometry/ConvexHullBuilder.cs.meta b/package/Unity.Physics/Collision/Geometry/ConvexHullBuilder.cs.meta new file mode 100755 index 000000000..3d42e7d79 --- /dev/null +++ b/package/Unity.Physics/Collision/Geometry/ConvexHullBuilder.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: bef4505d9d53cf144b284b839a755640 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Unity.Physics/Collision/Geometry/Mesh.cs b/package/Unity.Physics/Collision/Geometry/Mesh.cs new file mode 100755 index 000000000..46b1b9221 --- /dev/null +++ b/package/Unity.Physics/Collision/Geometry/Mesh.cs @@ -0,0 +1,402 @@ +using System; +using System.Collections.Generic; +using System.Runtime.CompilerServices; +using Unity.Collections.LowLevel.Unsafe; +using Unity.Mathematics; + +namespace Unity.Physics +{ + // A collision mesh, containing triangles and quads. + // Warning: This is just the header, the mesh's variable sized data follows it in memory. + // Therefore this struct must always be passed by reference, never by value. + public struct Mesh + { + // A set of vertex indices into the section's vertex buffer + // TODO: "Primitive" is an overloaded term, rename? + public struct PrimitiveVertexIndices + { + public byte A, B, C, D; + } + + // Flags describing how the primitive vertices are used + [Flags] + public enum PrimitiveFlags : byte + { + IsTriangle = 1 << 0, // the primitive stores a single triangle + IsTrianglePair = 1 << 1, // the primitive stores a pair of triangles + IsQuad = 1 << 2 // the primitive stores a pair of coplanar triangles, which can be represented as a single quad + } + + // A section of the mesh, containing up to 256 vertices. + public struct Section + { + public const int MaxNumVertices = 1 << 8; + + internal BlobArray PrimitiveFlagsBlob; + public BlobArray.Accessor PrimitiveFlags => new BlobArray.Accessor(ref PrimitiveFlagsBlob); + + internal BlobArray PrimitiveVertexIndicesBlob; + public BlobArray.Accessor PrimitiveVertexIndices => new BlobArray.Accessor(ref PrimitiveVertexIndicesBlob); + + internal BlobArray VerticesBlob; + public BlobArray.Accessor Vertices => new BlobArray.Accessor(ref VerticesBlob); + + internal BlobArray PrimitiveFilterIndicesBlob; + public BlobArray.Accessor PrimitiveFilterIndices => new BlobArray.Accessor(ref PrimitiveFilterIndicesBlob); + + internal BlobArray FiltersBlob; + public BlobArray.Accessor Filters => new BlobArray.Accessor(ref FiltersBlob); + + internal BlobArray PrimitiveMaterialIndicesBlob; + public BlobArray.Accessor PrimitiveMaterialIndices => new BlobArray.Accessor(ref PrimitiveMaterialIndicesBlob); + + internal BlobArray MaterialsBlob; + public BlobArray.Accessor Materials => new BlobArray.Accessor(ref MaterialsBlob); + } + + // The bounding volume + private BlobArray m_BvhNodesBlob; + public unsafe BoundingVolumeHierarchy BoundingVolumeHierarchy + { + get + { + fixed (BlobArray* blob = &m_BvhNodesBlob) + { + var firstNode = (BoundingVolumeHierarchy.Node*)((byte*)&(blob->Offset) + blob->Offset); + return new BoundingVolumeHierarchy(firstNode, nodeFilters: null); + } + } + } + + // The mesh sections + private BlobArray m_SectionsBlob; + public BlobArray.Accessor
Sections => new BlobArray.Accessor
(ref m_SectionsBlob); + + + // Get the number of bits required to store a key to any of the leaf colliders + public uint NumColliderKeyBits => (uint)((32 - math.lzcnt(Sections.Length - 1)) + 8 + 1); + + // Burst friendly HasFlag + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static bool IsPrimitveFlagSet(PrimitiveFlags flags, PrimitiveFlags testFlag) => (flags & testFlag) != 0; + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static int GetNumPolygonsInPrimitive(PrimitiveFlags primitiveFlags) => primitiveFlags == PrimitiveFlags.IsTrianglePair ? 2 : 1; + + // Get the flags of a primitive + internal PrimitiveFlags GetPrimitiveFlags(int primitiveKey) + { + int sectionIndex = primitiveKey >> 8; + int sectionPrimitiveIndex = primitiveKey & 0xFF; + + return Sections[sectionIndex].PrimitiveFlags[sectionPrimitiveIndex]; + } + + // Get the vertices, flags and collision filter of a primitive + internal void GetPrimitive(int primitiveKey, out float3x4 vertices, out PrimitiveFlags flags, out CollisionFilter filter) + { + int sectionIndex = primitiveKey >> 8; + int sectionPrimitiveIndex = primitiveKey & 0xFF; + + ref Section section = ref Sections[sectionIndex]; + + PrimitiveVertexIndices vertexIndices = section.PrimitiveVertexIndices[sectionPrimitiveIndex]; + + vertices = new float3x4( + section.Vertices[vertexIndices.A], + section.Vertices[vertexIndices.B], + section.Vertices[vertexIndices.C], + section.Vertices[vertexIndices.D]); + + flags = section.PrimitiveFlags[sectionPrimitiveIndex]; + + short filterIndex = section.PrimitiveFilterIndices[sectionPrimitiveIndex]; + filter = section.Filters[filterIndex]; + } + + // Get the vertices, flags, collision filter and material of a primitive + internal void GetPrimitive(int primitiveKey, out float3x4 vertices, out PrimitiveFlags flags, out CollisionFilter filter, out Material material) + { + int sectionIndex = primitiveKey >> 8; + int sectionPrimitiveIndex = primitiveKey & 0xFF; + + ref Section section = ref Sections[sectionIndex]; + + PrimitiveVertexIndices vertexIndices = section.PrimitiveVertexIndices[sectionPrimitiveIndex]; + + vertices = new float3x4( + section.Vertices[vertexIndices.A], + section.Vertices[vertexIndices.B], + section.Vertices[vertexIndices.C], + section.Vertices[vertexIndices.D]); + + flags = section.PrimitiveFlags[sectionPrimitiveIndex]; + + short filterIndex = section.PrimitiveFilterIndices[sectionPrimitiveIndex]; + filter = section.Filters[filterIndex]; + + short materialIndex = section.PrimitiveMaterialIndices[sectionPrimitiveIndex]; + material = section.Materials[materialIndex]; + } + + // Configure a polygon based on the given key. The polygon must be initialized already. + // Returns false if the polygon is filtered out with respect to the given filter. + internal bool GetPolygon(uint meshKey, CollisionFilter filter, ref PolygonCollider polygon) + { + int primitiveKey = (int)meshKey >> 1; + int polygonIndex = (int)meshKey & 1; + + int sectionIndex = primitiveKey >> 8; + int sectionPrimitiveIndex = primitiveKey & 0xFF; + + ref Section section = ref Sections[sectionIndex]; + + short filterIndex = section.PrimitiveFilterIndices[sectionPrimitiveIndex]; + if (!CollisionFilter.IsCollisionEnabled(filter, section.Filters[filterIndex])) + { + return false; + } + // Note: Currently not setting the filter on the output polygon, + // because the caller doesn't need it + + PrimitiveVertexIndices vertexIndices = section.PrimitiveVertexIndices[sectionPrimitiveIndex]; + var vertices = new float3x4( + section.Vertices[vertexIndices.A], + section.Vertices[vertexIndices.B], + section.Vertices[vertexIndices.C], + section.Vertices[vertexIndices.D]); + + PrimitiveFlags flags = section.PrimitiveFlags[sectionPrimitiveIndex]; + if (IsPrimitveFlagSet(flags, PrimitiveFlags.IsQuad)) + { + polygon.SetAsQuad(vertices[0], vertices[1], vertices[2], vertices[3]); + } + else + { + polygon.SetAsTriangle(vertices[0], vertices[1 + polygonIndex], vertices[2 + polygonIndex]); + } + + short materialIndex = section.PrimitiveMaterialIndices[sectionPrimitiveIndex]; + polygon.Material = section.Materials[materialIndex]; + + return true; + } + + internal bool GetFirstPolygon(out uint meshKey, ref PolygonCollider polygon) + { + if (Sections.Length == 0 || Sections[0].PrimitiveFlags.Length == 0) + { + meshKey = 0xffffffff; + return false; + } + + meshKey = 0; + + ref Section section = ref Sections[0]; + PrimitiveVertexIndices vertexIndices = section.PrimitiveVertexIndices[0]; + var vertices = new float3x4( + section.Vertices[vertexIndices.A], + section.Vertices[vertexIndices.B], + section.Vertices[vertexIndices.C], + section.Vertices[vertexIndices.D]); + + PrimitiveFlags flags = section.PrimitiveFlags[0]; + if (IsPrimitveFlagSet(flags, PrimitiveFlags.IsQuad)) + { + polygon.SetAsQuad(vertices[0], vertices[1], vertices[2], vertices[3]); + } + else + { + polygon.SetAsTriangle(vertices[0], vertices[1], vertices[2]); + } + + short filterIndex = section.PrimitiveFilterIndices[0]; + polygon.Filter = section.Filters[filterIndex]; + + short materialIndex = section.PrimitiveMaterialIndices[0]; + polygon.Material = section.Materials[materialIndex]; + + return true; + } + + internal bool GetNextPolygon(uint previousMeshKey, out uint meshKey, ref PolygonCollider polygon) + { + int primitiveKey = (int)previousMeshKey >> 1; + int polygonIndex = (int)previousMeshKey & 1; + + int sectionIndex = primitiveKey >> 8; + int sectionPrimitiveIndex = primitiveKey & 0xFF; + + // Get next primitive + { + ref Section section = ref Sections[sectionIndex]; + + if (polygonIndex == 0 && IsPrimitveFlagSet(section.PrimitiveFlags[sectionPrimitiveIndex], PrimitiveFlags.IsTrianglePair)) + { + // Move to next triangle + polygonIndex = 1; + } + else + { + // Move to next primitive + polygonIndex = 0; + + if (++sectionPrimitiveIndex == section.PrimitiveFlags.Length) + { + // Move to next geometry section + sectionPrimitiveIndex = 0; + sectionIndex++; + } + } + + if (sectionIndex >= Sections.Length) + { + // We ran out of keys. + meshKey = 0xffffffff; + return false; + } + } + + // Return its key and polygon + { + meshKey = (uint)(sectionIndex << 9 | sectionPrimitiveIndex << 1 | polygonIndex); + + ref Section section = ref Sections[sectionIndex]; + PrimitiveVertexIndices vertexIndices = section.PrimitiveVertexIndices[sectionPrimitiveIndex]; + var vertices = new float3x4( + section.Vertices[vertexIndices.A], + section.Vertices[vertexIndices.B], + section.Vertices[vertexIndices.C], + section.Vertices[vertexIndices.D]); + + PrimitiveFlags flags = section.PrimitiveFlags[sectionPrimitiveIndex]; + if (IsPrimitveFlagSet(flags, PrimitiveFlags.IsQuad)) + { + polygon.SetAsQuad(vertices[0], vertices[1], vertices[2], vertices[3]); + } + else + { + polygon.SetAsTriangle(vertices[0], vertices[1 + polygonIndex], vertices[2 + polygonIndex]); + } + + short filterIndex = section.PrimitiveFilterIndices[sectionPrimitiveIndex]; + polygon.Filter = section.Filters[filterIndex]; + + short materialIndex = section.PrimitiveMaterialIndices[sectionPrimitiveIndex]; + polygon.Material = section.Materials[materialIndex]; + + return true; + } + } + + #region Construction helpers + + // Calculate the number of bytes needed to store the given mesh data, excluding the header (sizeof(Mesh)) + internal static int CalculateMeshDataSize(int nodeCount, List tempSections) + { + int totalSize = 0; + + foreach (MeshBuilder.TempSection section in tempSections) + { + int numPrimitives = section.Primitives.Count; + totalSize += Math.NextMultipleOf(numPrimitives * UnsafeUtility.SizeOf(), 4); + totalSize += Math.NextMultipleOf(numPrimitives * UnsafeUtility.SizeOf(), 4); + totalSize += Math.NextMultipleOf(section.Vertices.Count * UnsafeUtility.SizeOf(), 4); + totalSize += Math.NextMultipleOf(numPrimitives * sizeof(short), 4); + totalSize += Math.NextMultipleOf(1 * UnsafeUtility.SizeOf(), 4); + totalSize += Math.NextMultipleOf(numPrimitives * sizeof(short), 4); + totalSize += Math.NextMultipleOf(1 * UnsafeUtility.SizeOf(), 4); + } + + int sectionBlobArraySize = Math.NextMultipleOf(tempSections.Count * UnsafeUtility.SizeOf
(), 16); + + int treeSize = nodeCount * UnsafeUtility.SizeOf(); + + return totalSize + sectionBlobArraySize + treeSize; + } + + // Initialize the data. Assumes the appropriate memory has already been allocated. + internal unsafe void Init(BoundingVolumeHierarchy.Node* nodes, int nodeCount, List tempSections, CollisionFilter filter, Material material) + { + byte* end = (byte*)UnsafeUtility.AddressOf(ref this) + sizeof(Mesh); + end = (byte*)Math.NextMultipleOf16((ulong)end); + + m_BvhNodesBlob.Offset = (int)(end - (byte*)(UnsafeUtility.AddressOf(ref m_BvhNodesBlob))); + m_BvhNodesBlob.Length = nodeCount; + int sizeOfNodes = sizeof(BoundingVolumeHierarchy.Node) * nodeCount; + UnsafeUtility.MemCpy(end, nodes, sizeOfNodes); + end += sizeOfNodes; + + Section* section = (Section*)end; + + m_SectionsBlob.Offset = (int)(end - (byte*)(UnsafeUtility.AddressOf(ref m_SectionsBlob))); + m_SectionsBlob.Length = tempSections.Count; + + end += Math.NextMultipleOf(sizeof(Section) * tempSections.Count, 16); + + for (int sectionIndex = 0; sectionIndex < tempSections.Count; sectionIndex++) + { + MeshBuilder.TempSection tempSection = tempSections[sectionIndex]; + + section->PrimitiveFlagsBlob.Offset = (int)(end - (byte*)(UnsafeUtility.AddressOf(ref section->PrimitiveFlagsBlob))); + section->PrimitiveFlagsBlob.Length = tempSection.PrimitivesFlags.Count; + end += Math.NextMultipleOf(section->PrimitiveFlagsBlob.Length * sizeof(PrimitiveFlags), 4); + + section->PrimitiveVertexIndicesBlob.Offset = (int)(end - (byte*)(UnsafeUtility.AddressOf(ref section->PrimitiveVertexIndicesBlob))); + section->PrimitiveVertexIndicesBlob.Length = tempSection.Primitives.Count; + end += Math.NextMultipleOf(section->PrimitiveVertexIndicesBlob.Length * sizeof(PrimitiveVertexIndices), 4); + + section->VerticesBlob.Offset = (int)(end - (byte*)(UnsafeUtility.AddressOf(ref section->VerticesBlob))); + section->VerticesBlob.Length = tempSection.Vertices.Count; + end += Math.NextMultipleOf(section->VerticesBlob.Length * sizeof(float3), 4); + + for (int i = 0; i < tempSection.PrimitivesFlags.Count; i++) + { + Sections[sectionIndex].PrimitiveFlags[i] = tempSection.PrimitivesFlags[i]; + Sections[sectionIndex].PrimitiveVertexIndices[i] = tempSection.Primitives[i]; + } + + for (int i = 0; i < tempSection.Vertices.Count; i++) + { + Sections[sectionIndex].Vertices[i] = tempSection.Vertices[i]; + } + + // Filters + + section->PrimitiveFilterIndicesBlob.Offset = (int)(end - (byte*)(UnsafeUtility.AddressOf(ref section->PrimitiveFilterIndicesBlob))); + section->PrimitiveFilterIndicesBlob.Length = tempSection.Primitives.Count; + end += Math.NextMultipleOf(section->PrimitiveFilterIndicesBlob.Length * sizeof(short), 4); + + section->FiltersBlob.Offset = (int)(end - (byte*)(UnsafeUtility.AddressOf(ref section->FiltersBlob))); + section->FiltersBlob.Length = 1; + end += Math.NextMultipleOf(section->FiltersBlob.Length * sizeof(CollisionFilter), 4); + + Sections[sectionIndex].Filters[0] = filter; + for (int i = 0; i < tempSection.Primitives.Count; i++) + { + Sections[sectionIndex].PrimitiveFilterIndices[i] = 0; + } + + // Materials + + section->PrimitiveMaterialIndicesBlob.Offset = (int)(end - (byte*)(UnsafeUtility.AddressOf(ref section->PrimitiveMaterialIndicesBlob))); + section->PrimitiveMaterialIndicesBlob.Length = tempSection.Primitives.Count; + end += Math.NextMultipleOf(section->PrimitiveMaterialIndicesBlob.Length * sizeof(short), 4); + + section->MaterialsBlob.Offset = (int)(end - (byte*)(UnsafeUtility.AddressOf(ref section->MaterialsBlob))); + section->MaterialsBlob.Length = 1; + end += Math.NextMultipleOf(section->FiltersBlob.Length * sizeof(Material), 4); + + Sections[sectionIndex].Materials[0] = material; + for (int i = 0; i < tempSection.Primitives.Count; i++) + { + Sections[sectionIndex].PrimitiveMaterialIndices[i] = 0; + } + + section++; + } + } + + #endregion + } +} diff --git a/package/Unity.Physics/Collision/Geometry/Mesh.cs.meta b/package/Unity.Physics/Collision/Geometry/Mesh.cs.meta new file mode 100755 index 000000000..e0220ff28 --- /dev/null +++ b/package/Unity.Physics/Collision/Geometry/Mesh.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: d32c929c965478641b2f367080c60872 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Unity.Physics/Collision/Geometry/MeshBuilder.cs b/package/Unity.Physics/Collision/Geometry/MeshBuilder.cs new file mode 100755 index 000000000..6958f7a5a --- /dev/null +++ b/package/Unity.Physics/Collision/Geometry/MeshBuilder.cs @@ -0,0 +1,1034 @@ +using System; +using System.Collections.Generic; +using System.Linq; +using Unity.Mathematics; + +namespace Unity.Physics +{ + // Utilities for building physics meshes + internal static class MeshBuilder + { + internal class TempSection + { + public List PrimitivesFlags; + public List Primitives; + public List Vertices; + } + + internal static unsafe List BuildSections(BoundingVolumeHierarchy.Node* nodes, int nodeCount, List primitives) + { + var tempSections = new List(); + + // Traverse the tree and break out geometry into sections + int* nodesIndexStack = stackalloc int[BoundingVolumeHierarchy.Constants.UnaryStackSize]; + int stackSize = 1; + nodesIndexStack[0] = 1; + + const float uniqueVerticesPerPrimitiveFactor = 1.5f; + + int[] primitivesCountInSubTree = ProducedPrimitivesCountPerSubTree(nodes, nodeCount); + + do + { + int nodeIndex = nodesIndexStack[--stackSize]; + int subTreeVertexCountEstimate = (int)(uniqueVerticesPerPrimitiveFactor * primitivesCountInSubTree[nodeIndex]); + + var subTreeIndices = new List(); + + if (subTreeVertexCountEstimate < Mesh.Section.MaxNumVertices) + { + subTreeIndices.Add(nodeIndex); + } + else + { + // Sub tree is too big, break it up. + BoundingVolumeHierarchy.Node node = nodes[nodeIndex]; + + for (int i = 0; i < 4; i++) + { + if (node.IsChildValid(i)) + { + int childNodeIndex = node.Data[i]; + int nodeSubTreeVertexCount = (int)(uniqueVerticesPerPrimitiveFactor * primitivesCountInSubTree[childNodeIndex]); + + if (nodeSubTreeVertexCount < Mesh.Section.MaxNumVertices) + { + subTreeIndices.Add(childNodeIndex); + } + else + { + nodesIndexStack[stackSize++] = childNodeIndex; + } + } + } + } + + float tempUniqueVertexPrimitiveFactor = 1.0f; + const float factorStepIncrement = 0.25f; + + while (subTreeIndices.Any()) + { + // Try to combine sub trees if multiple sub trees can fit into one section. + var nodeIndices = new List(); + int vertexCountEstimate = 0; + + foreach (int subTreeNodeIndex in subTreeIndices) + { + int nodeIndexCount = (int)(tempUniqueVertexPrimitiveFactor * primitivesCountInSubTree[subTreeNodeIndex]); + if (vertexCountEstimate + nodeIndexCount < Mesh.Section.MaxNumVertices) + { + vertexCountEstimate += nodeIndexCount; + nodeIndices.Add(subTreeNodeIndex); + } + } + + if (!nodeIndices.Any()) + { + // We failed to fit any sub tree into sections. + // Split up nodes and push them to stack. + foreach (int subTreeNodeIndex in subTreeIndices) + { + BoundingVolumeHierarchy.Node nodeToSplit = nodes[subTreeNodeIndex]; + + for (int i = 0; i < 4; i++) + { + if (nodeToSplit.IsChildValid(i)) + { + nodesIndexStack[stackSize++] = nodeToSplit.Data[i]; + } + } + } + + subTreeIndices.Clear(); + continue; + } + + // Collect vertices from all sub trees. + var tmpVertices = new List(); + foreach (int subTreeNodeIndex in nodeIndices) + { + tmpVertices.AddRange(CollectAllVerticesFromSubTree(nodes, subTreeNodeIndex, primitives)); + } + + float3[] allVertices = tmpVertices.ToArray(); + + int[] vertexIndices = new int[allVertices.Length]; + for (int i = 0; i < vertexIndices.Length; i++) + { + vertexIndices[i] = i; + } + + MeshConnectivityBuilder.WeldVertices(vertexIndices, ref allVertices); + + if (allVertices.Length < Mesh.Section.MaxNumVertices) + { + tempSections.Add(BuildSectionGeometry(tempSections.Count, primitives, nodeIndices, nodes, allVertices)); + + // Remove used indices + foreach (int nodeTreeIndex in nodeIndices) + { + subTreeIndices.Remove(nodeTreeIndex); + } + } + else + { + // Estimate of num vertices per primitives was wrong. + // Increase the tempUniqueVertexPrimitiveFactor. + tempUniqueVertexPrimitiveFactor += factorStepIncrement; + } + } + } + while (stackSize > 0); + + return tempSections; + } + + private static unsafe int[] ProducedPrimitivesCountPerSubTree(BoundingVolumeHierarchy.Node* nodes, int nodeCount) + { + int[] primitivesPerNode = Enumerable.Repeat(0, nodeCount).ToArray(); + + for (int nodeIndex = nodeCount - 1; nodeIndex >= 0; nodeIndex--) + { + BoundingVolumeHierarchy.Node node = nodes[nodeIndex]; + + if (node.IsLeaf) + { + primitivesPerNode[nodeIndex] = node.NumValidChildren(); + } + else + { + primitivesPerNode[nodeIndex] = + primitivesPerNode[node.Data[0]] + primitivesPerNode[node.Data[1]] + + primitivesPerNode[node.Data[2]] + primitivesPerNode[node.Data[3]]; + } + } + + return primitivesPerNode; + } + + private static unsafe List CollectAllVerticesFromSubTree(BoundingVolumeHierarchy.Node* nodes, int subTreeNodeIndex, List primitives) + { + var vertices = new List(); + + int* nodesIndexStack = stackalloc int[BoundingVolumeHierarchy.Constants.UnaryStackSize]; + int stackSize = 1; + nodesIndexStack[0] = subTreeNodeIndex; + + do + { + BoundingVolumeHierarchy.Node node = nodes[nodesIndexStack[--stackSize]]; + + if (node.IsLeaf) + { + for (int i = 0; i < 4; i++) + { + if (node.IsChildValid(i)) + { + MeshConnectivityBuilder.Primitive p = primitives[node.Data[i]]; + vertices.Add(p.Vertices[0]); + vertices.Add(p.Vertices[1]); + vertices.Add(p.Vertices[2]); + + if (p.Flags.HasFlag(MeshConnectivityBuilder.PrimitiveFlags.DefaultTrianglePairFlags)) + { + vertices.Add(p.Vertices[3]); + } + } + } + } + else + { + for (int i = 0; i < 4; i++) + { + if (node.IsChildValid(i)) + { + nodesIndexStack[stackSize++] = node.Data[i]; + } + } + } + } while (stackSize > 0); + + return vertices; + } + + private static Mesh.PrimitiveFlags ConvertPrimitiveFlags(MeshConnectivityBuilder.PrimitiveFlags flags) + { + Mesh.PrimitiveFlags newFlags = 0; + newFlags |= flags.HasFlag(MeshConnectivityBuilder.PrimitiveFlags.IsTrianglePair) ? Mesh.PrimitiveFlags.IsTrianglePair : Mesh.PrimitiveFlags.IsTriangle; + + if (flags.HasFlag(MeshConnectivityBuilder.PrimitiveFlags.IsFlatConvexQuad)) + { + newFlags |= Mesh.PrimitiveFlags.IsQuad; + } + + return newFlags; + } + + private static unsafe TempSection BuildSectionGeometry( + int sectionIndex, List primitives, List subTreeNodeIndices, BoundingVolumeHierarchy.Node* nodes, float3[] vertices) + { + var section = new TempSection(); + section.Vertices = vertices.ToList(); + section.Primitives = new List(); + section.PrimitivesFlags = new List(); + + foreach (int root in subTreeNodeIndices) + { + int* nodesIndexStack = stackalloc int[BoundingVolumeHierarchy.Constants.UnaryStackSize]; + int stackSize = 1; + nodesIndexStack[0] = root; + + do + { + int nodeIndex = nodesIndexStack[--stackSize]; + ref BoundingVolumeHierarchy.Node node = ref nodes[nodeIndex]; + + if (node.IsLeaf) + { + for (int i = 0; i < 4; i++) + { + if (node.IsChildValid(i)) + { + MeshConnectivityBuilder.Primitive p = primitives[node.Data[i]]; + section.PrimitivesFlags.Add(ConvertPrimitiveFlags(p.Flags)); + + int vertexCount = p.Flags.HasFlag(MeshConnectivityBuilder.PrimitiveFlags.IsTrianglePair) ? 4 : 3; + + Mesh.PrimitiveVertexIndices sectionPrimitive = new Mesh.PrimitiveVertexIndices(); + byte* vertexIndices = §ionPrimitive.A; + + for (int v = 0; v < vertexCount; v++) + { + vertexIndices[v] = (byte)Array.IndexOf(vertices, p.Vertices[v]); + } + + if (vertexCount == 3) + { + sectionPrimitive.D = sectionPrimitive.C; + } + + section.Primitives.Add(sectionPrimitive); + + int primitiveSectionIndex = section.Primitives.Count - 1; + + // Update primitive index in the BVH. + node.Data[i] = (sectionIndex << 8) | primitiveSectionIndex; + } + } + } + else + { + for (int i = 0; i < 4; i++) + { + if (node.IsChildValid(i)) + { + nodesIndexStack[stackSize++] = node.Data[i]; + } + } + } + } while (stackSize > 0); + } + + return section; + } + } + + internal class MeshConnectivityBuilder + { + const float k_MergeCoplanarTrianglesTolerance = 1e-4f; + + internal Vertex[] Vertices; + internal Triangle[] Triangles; + internal Edge[] Edges; + + /// Vertex. + internal struct Vertex + { + /// Number of triangles referencing this vertex, or, equivalently, number of edge starting from this vertex. + internal int Cardinality; + + /// true if the vertex is on the boundary, false otherwise. + /// Note: if true the first edge of the ring is naked. + /// Conditions: number of naked edges in the 1-ring is greater than 0. + internal bool Boundary; + + /// true if the vertex is on the border, false otherwise. + /// Note: if true the first edge of the ring is naked. + /// Conditions: number of naked edges in the 1-ring is equal to 1. + internal bool Border; + + /// true is the vertex 1-ring is manifold. + /// Conditions: number of naked edges in the 1-ring is less than 2 and cardinality is greater than 0. + internal bool Manifold; + + /// Index of the first edge. + internal int FirstEdge; + } + + /// (Half) Edge. + internal struct Edge + { + internal static Edge Invalid() => new Edge { IsValid = false }; + + // Triangle index + internal int Triangle; + + // Starting vertex index + internal int Start; + + internal bool IsValid; + } + + internal class Triangle + { + public void Clear() + { + IsValid = false; + Links[0].IsValid = false; + Links[1].IsValid = false; + Links[2].IsValid = false; + } + + internal Edge[] Links = new Edge[3]; + internal bool IsValid; + } + + [Flags] + internal enum PrimitiveFlags + { + IsTrianglePair = 1 << 0, + IsFlat = 1 << 1, + IsConvex = 1 << 2, + + DisableInternalEdge = 1 << 3, + DisableAllEdges = 1 << 4, + + IsFlatConvexQuad = IsTrianglePair | IsFlat | IsConvex, + + DefaultTriangleFlags = IsFlat | IsConvex, + DefaultTrianglePairFlags = IsTrianglePair + } + + internal struct Primitive + { + internal float3x4 Vertices; + internal PrimitiveFlags Flags; + } + + /// Get the opposite edge. + internal Edge GetLink(Edge e) => e.IsValid ? Triangles[e.Triangle].Links[e.Start] : e; + internal bool IsBound(Edge e) => GetLink(e).IsValid; + internal bool IsNaked(Edge e) => !IsBound(e); + internal Edge GetNext(Edge e) => e.IsValid ? new Edge { Triangle = e.Triangle, Start = (e.Start + 1) % 3, IsValid = true } : e; + internal Edge GetPrev(Edge e) => e.IsValid ? new Edge { Triangle = e.Triangle, Start = (e.Start + 2) % 3, IsValid = true } : e; + + internal int GetStartVertexIndex(Edge e) => Triangles[e.Triangle].Links[e.Start].Start; + + internal int GetEndVertexIndex(Edge e) => Triangles[e.Triangle].Links[(e.Start + 1) % 3].Start; + + internal bool IsEdgeConcaveOrFlat(Edge edge, int[] indices, float3[] vertices, float4[] planes) + { + if (IsNaked(edge)) + { + return false; + } + + float3 apex = vertices[GetApexVertexIndex(indices, edge)]; + if (Math.Dotxyz1(planes[edge.Triangle], apex) < -k_MergeCoplanarTrianglesTolerance) + { + return false; + } + + return true; + } + + internal bool IsTriangleConcaveOrFlat(Edge edge, int[] indices, float3[] vertices, float4[] planes) + { + for (int i = 0; i < 3; i++) + { + Edge e = GetNext(edge); + if (!IsEdgeConcaveOrFlat(e, indices, vertices, planes)) + { + return false; + } + } + + return true; + } + + internal bool IsFlat(Edge edge, int[] indices, float3[] vertices, float4[] planes) + { + Edge link = GetLink(edge); + if (!link.IsValid) + { + return false; + } + + float3 apex = vertices[GetApexVertexIndex(indices, link)]; + bool flat = math.abs(Math.Dotxyz1(planes[edge.Triangle], apex)) < k_MergeCoplanarTrianglesTolerance; + + apex = vertices[GetApexVertexIndex(indices, edge)]; + flat |= math.abs(Math.Dotxyz1(planes[link.Triangle], apex)) < k_MergeCoplanarTrianglesTolerance; + + return flat; + } + + internal bool IsConvexQuad(Primitive quad, Edge edge, float4[] planes) + { + float4x2 quadPlanes; + quadPlanes.c0 = planes[edge.Triangle]; + quadPlanes.c1 = planes[GetLink(edge).Triangle]; + if (Math.Dotxyz1(quadPlanes[0], quad.Vertices[3]) < k_MergeCoplanarTrianglesTolerance) + { + if (Math.Dotxyz1(quadPlanes[1], quad.Vertices[1]) < k_MergeCoplanarTrianglesTolerance) + { + bool convex = true; + for (int i = 0; convex && i < 4; i++) + { + float3 delta = quad.Vertices[(i + 1) % 4] - quad.Vertices[i]; + float3 normal = math.normalize(math.cross(delta, quadPlanes[i >> 1].xyz)); + float4 edgePlane = new float4(normal, math.dot(-normal, quad.Vertices[i])); + for (int j = 0; j < 2; j++) + { + if (Math.Dotxyz1(edgePlane, quad.Vertices[(i + j + 1) % 4]) > k_MergeCoplanarTrianglesTolerance) + { + convex = false; + break; + } + } + } + return convex; + } + } + + return false; + } + + internal bool CanEdgeBeDisabled(Edge e, PrimitiveFlags[] flags, int[] indices, float3[] vertices, float4[] planes) + { + if (!e.IsValid || IsEdgeConcaveOrFlat(e, indices, vertices, planes) || flags[e.Triangle].HasFlag(PrimitiveFlags.DisableAllEdges)) + { + return false; + } + + return true; + } + + internal bool CanAllEdgesBeDisabled(Edge[] edges, PrimitiveFlags[] flags, int[] indices, float3[] vertices, float4[] planes) + { + bool allDisabled = true; + foreach (Edge e in edges) + { + allDisabled &= CanEdgeBeDisabled(e, flags, indices, vertices, planes); + } + + return allDisabled; + } + + // Utility function + private static void Swap(ref T a, ref T b) where T : struct + { + T t = a; + a = b; + b = t; + } + + private struct VertexWithHash + { + internal float3 Vertex; + internal ulong Hash; + internal int Index; + } + + private static ulong SpatialHash(float3 vertex) + { + ulong x, y, z; + unsafe + { + float* tmp = &vertex.x; + x = *((ulong*)tmp); + + tmp = &vertex.y; + y = *((ulong*)tmp); + + tmp = &vertex.z; + z = *((ulong*)tmp); + } + + const ulong p1 = 73856093; + const ulong p2 = 19349663; + const ulong p3 = 83492791; + + return (x * p1) ^ (y * p2) ^ (z * p3); + } + + public static void WeldVertices(int[] indices, ref float3[] vertices) + { + int numVertices = vertices.Length; + var verticesAndHashes = new VertexWithHash[numVertices]; + for (int i = 0; i < numVertices; i++) + { + verticesAndHashes[i].Index = i; + verticesAndHashes[i].Vertex = vertices[i]; + verticesAndHashes[i].Hash = SpatialHash(verticesAndHashes[i].Vertex); + } + + var uniqueVertices = new List(); + var remap = new int[numVertices]; + verticesAndHashes = verticesAndHashes.OrderBy(v => v.Hash).ToArray(); + + for (int i = 0; i < numVertices; i++) + { + if (verticesAndHashes[i].Index == int.MaxValue) + { + continue; + } + + uniqueVertices.Add(vertices[verticesAndHashes[i].Index]); + remap[verticesAndHashes[i].Index] = uniqueVertices.Count - 1; + + for (int j = i + 1; j < numVertices; j++) + { + if (verticesAndHashes[j].Index == int.MaxValue) + { + continue; + } + + if (verticesAndHashes[i].Hash == verticesAndHashes[j].Hash) + { + if (verticesAndHashes[i].Vertex.x == verticesAndHashes[j].Vertex.x && + verticesAndHashes[i].Vertex.y == verticesAndHashes[j].Vertex.y && + verticesAndHashes[i].Vertex.z == verticesAndHashes[j].Vertex.z) + { + remap[verticesAndHashes[j].Index] = remap[verticesAndHashes[i].Index]; + verticesAndHashes[j].Index = int.MaxValue; + } + } + } + } + + vertices = uniqueVertices.ToArray(); + + for (int i = 0; i < indices.Length; i++) + { + indices[i] = remap[indices[i]]; + } + } + + public static bool IsTriangleDegenerate(float3 a, float3 b, float3 c) + { + const float defaultTriangleDegeneracyTolerance = 1e-7f; + + // Small area check + { + float3 edge1 = a - b; + float3 edge2 = a - c; + float3 cross = math.cross(edge1, edge2); + + float3 edge1B = b - a; + float3 edge2B = b - c; + float3 crossB = math.cross(edge1B, edge2B); + + bool cmp0 = defaultTriangleDegeneracyTolerance > math.lengthsq(cross); + bool cmp1 = defaultTriangleDegeneracyTolerance > math.lengthsq(crossB); + if (cmp0 || cmp1) + { + return true; + } + } + + // Point triangle distance check + { + float3 q = a - b; + float3 r = c - b; + + float qq = math.dot(q, q); + float rr = math.dot(r, r); + float qr = math.dot(q, r); + + float qqrr = qq * rr; + float qrqr = qr * qr; + float det = (qqrr - qrqr); + + return det == 0.0f; + } + } + + internal MeshConnectivityBuilder(int[] indices, float3[] vertices) + { + int numIndices = indices.Length; + int numTriangles = numIndices / 3; + int numVertices = vertices.Length; + + Vertices = new Vertex[numVertices]; + Triangles = new Triangle[numTriangles]; + for (int i = 0; i < numTriangles; i++) + { + Triangles[i] = new Triangle(); + Triangles[i].Clear(); + } + + int numEdges = 0; + + // Compute cardinality and triangle flags. + for (int triangleIndex = 0; triangleIndex < numTriangles; triangleIndex++) + { + var triangleIndices = new ArraySegment(indices, triangleIndex * 3, 3); + Triangles[triangleIndex].IsValid = + triangleIndices.Array[triangleIndices.Offset + 0] != triangleIndices.Array[triangleIndices.Offset + 1] && + triangleIndices.Array[triangleIndices.Offset + 1] != triangleIndices.Array[triangleIndices.Offset + 2] && + triangleIndices.Array[triangleIndices.Offset + 0] != triangleIndices.Array[triangleIndices.Offset + 2]; + + if (Triangles[triangleIndex].IsValid) + { + Vertices[triangleIndices.Array[triangleIndices.Offset + 0]].Cardinality++; + Vertices[triangleIndices.Array[triangleIndices.Offset + 1]].Cardinality++; + Vertices[triangleIndices.Array[triangleIndices.Offset + 2]].Cardinality++; + } + } + + // Compute vertex first edge index. + for (int vertexIndex = 0; vertexIndex < numVertices; ++vertexIndex) + { + int cardinality = Vertices[vertexIndex].Cardinality; + Vertices[vertexIndex].FirstEdge = cardinality > 0 ? numEdges : 0; + numEdges += cardinality; + } + + // Compute edges and triangles links. + int[] counters = Enumerable.Repeat(0, numVertices).ToArray(); + Edges = new Edge[numEdges]; + + for (int triangleIndex = 0; triangleIndex < numTriangles; triangleIndex++) + { + if (!Triangles[triangleIndex].IsValid) + { + continue; + } + + int indexInIndices = triangleIndex * 3; + for (int i = 2, j = 0; j < 3; i = j++) + { + int vertexI = indices[indexInIndices + i]; + int thisEdgeIndex = Vertices[vertexI].FirstEdge + counters[vertexI]++; + Edges[thisEdgeIndex] = new Edge { Triangle = triangleIndex, Start = i, IsValid = true }; + + int vertexJ = indices[indexInIndices + j]; + Vertex other = Vertices[vertexJ]; + int count = counters[vertexJ]; + + for (int k = 0; k < count; k++) + { + Edge edge = Edges[other.FirstEdge + k]; + + int endVertexOffset = (edge.Start + 1) % 3; + int endVertex = indices[edge.Triangle * 3 + endVertexOffset]; + if (endVertex == vertexI) + { + Triangles[triangleIndex].Links[i] = edge; + Triangles[edge.Triangle].Links[edge.Start] = Edges[thisEdgeIndex]; + break; + } + } + } + } + + // Compute vertices attributes. + for (int vertexIndex = 0; vertexIndex < numVertices; vertexIndex++) + { + int nakedEdgeIndex = -1; + int numNakedEdge = 0; + { + int firstEdgeIndex = Vertices[vertexIndex].FirstEdge; + int numVertexEdges = Vertices[vertexIndex].Cardinality; + for (int i = 0; i < numVertexEdges; i++) + { + int edgeIndex = firstEdgeIndex + i; + if (IsNaked(Edges[edgeIndex])) + { + nakedEdgeIndex = i; + numNakedEdge++; + } + } + } + + ref Vertex vertex = ref Vertices[vertexIndex]; + vertex.Manifold = numNakedEdge < 2 && vertex.Cardinality > 0; + vertex.Boundary = numNakedEdge > 0; + vertex.Border = numNakedEdge == 1 && vertex.Manifold; + + // Make sure that naked edge appears first. + if (nakedEdgeIndex > 0) + { + Swap(ref Edges[vertex.FirstEdge], ref Edges[vertex.FirstEdge + nakedEdgeIndex]); + } + + // Order ring as fan. + if (vertex.Manifold) + { + int firstEdge = vertex.FirstEdge; + int count = vertex.Cardinality; + for (int i = 0; i < count - 1; i++) + { + Edge prevEdge = GetPrev(Edges[firstEdge + i]); + if (IsBound(prevEdge)) + { + int triangle = GetLink(prevEdge).Triangle; + if (Edges[firstEdge + i + 1].Triangle != triangle) + { + bool found = false; + for (int j = i + 2; j < count; ++j) + { + if (Edges[firstEdge + j].Triangle == triangle) + { + Swap(ref Edges[firstEdge + i + 1], ref Edges[firstEdge + j]); + found = true; + break; + } + } + + if (!found) + { + vertex.Manifold = false; + vertex.Border = false; + break; + } + } + } + } + + if (vertex.Manifold) + { + Edge lastEdge = GetPrev(Edges[firstEdge + count - 1]); + if (vertex.Border) + { + if (IsBound(lastEdge)) + { + vertex.Manifold = false; + vertex.Border = false; + } + } + else + { + if (IsNaked(lastEdge) || GetLink(lastEdge).Triangle != Edges[firstEdge].Triangle) + { + vertex.Manifold = false; + } + } + } + } + } + } + + private struct EdgeData + { + internal Edge Edge; + internal float Value; + } + + private static int4 GetVertexIndices(int[] indices, Edge edge) + { + int4 vertexIndices; + int triangleIndicesIndex = edge.Triangle * 3; + vertexIndices.x = indices[triangleIndicesIndex + edge.Start]; + vertexIndices.y = indices[triangleIndicesIndex + ((edge.Start + 1) % 3)]; + vertexIndices.z = indices[triangleIndicesIndex + ((edge.Start + 2) % 3)]; + vertexIndices.w = 0; + return vertexIndices; + } + + private static int GetApexVertexIndex(int[] indices, Edge edge) + { + int triangleIndicesIndex = edge.Triangle * 3; + return indices[triangleIndicesIndex + ((edge.Start + 2) % 3)]; + } + + private static float CalcTwiceSurfaceArea(float3 a, float3 b, float3 c) + { + float3 d0 = b - a; + float3 d1 = c - a; + return math.length(math.cross(d0, d1)); + } + + internal List EnumerateQuadDominantGeometry(int[] indices, float3[] vertices) + { + int numTriangles = indices.Length / 3; + PrimitiveFlags[] flags = new PrimitiveFlags[numTriangles]; + var quadRoots = new List(); + var triangleRoots = new List(); + + // Generate triangle planes + var planes = new float4[indices.Length / 3]; + for (int i = 0; i < numTriangles; i++) + { + int triangleIndex = i * 3; + float3 v0 = vertices[indices[triangleIndex]]; + float3 v1 = vertices[indices[triangleIndex + 1]]; + float3 v2 = vertices[indices[triangleIndex + 2]]; + + float3 normal = math.normalize(math.cross(v0 - v1, v0 - v2)); + planes[i] = new float4(normal, -math.dot(normal, v0)); + } + + var edges = new EdgeData[Edges.Length]; + + for (int i = 0; i < edges.Length; i++) + { + Edge e = Edges[i]; + + ref EdgeData edgeData = ref edges[i]; + edgeData.Edge = Edge.Invalid(); + edgeData.Value = float.MaxValue; + + if (IsBound(e)) + { + Edge linkEdge = GetLink(e); + int4 vis = GetVertexIndices(indices, e); + vis[3] = GetApexVertexIndex(indices, linkEdge); + + float3x4 quadVertices = new float3x4(vertices[vis[0]], vertices[vis[1]], vertices[vis[2]], vertices[vis[3]]); + Aabb quadAabb = Aabb.CreateFromPoints(quadVertices); + + float aabbSurfaceArea = quadAabb.SurfaceArea; + + if (aabbSurfaceArea > Math.Constants.Eps) + { + float quadSurfaceArea = CalcTwiceSurfaceArea(quadVertices[0], quadVertices[1], quadVertices[2]) + CalcTwiceSurfaceArea(quadVertices[0], quadVertices[1], quadVertices[3]); + edgeData.Value = (aabbSurfaceArea - quadSurfaceArea) / aabbSurfaceArea; + edgeData.Edge = vis[0] < vis[1] ? e : linkEdge; + } + } + } + + edges = edges.OrderBy(e => e.Value).ToArray(); + + bool[] freeTriangles = Enumerable.Repeat(true, numTriangles).ToArray(); + + var primitives = new List(); + + // Generate quads + foreach (EdgeData edgeData in edges) + { + if (!edgeData.Edge.IsValid) + { + break; + } + + int t0 = edgeData.Edge.Triangle; + Edge linkEdge = GetLink(edgeData.Edge); + int t1 = linkEdge.Triangle; + + if (freeTriangles[t0] && freeTriangles[t1]) + { + Edge nextEdge = GetNext(edgeData.Edge); + int4 vis = GetVertexIndices(indices, nextEdge); + vis[3] = GetApexVertexIndex(indices, linkEdge); + + var primitive = new Primitive + { + Vertices = new float3x4(vertices[vis[0]], vertices[vis[1]], vertices[vis[2]], vertices[vis[3]]), + Flags = PrimitiveFlags.DefaultTrianglePairFlags + }; + + if (IsTriangleDegenerate(primitive.Vertices[0], primitive.Vertices[1], primitive.Vertices[2]) || + IsTriangleDegenerate(primitive.Vertices[0], primitive.Vertices[2], primitive.Vertices[3])) + { + continue; + } + + if (IsEdgeConcaveOrFlat(edgeData.Edge, indices, vertices, planes)) + { + primitive.Flags |= PrimitiveFlags.DisableInternalEdge; + + if (IsTriangleConcaveOrFlat(edgeData.Edge, indices, vertices, planes) && + IsTriangleConcaveOrFlat(linkEdge, indices, vertices, planes)) + { + primitive.Flags |= PrimitiveFlags.DisableAllEdges; + } + } + + if (IsFlat(edgeData.Edge, indices, vertices, planes)) + { + primitive.Flags |= PrimitiveFlags.IsFlat; + } + + if (IsConvexQuad(primitive, edgeData.Edge, planes)) + { + primitive.Flags |= PrimitiveFlags.IsConvex; + } + + primitives.Add(primitive); + + freeTriangles[t0] = false; + freeTriangles[t1] = false; + + flags[t0] = primitive.Flags; + flags[t1] = primitive.Flags; + + quadRoots.Add(edgeData.Edge); + } + } + + // Generate triangles + for (int i = 0; i < numTriangles; i++) + { + if (!freeTriangles[i]) + { + continue; + } + + Edge edge = new Edge { Triangle = i, Start = 0, IsValid = true }; + while (edge.IsValid && GetStartVertexIndex(edge) < GetEndVertexIndex(edge)) + { + edge = GetNext(edge); + } + + int4 vis = GetVertexIndices(indices, edge); + + var primitive = new Primitive + { + Vertices = new float3x4(vertices[vis[0]], vertices[vis[1]], vertices[vis[2]], vertices[vis[2]]), + Flags = PrimitiveFlags.DefaultTriangleFlags + }; + + if (IsTriangleConcaveOrFlat(edge, indices, vertices, planes)) + { + primitive.Flags |= PrimitiveFlags.DisableAllEdges; + } + + primitives.Add(primitive); + triangleRoots.Add(edge); + flags[edge.Triangle] = primitives.Last().Flags; + } + + DisableEdgesOfAdjacentPrimitives(primitives, indices, vertices, planes, flags, quadRoots, triangleRoots); + + return primitives; + } + + private void DisableEdgesOfAdjacentPrimitives( + List primitives, int[] indices, float3[] vertices, float4[] planes, PrimitiveFlags[] flags, + List quadRoots, List triangleRoots) + { + for (int quadIndex = 0; quadIndex < quadRoots.Count; quadIndex++) + { + Edge root = quadRoots[quadIndex]; + Edge link = GetLink(root); + PrimitiveFlags quadFlags = flags[root.Triangle]; + if (quadFlags.HasFlag(PrimitiveFlags.IsFlatConvexQuad) && !quadFlags.HasFlag(PrimitiveFlags.DisableAllEdges)) + { + Edge[] outerBoundary = new Edge[4]; + outerBoundary[0] = GetLink(GetNext(root)); + outerBoundary[1] = GetLink(GetPrev(root)); + outerBoundary[2] = GetLink(GetNext(link)); + outerBoundary[3] = GetLink(GetPrev(link)); + + if (CanAllEdgesBeDisabled(outerBoundary, flags, indices, vertices, planes)) + { + quadFlags |= PrimitiveFlags.DisableAllEdges; + } + } + + // Sync triangle flags. + flags[root.Triangle] = quadFlags; + flags[link.Triangle] = quadFlags; + + // Write primitive flags. + primitives[quadIndex] = new Primitive + { + Vertices = primitives[quadIndex].Vertices, + Flags = quadFlags + }; + } + + for (int triangleIndex = 0; triangleIndex < triangleRoots.Count; triangleIndex++) + { + Edge root = triangleRoots[triangleIndex]; + PrimitiveFlags triangleFlags = flags[root.Triangle]; + if (!triangleFlags.HasFlag(PrimitiveFlags.DisableAllEdges)) + { + Edge[] outerBoundary = new Edge[3]; + outerBoundary[0] = GetLink(root); + outerBoundary[1] = GetLink(GetNext(root)); + outerBoundary[2] = GetLink(GetPrev(root)); + + if (CanAllEdgesBeDisabled(outerBoundary, flags, indices, vertices, planes)) + { + triangleFlags |= PrimitiveFlags.DisableAllEdges; + } + } + + // Sync triangle flags. + flags[root.Triangle] = triangleFlags; + + // Write primitive flags. + int primitiveIndex = quadRoots.Count + triangleIndex; + primitives[primitiveIndex] = new Primitive + { + Vertices = primitives[primitiveIndex].Vertices, + Flags = triangleFlags + }; + } + } + } +} diff --git a/package/Unity.Physics/Collision/Geometry/MeshBuilder.cs.meta b/package/Unity.Physics/Collision/Geometry/MeshBuilder.cs.meta new file mode 100755 index 000000000..88da63524 --- /dev/null +++ b/package/Unity.Physics/Collision/Geometry/MeshBuilder.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 8ae9aaedb8c908048b860130036ccb16 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Unity.Physics/Collision/Queries.meta b/package/Unity.Physics/Collision/Queries.meta new file mode 100755 index 000000000..920a3ce2d --- /dev/null +++ b/package/Unity.Physics/Collision/Queries.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: b3ed586dd01c9f044b17d18c46942354 +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Unity.Physics/Collision/Queries/Collector.cs b/package/Unity.Physics/Collision/Queries/Collector.cs new file mode 100755 index 000000000..ff90be369 --- /dev/null +++ b/package/Unity.Physics/Collision/Queries/Collector.cs @@ -0,0 +1,156 @@ +using Unity.Collections; +using UnityEngine.Assertions; +using static Unity.Physics.Math; + +namespace Unity.Physics +{ + public interface IQueryResult + { + float Fraction { get; } + + void Transform(MTransform transform, uint parentSubKey, uint parentSubKeyBits); + void Transform(MTransform transform, int rigidBodyIndex); + } + + // Interface for collecting hits during a collision query + public interface ICollector where T : struct, IQueryResult + { + // Whether to exit the query as soon as any hit has been accepted + bool EarlyOutOnFirstHit { get; } + + // The maximum fraction of the query within which to check for hits + // For casts, this is a fraction along the ray + // For distance queries, this is a distance from the query object + float MaxFraction { get; } + + // The number of hits that have been collected + int NumHits { get; } + + // Called when the query hits something + // Return true to accept the hit, or false to ignore it + bool AddHit(T hit); + + void TransformNewHits(int oldNumHits, float oldFraction, MTransform transform, uint numSubKeyBits, uint subKey); + void TransformNewHits(int oldNumHits, float oldFraction, MTransform transform, int rigidBodyIndex); + } + + // A collector which exits the query as soon as any hit is detected. + public struct AnyHitCollector : ICollector where T : struct, IQueryResult + { + public bool EarlyOutOnFirstHit => true; + public float MaxFraction { get; } + public int NumHits => 0; + + public AnyHitCollector(float maxFraction) + { + MaxFraction = maxFraction; + } + + #region ICollector + + public bool AddHit(T hit) + { + Assert.IsTrue(hit.Fraction < MaxFraction); + return true; + } + + public void TransformNewHits(int oldNumHits, float oldFraction, MTransform transform, uint numSubKeyBits, uint subKey) { } + public void TransformNewHits(int oldNumHits, float oldFraction, MTransform transform, int rigidBodyIndex) { } + + #endregion + } + + // A collector which stores only the closest hit. + public struct ClosestHitCollector : ICollector where T : struct, IQueryResult + { + public bool EarlyOutOnFirstHit => false; + public float MaxFraction { get; private set; } + public int NumHits { get; private set; } + + private T m_ClosestHit; + public T ClosestHit => m_ClosestHit; + + public ClosestHitCollector(float maxFraction) + { + MaxFraction = maxFraction; + m_ClosestHit = default(T); + NumHits = 0; + } + + #region ICollector + + public bool AddHit(T hit) + { + Assert.IsTrue(hit.Fraction <= MaxFraction); + MaxFraction = hit.Fraction; + m_ClosestHit = hit; + NumHits = 1; + return true; + } + + public void TransformNewHits(int oldNumHits, float oldFraction, MTransform transform, uint numSubKeyBits, uint subKey) + { + if (m_ClosestHit.Fraction < oldFraction) + { + m_ClosestHit.Transform(transform, numSubKeyBits, subKey); + } + } + + public void TransformNewHits(int oldNumHits, float oldFraction, MTransform transform, int rigidBodyIndex) + { + if (m_ClosestHit.Fraction < oldFraction) + { + m_ClosestHit.Transform(transform, rigidBodyIndex); + } + } + + #endregion + } + + // A collector which stores every hit. + public struct AllHitsCollector : ICollector where T : struct, IQueryResult + { + public bool EarlyOutOnFirstHit => false; + public float MaxFraction { get; } + public int NumHits => AllHits.Length; + + public NativeList AllHits; + + public AllHitsCollector(float maxFraction, ref NativeList allHits) + { + MaxFraction = maxFraction; + AllHits = allHits; + } + + #region + + public bool AddHit(T hit) + { + Assert.IsTrue(hit.Fraction < MaxFraction); + AllHits.Add(hit); + return true; + } + + public void TransformNewHits(int oldNumHits, float oldFraction, MTransform transform, uint numSubKeyBits, uint subKey) + { + for (int i = oldNumHits; i < NumHits; i++) + { + T hit = AllHits[i]; + hit.Transform(transform, numSubKeyBits, subKey); + AllHits[i] = hit; + } + } + + public void TransformNewHits(int oldNumHits, float oldFraction, MTransform transform, int rigidBodyIndex) + { + for (int i = oldNumHits; i < NumHits; i++) + { + T hit = AllHits[i]; + hit.Transform(transform, rigidBodyIndex); + AllHits[i] = hit; + } + } + + #endregion + } +} diff --git a/package/Unity.Physics/Collision/Queries/Collector.cs.meta b/package/Unity.Physics/Collision/Queries/Collector.cs.meta new file mode 100755 index 000000000..061f11443 --- /dev/null +++ b/package/Unity.Physics/Collision/Queries/Collector.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 4bcb5862f42a8d744be66aced5cd05f4 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Unity.Physics/Collision/Queries/Collidable.cs b/package/Unity.Physics/Collision/Queries/Collidable.cs new file mode 100755 index 000000000..0bedbb9f7 --- /dev/null +++ b/package/Unity.Physics/Collision/Queries/Collidable.cs @@ -0,0 +1,210 @@ +using Unity.Collections; +using Unity.Mathematics; + +namespace Unity.Physics +{ + // Interface for objects that can be hit by physics queries. + public interface ICollidable // TODO: rename to Physics.IQueryable? + { + // Bounding box + + // Calculate an axis aligned bounding box around the object, in local space. + Aabb CalculateAabb(); + + // Calculate an axis aligned bounding box around the object, in the given space. + Aabb CalculateAabb(RigidTransform transform); + + // Cast ray + + // Cast a ray against the object. + // Return true if it hits. + bool CastRay(RaycastInput input); + + // Cast a ray against the object. + // Return true if it hits, with details of the closest hit in "closestHit". + bool CastRay(RaycastInput input, out RaycastHit closestHit); + + // Cast a ray against the object. + // Return true if it hits, with details of every hit in "allHits". + bool CastRay(RaycastInput input, ref NativeList allHits); + + // Generic ray cast. + // Return true if it hits, with details stored in the collector implementation. + bool CastRay(RaycastInput input, ref T collector) where T : struct, ICollector; + + // Cast collider + + // Cast a collider against the object. + // Return true if it hits. + bool CastCollider(ColliderCastInput input); + + // Cast a collider against the object. + // Return true if it hits, with details of the closest hit in "closestHit". + bool CastCollider(ColliderCastInput input, out ColliderCastHit closestHit); + + // Cast a collider against the object. + // Return true if it hits, with details of every hit in "allHits". + bool CastCollider(ColliderCastInput input, ref NativeList allHits); + + // Generic collider cast. + // Return true if it hits, with details stored in the collector implementation. + bool CastCollider(ColliderCastInput input, ref T collector) where T : struct, ICollector; + + // Point distance query + + // Calculate the distance from a point to the object. + // Return true if there are any hits. + bool CalculateDistance(PointDistanceInput input); + + // Calculate the distance from a point to the object. + // Return true if there are any hits, with details of the closest hit in "closestHit". + bool CalculateDistance(PointDistanceInput input, out DistanceHit closestHit); + + // Calculate the distance from a point to the object. + // Return true if there are any hits, with details of every hit in "allHits". + bool CalculateDistance(PointDistanceInput input, ref NativeList allHits); + + // Calculate the distance from a point to the object. + // Return true if there are any hits, with details stored in the collector implementation. + bool CalculateDistance(PointDistanceInput input, ref T collector) where T : struct, ICollector; + + // Collider distance query + + // Calculate the distance from a collider to the object. + // Return true if there are any hits. + bool CalculateDistance(ColliderDistanceInput input); + + // Calculate the distance from a collider to the object. + // Return true if there are any hits, with details of the closest hit in "closestHit". + bool CalculateDistance(ColliderDistanceInput input, out DistanceHit closestHit); + + // Calculate the distance from a collider to the object. + // Return true if there are any hits, with details of every hit in "allHits". + bool CalculateDistance(ColliderDistanceInput input, ref NativeList allHits); + + // Calculate the distance from a collider to the object. + // Return true if there are any hits, with details stored in the collector implementation. + bool CalculateDistance(ColliderDistanceInput input, ref T collector) where T : struct, ICollector; + } + + + // Wrappers around generic ICollidable queries + public static class QueryWrappers + { + #region Ray casts + + public static bool RayCast(ref T target, RaycastInput input) where T : struct, ICollidable + { + var collector = new AnyHitCollector(1.0f); + return target.CastRay(input, ref collector); + } + + public static bool RayCast(ref T target, RaycastInput input, out RaycastHit closestHit) where T : struct, ICollidable + { + var collector = new ClosestHitCollector(1.0f); + if (target.CastRay(input, ref collector)) + { + closestHit = collector.ClosestHit; // TODO: would be nice to avoid this copy + return true; + } + + closestHit = new RaycastHit(); + return false; + } + + public static bool RayCast(ref T target, RaycastInput input, ref NativeList allHits) where T : struct, ICollidable + { + var collector = new AllHitsCollector(1.0f, ref allHits); + return target.CastRay(input, ref collector); + } + + #endregion + + #region Collider casts + + public static bool ColliderCast(ref T target, ColliderCastInput input) where T : struct, ICollidable + { + var collector = new AnyHitCollector(1.0f); + return target.CastCollider(input, ref collector); + } + + public static bool ColliderCast(ref T target, ColliderCastInput input, out ColliderCastHit result) where T : struct, ICollidable + { + var collector = new ClosestHitCollector(1.0f); + if (target.CastCollider(input, ref collector)) + { + result = collector.ClosestHit; // TODO: would be nice to avoid this copy + return true; + } + + result = new ColliderCastHit(); + return false; + } + + public static bool ColliderCast(ref T target, ColliderCastInput input, ref NativeList allHits) where T : struct, ICollidable + { + var collector = new AllHitsCollector(1.0f, ref allHits); + return target.CastCollider(input, ref collector); + } + + #endregion + + #region Point distance queries + + public static bool CalculateDistance(ref T target, PointDistanceInput input) where T : struct, ICollidable + { + var collector = new AnyHitCollector(input.MaxDistance); + return target.CalculateDistance(input, ref collector); + } + + public static bool CalculateDistance(ref T target, PointDistanceInput input, out DistanceHit result) where T : struct, ICollidable + { + var collector = new ClosestHitCollector(input.MaxDistance); + if (target.CalculateDistance(input, ref collector)) + { + result = collector.ClosestHit; // TODO: would be nice to avoid this copy + return true; + } + + result = new DistanceHit(); + return false; + } + + public static bool CalculateDistance(ref T target, PointDistanceInput input, ref NativeList allHits) where T : struct, ICollidable + { + var collector = new AllHitsCollector(input.MaxDistance, ref allHits); + return target.CalculateDistance(input, ref collector); + } + + #endregion + + #region Collider distance queries + + public static bool CalculateDistance(ref T target, ColliderDistanceInput input) where T : struct, ICollidable + { + var collector = new AnyHitCollector(input.MaxDistance); + return target.CalculateDistance(input, ref collector); + } + + public static bool CalculateDistance(ref T target, ColliderDistanceInput input, out DistanceHit result) where T : struct, ICollidable + { + var collector = new ClosestHitCollector(input.MaxDistance); + if (target.CalculateDistance(input, ref collector)) + { + result = collector.ClosestHit; // TODO: would be nice to avoid this copy + return true; + } + + result = new DistanceHit(); + return false; + } + + public static bool CalculateDistance(ref T target, ColliderDistanceInput input, ref NativeList allHits) where T : struct, ICollidable + { + var collector = new AllHitsCollector(input.MaxDistance, ref allHits); + return target.CalculateDistance(input, ref collector); + } + + #endregion + } +} diff --git a/package/Unity.Physics/Collision/Queries/Collidable.cs.meta b/package/Unity.Physics/Collision/Queries/Collidable.cs.meta new file mode 100755 index 000000000..0fbe0b51e --- /dev/null +++ b/package/Unity.Physics/Collision/Queries/Collidable.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: b3620834b66d9f046925e0d6a275b8c2 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Unity.Physics/Collision/Queries/ColliderCast.cs b/package/Unity.Physics/Collision/Queries/ColliderCast.cs new file mode 100755 index 000000000..7f16c97fa --- /dev/null +++ b/package/Unity.Physics/Collision/Queries/ColliderCast.cs @@ -0,0 +1,253 @@ +using System; +using Unity.Mathematics; +using static Unity.Physics.BoundingVolumeHierarchy; +using static Unity.Physics.Math; + +namespace Unity.Physics +{ + // The input to collider cast queries + public unsafe struct ColliderCastInput + { + public Collider* Collider; + public float3 Position + { + get => m_Ray.Origin; + set => m_Ray.Origin = value; + } + public quaternion Orientation { get; set; } + public float3 Direction + { + get => m_Ray.Direction; + set => m_Ray.Direction = value; + } + + private Ray m_Ray; + public Ray Ray => m_Ray; + } + + // A hit from a collider cast query + public struct ColliderCastHit : IQueryResult + { + public float Fraction { get; set; } + + public float3 Position; + public float3 SurfaceNormal; + public int RigidBodyIndex; + public ColliderKey ColliderKey; + + public void Transform(MTransform transform, uint numSubKeyBits, uint subKey) + { + Position = Mul(transform, Position); + SurfaceNormal = math.mul(transform.Rotation, SurfaceNormal); + ColliderKey.PushSubKey(numSubKeyBits, subKey); + } + + public void Transform(MTransform transform, int rigidBodyIndex) + { + Position = Mul(transform, Position); + SurfaceNormal = math.mul(transform.Rotation, SurfaceNormal); + RigidBodyIndex = rigidBodyIndex; + } + } + + // Collider cast query implementations + public static class ColliderCastQueries + { + public static unsafe bool ColliderCollider(ColliderCastInput input, Collider* target, ref T collector) where T : struct, ICollector + { + if (!CollisionFilter.IsCollisionEnabled(input.Collider->Filter, target->Filter)) + { + return false; + } + + switch (input.Collider->CollisionType) + { + case CollisionType.Convex: + switch (target->Type) + { + case ColliderType.Sphere: + case ColliderType.Capsule: + case ColliderType.Triangle: + case ColliderType.Quad: + case ColliderType.Box: + case ColliderType.Convex: + return ConvexConvex(input, target, ref collector); + case ColliderType.Mesh: + return ConvexMesh(input, (MeshCollider*)target, ref collector); + case ColliderType.Compound: + return ConvexCompound(input, (CompoundCollider*)target, ref collector); + default: + throw new NotImplementedException(); + } + case CollisionType.Composite: + // no support for casting composite shapes + throw new NotImplementedException(); + default: + throw new NotImplementedException(); + } + } + + private static unsafe bool ConvexConvex(ColliderCastInput input, Collider* target, ref T collector) where T : struct, ICollector + { + //Assert.IsTrue(target->CollisionType == CollisionType.Convex && input.Collider->CollisionType == CollisionType.Convex, "ColliderCast.ConvexConvex can only process convex colliders"); + + // Get the current transform + MTransform targetFromQuery = new MTransform(input.Orientation, input.Position); + + // Conservative advancement + const float tolerance = 1e-3f; // return if this close to a hit + const float keepDistance = 1e-4f; // avoid bad cases for GJK (penetration / exact hit) + int iterations = 10; // return after this many advances, regardless of accuracy + float fraction = 0.0f; + while (true) + { + if (fraction >= collector.MaxFraction) + { + // Exceeded the maximum fraction without a hit + return false; + } + + // Find the current distance + DistanceQueries.Result distanceResult = DistanceQueries.ConvexConvex(target, input.Collider, targetFromQuery); + + // Check for a hit + if (distanceResult.Distance < tolerance || --iterations == 0) + { + targetFromQuery.Translation = input.Ray.Origin; + return collector.AddHit(new ColliderCastHit + { + Position = distanceResult.PositionOnBinA, + SurfaceNormal = -distanceResult.NormalInA, + Fraction = fraction, + ColliderKey = ColliderKey.Empty, + RigidBodyIndex = -1 + }); + } + + // Check for a miss + float dot = math.dot(distanceResult.NormalInA, input.Direction); + if (dot <= 0.0f) + { + // Collider is moving away from the target, it will never hit + return false; + } + + // Advance + fraction += (distanceResult.Distance - keepDistance) / dot; + if (fraction >= collector.MaxFraction) + { + // Exceeded the maximum fraction without a hit + return false; + } + + targetFromQuery.Translation = input.Position + fraction * input.Direction; + } + } + + private unsafe struct ConvexMeshLeafProcessor : IColliderCastLeafProcessor + { + private readonly Mesh* m_Mesh; + private readonly uint m_NumColliderKeyBits; + + public ConvexMeshLeafProcessor(MeshCollider* meshCollider) + { + m_Mesh = &meshCollider->Mesh; + m_NumColliderKeyBits = meshCollider->NumColliderKeyBits; + } + + public bool ColliderCastLeaf(ColliderCastInput input, int primitiveKey, ref T collector) + where T : struct, ICollector + { + m_Mesh->GetPrimitive(primitiveKey, out float3x4 vertices, out Mesh.PrimitiveFlags flags, out CollisionFilter filter); + + if (!CollisionFilter.IsCollisionEnabled(input.Collider->Filter, filter)) // TODO: could do this check within GetPrimitive() + { + return false; + } + + int numPolygons = Mesh.GetNumPolygonsInPrimitive(flags); + bool isQuad = Mesh.IsPrimitveFlagSet(flags, Mesh.PrimitiveFlags.IsQuad); + + bool acceptHit = false; + int numHits = collector.NumHits; + + var polygon = new PolygonCollider(); + polygon.InitEmpty(); + for (int polygonIndex = 0; polygonIndex < numPolygons; polygonIndex++) + { + float fraction = collector.MaxFraction; + + if (isQuad) + { + polygon.SetAsQuad(vertices[0], vertices[1], vertices[2], vertices[3]); + } + else + { + polygon.SetAsTriangle(vertices[0], vertices[1 + polygonIndex], vertices[2 + polygonIndex]); + } + + if (ConvexConvex(input, (Collider*)&polygon, ref collector)) + { + acceptHit = true; + // TODO.ma make a version that doesn't transform, just updates collider key + collector.TransformNewHits(numHits++, fraction, MTransform.Identity, m_NumColliderKeyBits, (uint)(primitiveKey << 1 | polygonIndex)); + } + } + + return acceptHit; + } + } + + private static unsafe bool ConvexMesh(ColliderCastInput input, MeshCollider* meshCollider, ref T collector) + where T : struct, ICollector + { + var leafProcessor = new ConvexMeshLeafProcessor(meshCollider); + return meshCollider->Mesh.BoundingVolumeHierarchy.ColliderCast(input, ref leafProcessor, ref collector); + } + + private unsafe struct ConvexCompoundLeafProcessor : IColliderCastLeafProcessor + { + private readonly CompoundCollider* m_CompoundCollider; + + public ConvexCompoundLeafProcessor(CompoundCollider* compoundCollider) + { + m_CompoundCollider = compoundCollider; + } + + public bool ColliderCastLeaf(ColliderCastInput input, int leafData, ref T collector) + where T : struct, ICollector + { + ref CompoundCollider.Child child = ref m_CompoundCollider->Children[leafData]; + + if (!CollisionFilter.IsCollisionEnabled(input.Collider->Filter, child.Collider->Filter)) + { + return false; + } + + // Transform the cast into child space + ColliderCastInput inputLs = input; + RigidTransform childFromCompound = math.inverse(child.CompoundFromChild); + inputLs.Position = math.transform(childFromCompound, input.Ray.Origin); + inputLs.Orientation = math.mul(childFromCompound.rot, input.Orientation); + inputLs.Direction = math.mul(childFromCompound.rot, input.Direction); + + int numHits = collector.NumHits; + float fraction = collector.MaxFraction; + if (child.Collider->CastCollider(inputLs, ref collector)) + { + // Transform results back to compound space + collector.TransformNewHits(numHits, fraction, new MTransform(child.CompoundFromChild), m_CompoundCollider->NumColliderKeyBits, (uint)leafData); + return true; + } + return false; + } + } + + private static unsafe bool ConvexCompound(ColliderCastInput input, CompoundCollider* compoundCollider, ref T collector) + where T : struct, ICollector + { + var leafProcessor = new ConvexCompoundLeafProcessor(compoundCollider); + return compoundCollider->BoundingVolumeHierarchy.ColliderCast(input, ref leafProcessor, ref collector); + } + } +} diff --git a/package/Unity.Physics/Collision/Queries/ColliderCast.cs.meta b/package/Unity.Physics/Collision/Queries/ColliderCast.cs.meta new file mode 100755 index 000000000..3303d746f --- /dev/null +++ b/package/Unity.Physics/Collision/Queries/ColliderCast.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 1d24d042594cff64299a5ef3681e304a +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Unity.Physics/Collision/Queries/ConvexConvexDistance.cs b/package/Unity.Physics/Collision/Queries/ConvexConvexDistance.cs new file mode 100755 index 000000000..77c6e0f0d --- /dev/null +++ b/package/Unity.Physics/Collision/Queries/ConvexConvexDistance.cs @@ -0,0 +1,542 @@ +using System.Diagnostics; +using Unity.Mathematics; +using static Unity.Physics.Math; +using Debug = UnityEngine.Debug; +using UnityEngine.Assertions; + +namespace Unity.Physics +{ + // Low level convex-convex distance query implementations + internal static class ConvexConvexDistanceQueries + { + // Convex distance result + public struct Result + { + public DistanceQueries.Result ClosestPoints; + public uint3 Simplex; + public int Iterations; + + public const uint InvalidSimplexVertex = 0xffffffff; + + public bool Valid => math.all(ClosestPoints.NormalInA == new float3(0)); + public int SimplexDimension => Simplex.z == InvalidSimplexVertex ? (Simplex.y == InvalidSimplexVertex ? 1 : 2) : 3; + + public int SimplexVertexA(int index) => (int)(Simplex[index] >> 16); + public int SimplexVertexB(int index) => (int)(Simplex[index] & 0xffff); + } + + public enum PenetrationHandling + { + DotNotCompute, + Exact3D + } + + // Supporting vertex + [DebuggerDisplay("{Xyz}:{Id}")] + private struct SupportVertex + { + public float3 Xyz; + public uint Id; + + public int IdA => (int)(Id >> 16); + public int IdB => (int)(Id & 0xffff); + } + + // Simplex + private struct Simplex + { + public SupportVertex A, B, C, D; + public float3 Direction; // Points from the origin towards the closest point on the simplex + public float ScaledDistance; // ClosestPoint = Direction * ScaledDistance / lengthSq(Direction) + public int NumVertices; + + /// + /// Compute the closest point on the simplex, returns true if the simplex contains a duplicate vertex + /// + public void SolveDistance() + { + int inputVertices = NumVertices; + + switch (NumVertices) + { + // Point. + case 1: + Direction = A.Xyz; + ScaledDistance = math.lengthsq(Direction); + break; + + // Line. + case 2: + { + float3 delta = B.Xyz - A.Xyz; + float den = math.dot(delta, delta); + float num = math.dot(-A.Xyz, delta); + + // Reduce if closest point do not project on the line segment. + if (num >= den) { NumVertices = 1; A = B; goto case 1; } + + // Compute support direction + Direction = math.cross(math.cross(delta, A.Xyz), delta); + ScaledDistance = math.dot(Direction, A.Xyz); + } + break; + + // Triangle. + case 3: + { + float3 ca = A.Xyz - C.Xyz; + float3 cb = B.Xyz - C.Xyz; + float3 n = math.cross(cb, ca); + + // Reduce if closest point do not project in the triangle. + float3 crossA = math.cross(cb, n); + float3 crossB = math.cross(n, ca); + float detA = math.dot(crossA, B.Xyz); + float detB = math.dot(crossB, C.Xyz); + if (detA < 0) + { + if (detB >= 0 || Det(n, crossA, C.Xyz) < 0) + { + A = B; + } + } + else if (detB >= 0) + { + float dot = math.dot(C.Xyz, n); + if (dot < 0) + { + // Reorder vertices so that n points away from the origin + SupportVertex temp = A; + A = B; + B = temp; + n = -n; + dot = -dot; + } + Direction = n; + ScaledDistance = dot; + break; + } + + B = C; + NumVertices = 2; + goto case 2; + } + + // Tetrahedra. + case 4: + { + FourTransposedPoints tetra = new FourTransposedPoints(A.Xyz, B.Xyz, C.Xyz, D.Xyz); + FourTransposedPoints d = new FourTransposedPoints(D.Xyz); + + // This routine finds the closest feature to the origin on the tetra by testing the origin against the planes of the + // voronoi diagram. If the origin is near the border of two regions in the diagram, then the plane tests might exclude + // it from both because of float rounding. To avoid this problem we use some tolerance testing the face planes and let + // EPA handle those border cases. 1e-5 is a somewhat arbitrary value and the actual distance scales with the tetra, so + // this might need to be tuned later! + float3 faceTest = tetra.Cross(tetra.V1203).Dot(d).xyz; + if (math.all(faceTest >= -1e-5f)) + { + // Origin is inside the tetra + Direction = float3.zero; + break; + } + + // Check if the closest point is on a face + bool3 insideFace = (faceTest >= 0).xyz; + FourTransposedPoints edges = d - tetra; + FourTransposedPoints normals = edges.Cross(edges.V1203); + bool3 insideEdge0 = (normals.Cross(edges).Dot(d) >= 0).xyz; + bool3 insideEdge1 = (edges.V1203.Cross(normals).Dot(d) >= 0).xyz; + bool3 onFace = (insideEdge0 & insideEdge1 & !insideFace); + if (math.any(onFace)) + { + if (onFace.y) { A = B; B = C; } + else if (onFace.z) { B = C; } + } + else + { + // Check if the closest point is on an edge + // TODO maybe we can safely drop two vertices in this case + bool3 insideVertex = (edges.Dot(d) >= 0).xyz; + bool3 onEdge = (!insideEdge0 & !insideEdge1.zxy & insideVertex); + if (math.any(onEdge.yz)) { A = B; B = C; } + } + + C = D; + NumVertices = 3; + goto case 3; + } + } + } + + // Compute the barycentric coordinates of the closest point. + public float4 ComputeBarycentricCoordinates(float3 closestPoint) + { + float4 coordinates = new float4(0); + switch (NumVertices) + { + case 1: + coordinates.x = 1; + break; + case 2: + coordinates.x = math.distance(B.Xyz, closestPoint) / math.distance(A.Xyz, B.Xyz); + coordinates.y = 1 - coordinates.x; + break; + case 3: + coordinates.x = math.length(math.cross(B.Xyz - closestPoint, C.Xyz - closestPoint)); + coordinates.y = math.length(math.cross(C.Xyz - closestPoint, A.Xyz - closestPoint)); + coordinates.z = math.length(math.cross(A.Xyz - closestPoint, B.Xyz - closestPoint)); + coordinates /= math.csum(coordinates.xyz); + break; + case 4: + coordinates.x = Det(D.Xyz, C.Xyz, B.Xyz); + coordinates.y = Det(D.Xyz, A.Xyz, C.Xyz); + coordinates.z = Det(D.Xyz, B.Xyz, A.Xyz); + coordinates.w = Det(A.Xyz, B.Xyz, C.Xyz); + coordinates /= math.csum(coordinates.xyzw); + break; + } + + return coordinates; + } + } + + /// + /// Generalized convex-convex distance. + /// + /// Vertices of the first collider in local space + /// Vertices of the second collider in local space + /// Transform from the local space of B to the local space of A + /// How to compute penetration. + /// + public static unsafe Result ConvexConvex( + float3* verticesA, int numVerticesA, float3* verticesB, int numVerticesB, + MTransform aFromB, PenetrationHandling penetrationHandling) + { + const float epsTerminationSq = 1e-8f; // Main loop quits when it cannot find a point that improves the simplex by at least this much + const float epsPenetrationSq = 1e-9f; // Epsilon used to check for penetration. Should be smaller than shape cast ConvexConvex keepDistance^2. + + // Initialize simplex. + Simplex simplex = new Simplex(); + simplex.NumVertices = 1; + simplex.A = GetSupportingVertex(new float3(1, 0, 0), verticesA, numVerticesA, verticesB, numVerticesB, aFromB); + simplex.Direction = simplex.A.Xyz; + simplex.ScaledDistance = math.lengthsq(simplex.A.Xyz); + float scaleSq = simplex.ScaledDistance; + + // Iterate. + int iteration = 0; + bool penetration = false; + const int maxIterations = 64; + for (; iteration < maxIterations; ++iteration) + { + // Find a new support vertex + SupportVertex newSv = GetSupportingVertex(-simplex.Direction, verticesA, numVerticesA, verticesB, numVerticesB, aFromB); + + // If the new vertex is not significantly closer to the origin, quit + float scaledImprovement = math.dot(simplex.A.Xyz - newSv.Xyz, simplex.Direction); + if (scaledImprovement * math.abs(scaledImprovement) < epsTerminationSq * scaleSq) + { + break; + } + + // Add the new vertex and reduce the simplex + switch (simplex.NumVertices++) + { + case 1: simplex.B = newSv; break; + case 2: simplex.C = newSv; break; + default: simplex.D = newSv; break; + } + simplex.SolveDistance(); + + // Check for penetration + scaleSq = math.lengthsq(simplex.Direction); + float scaledDistanceSq = simplex.ScaledDistance * simplex.ScaledDistance; + if (simplex.NumVertices == 4 || scaledDistanceSq <= epsPenetrationSq * scaleSq) + { + penetration = true; + break; + } + } + + // Finalize result. + var ret = new Result { Iterations = iteration + 1 }; + + // Handle penetration. + if (penetration && penetrationHandling != PenetrationHandling.DotNotCompute) + { + // Allocate a hull for EPA + int verticesCapacity = 64; + int triangleCapacity = 2 * verticesCapacity; + ConvexHullBuilder.Vertex* vertices = stackalloc ConvexHullBuilder.Vertex[verticesCapacity]; + ConvexHullBuilder.Triangle* triangles = stackalloc ConvexHullBuilder.Triangle[triangleCapacity]; + var hull = new ConvexHullBuilder(vertices, verticesCapacity, triangles, triangleCapacity); + + // Initialize int space + // TODO - either the hull should be robust when int space changes after points are already added, or the ability to do so should be removed, probably the latter + // Currently for example a valid triangle can collapse to a line segment when the bounds grow + hull.IntegerSpaceAabb = GetSupportingAabb(verticesA, numVerticesA, verticesB, numVerticesB, aFromB); + + // Add simplex vertices to the hull, remove any vertices from the simplex that do not increase the hull dimension + hull.AddPoint(simplex.A.Xyz, simplex.A.Id); + if (simplex.NumVertices > 1) + { + hull.AddPoint(simplex.B.Xyz, simplex.B.Id); + if (simplex.NumVertices > 2) + { + int dimension = hull.Dimension; + hull.AddPoint(simplex.C.Xyz, simplex.C.Id); + if (dimension == 0 && hull.Dimension == 1) + { + simplex.B = simplex.C; + } + if (simplex.NumVertices > 3) + { + dimension = hull.Dimension; + hull.AddPoint(simplex.D.Xyz, simplex.D.Id); + if (dimension > hull.Dimension) + { + if (dimension == 0) + { + simplex.B = simplex.D; + } + else if (dimension == 1) + { + simplex.C = simplex.D; + } + } + } + } + } + simplex.NumVertices = (hull.Dimension + 1); + + // If the simplex is not 3D, try expanding the hull in all directions + while (hull.Dimension < 3) + { + // Choose expansion directions + float3 support0, support1, support2; + switch (simplex.NumVertices) + { + case 1: + support0 = new float3(1, 0, 0); + support1 = new float3(0, 1, 0); + support2 = new float3(0, 0, 1); + break; + case 2: + Math.CalculatePerpendicularNormalized(math.normalize(simplex.B.Xyz - simplex.A.Xyz), out support0, out support1); + support2 = float3.zero; + break; + default: + UnityEngine.Assertions.Assert.IsTrue(simplex.NumVertices == 3); + support0 = math.cross(simplex.B.Xyz - simplex.A.Xyz, simplex.C.Xyz - simplex.A.Xyz); + support1 = float3.zero; + support2 = float3.zero; + break; + } + + // Try each one + int numSupports = 4 - simplex.NumVertices; + bool success = false; + for (int i = 0; i < numSupports; i++) + { + for (int j = 0; j < 2; j++) // +/- each direction + { + SupportVertex vertex = GetSupportingVertex(support0, verticesA, numVerticesA, verticesB, numVerticesB, aFromB); + hull.AddPoint(vertex.Xyz, vertex.Id); + if (hull.Dimension == simplex.NumVertices) + { + switch (simplex.NumVertices) + { + case 1: simplex.B = vertex; break; + case 2: simplex.C = vertex; break; + default: simplex.D = vertex; break; + } + + // Next dimension + success = true; + simplex.NumVertices++; + i = numSupports; + break; + } + support0 = -support0; + } + support0 = support1; + support1 = support2; + } + + if (!success) + { + break; + } + } + + // We can still fail to build a tetrahedron if the minkowski difference is really flat. + // In those cases just find the closest point to the origin on the infinite extension of the simplex (point / line / plane) + if (hull.Dimension != 3) + { + switch (simplex.NumVertices) + { + case 1: + { + ret.ClosestPoints.Distance = math.length(simplex.A.Xyz); + ret.ClosestPoints.NormalInA = -math.normalizesafe(simplex.A.Xyz, new float3(1, 0, 0)); + break; + } + case 2: + { + float3 edge = math.normalize(simplex.B.Xyz - simplex.A.Xyz); + float3 direction = math.cross(math.cross(edge, simplex.A.Xyz), edge); + Math.CalculatePerpendicularNormalized(edge, out float3 safeNormal, out float3 unused); // backup, take any direction perpendicular to the edge + float3 normal = math.normalizesafe(direction, safeNormal); + ret.ClosestPoints.Distance = math.dot(normal, simplex.A.Xyz); + ret.ClosestPoints.NormalInA = -normal; + break; + } + default: + { + UnityEngine.Assertions.Assert.IsTrue(simplex.NumVertices == 3); + float3 normal = math.normalize(math.cross(simplex.B.Xyz - simplex.A.Xyz, simplex.C.Xyz - simplex.A.Xyz)); + float dot = math.dot(normal, simplex.A.Xyz); + ret.ClosestPoints.Distance = math.abs(dot); + ret.ClosestPoints.NormalInA = math.select(-normal, normal, dot < 0); + break; + } + } + } + else + { + int closestTriangleIndex; + Plane closestPlane = new Plane(); + float stopThreshold = 1e-4f; + uint* uidsCache = stackalloc uint[triangleCapacity]; + for (int i = 0; i < triangleCapacity; i++) + { + uidsCache[i] = 0; + } + float* distancesCache = stackalloc float[triangleCapacity]; + do + { + // Select closest triangle. + closestTriangleIndex = -1; + foreach (int triangleIndex in hull.Triangles.Indices) + { + if (hull.Triangles[triangleIndex].Uid != uidsCache[triangleIndex]) + { + uidsCache[triangleIndex] = hull.Triangles[triangleIndex].Uid; + distancesCache[triangleIndex] = hull.ComputePlane(triangleIndex).Distance; + } + if (closestTriangleIndex == -1 || distancesCache[closestTriangleIndex] < distancesCache[triangleIndex]) + { + closestTriangleIndex = triangleIndex; + } + } + closestPlane = hull.ComputePlane(closestTriangleIndex); + + // Add supporting vertex or exit. + SupportVertex sv = GetSupportingVertex(closestPlane.Normal, verticesA, numVerticesA, verticesB, numVerticesB, aFromB); + float d2P = math.dot(closestPlane.Normal, sv.Xyz) + closestPlane.Distance; + if (math.abs(d2P) > stopThreshold && hull.AddPoint(sv.Xyz, sv.Id)) + stopThreshold *= 1.3f; + else + break; + } while (++iteration < maxIterations); + + // Generate simplex. + ConvexHullBuilder.Triangle triangle = hull.Triangles[closestTriangleIndex]; + simplex.NumVertices = 3; + simplex.A.Xyz = hull.Vertices[triangle.Vertex0].Position; simplex.A.Id = hull.Vertices[triangle.Vertex0].UserData; + simplex.B.Xyz = hull.Vertices[triangle.Vertex1].Position; simplex.B.Id = hull.Vertices[triangle.Vertex1].UserData; + simplex.C.Xyz = hull.Vertices[triangle.Vertex2].Position; simplex.C.Id = hull.Vertices[triangle.Vertex2].UserData; + simplex.Direction = -closestPlane.Normal; + simplex.ScaledDistance = closestPlane.Distance; + + // Set normal and distance. + ret.ClosestPoints.NormalInA = -closestPlane.Normal; + ret.ClosestPoints.Distance = closestPlane.Distance; + } + } + else + { + // Compute distance and normal. + float lengthSq = math.lengthsq(simplex.Direction); + float invLength = math.rsqrt(lengthSq); + bool smallLength = lengthSq < 1e-10f; + ret.ClosestPoints.Distance = math.select(simplex.ScaledDistance * invLength, 0.0f, smallLength); + ret.ClosestPoints.NormalInA = math.select(simplex.Direction * invLength, new float3(1, 0, 0), smallLength); + + // Make sure the normal is always valid. + if (!math.all(math.isfinite(ret.ClosestPoints.NormalInA))) + { + ret.ClosestPoints.NormalInA = new float3(1, 0, 0); + } + } + + // Compute position. + float3 closestPoint = ret.ClosestPoints.NormalInA * ret.ClosestPoints.Distance; + float4 coordinates = simplex.ComputeBarycentricCoordinates(closestPoint); + ret.ClosestPoints.PositionOnAinA = + verticesA[simplex.A.IdA] * coordinates.x + + verticesA[simplex.B.IdA] * coordinates.y + + verticesA[simplex.C.IdA] * coordinates.z + + verticesA[simplex.D.IdA] * coordinates.w; + + // Encode simplex. + ret.Simplex.x = simplex.A.Id; + ret.Simplex.y = simplex.NumVertices >= 2 ? simplex.B.Id : Result.InvalidSimplexVertex; + ret.Simplex.z = simplex.NumVertices >= 3 ? simplex.C.Id : Result.InvalidSimplexVertex; + + // Done. + UnityEngine.Assertions.Assert.IsTrue(math.isfinite(ret.ClosestPoints.Distance)); + UnityEngine.Assertions.Assert.IsTrue(math.abs(math.lengthsq(ret.ClosestPoints.NormalInA) - 1.0f) < 1e-5f); + return ret; + } + + // Returns the supporting vertex index given a direction in local space. + private static unsafe int GetSupportingVertexIndex(float3 direction, float3* vertices, int numVertices) + { + int maxI = -1; + float maxD = 0; + for (int i = 0; i < numVertices; ++i) + { + float d = math.dot(direction, vertices[i]); + if (maxI == -1 || d > maxD) + { + maxI = i; + maxD = d; + } + } + return maxI; + } + + // Returns the supporting vertex of the CSO given a direction in 'A' space. + private static unsafe SupportVertex GetSupportingVertex( + float3 direction, float3* verticesA, int numVerticesA, float3* verticesB, int numVerticesB, MTransform aFromB) + { + int ia = GetSupportingVertexIndex(direction, verticesA, numVerticesA); + int ib = GetSupportingVertexIndex(math.mul(aFromB.InverseRotation, -direction), verticesB, numVerticesB); + return new SupportVertex { Xyz = verticesA[ia] - Mul(aFromB, verticesB[ib]), Id = ((uint)ia) << 16 | (uint)ib }; + } + + // Returns an AABB containing the CSO in A-space + private static unsafe Aabb GetSupportingAabb( + float3* verticesA, int numVerticesA, float3* verticesB, int numVerticesB, MTransform aFromB) + { + Aabb aabbA = new Aabb { Min = verticesA[0], Max = verticesA[0] }; + for (int i = 1; i < numVerticesA; i++) + { + aabbA.Min = math.min(aabbA.Min, verticesA[i]); + aabbA.Max = math.max(aabbA.Max, verticesA[i]); + } + + Aabb aabbB = new Aabb { Min = verticesB[0], Max = verticesB[0] }; + for (int i = 1; i < numVerticesB; i++) + { + aabbB.Min = math.min(aabbB.Min, verticesB[i]); + aabbB.Max = math.max(aabbB.Max, verticesB[i]); + } + + Aabb aabbBinA = Math.TransformAabb(aFromB, aabbB); + return new Aabb { Min = aabbA.Min - aabbBinA.Max, Max = aabbA.Max - aabbBinA.Min }; + } + } +} diff --git a/package/Unity.Physics/Collision/Queries/ConvexConvexDistance.cs.meta b/package/Unity.Physics/Collision/Queries/ConvexConvexDistance.cs.meta new file mode 100755 index 000000000..edc7644cd --- /dev/null +++ b/package/Unity.Physics/Collision/Queries/ConvexConvexDistance.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 55cb3c5abf2aca04680b44c0db1bb91f +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Unity.Physics/Collision/Queries/ConvexConvexManifold.cs b/package/Unity.Physics/Collision/Queries/ConvexConvexManifold.cs new file mode 100755 index 000000000..b6b0a6d4c --- /dev/null +++ b/package/Unity.Physics/Collision/Queries/ConvexConvexManifold.cs @@ -0,0 +1,846 @@ +using Unity.Mathematics; +using UnityEngine.Assertions; +using static Unity.Physics.Math; + +namespace Unity.Physics +{ + // Low level convex-convex contact manifold query implementations + internal static class ConvexConvexManifoldQueries + { + // The output of convex-convex manifold queries + public unsafe struct Manifold + { + public int NumContacts; + public float3 Normal; + + public const int k_MaxNumContacts = 32; + private fixed float m_ContactPositions[k_MaxNumContacts * 3]; + private fixed float m_Distances[k_MaxNumContacts]; + + // Create a single point manifold from a distance query result + public Manifold(DistanceQueries.Result convexDistance, MTransform worldFromA) + { + NumContacts = 1; + Normal = math.mul(worldFromA.Rotation, convexDistance.NormalInA); + this[0] = new ContactPoint + { + Distance = convexDistance.Distance, + Position = Mul(worldFromA, convexDistance.PositionOnBinA) + }; + } + + public ContactPoint this[int contactIndex] + { + get + { + Assert.IsTrue(contactIndex >= 0 && contactIndex < k_MaxNumContacts); + + int offset = contactIndex * 3; + var contact = new ContactPoint(); + + fixed (float* positions = m_ContactPositions) + { + contact.Position.x = positions[offset]; + contact.Position.y = positions[offset + 1]; + contact.Position.z = positions[offset + 2]; + } + + fixed (float* distances = m_Distances) + { + contact.Distance = distances[contactIndex]; + } + + return contact; + } + set + { + Assert.IsTrue(contactIndex >= 0 && contactIndex < k_MaxNumContacts); + + int offset = contactIndex * 3; + fixed (float* positions = m_ContactPositions) + { + positions[offset] = value.Position.x; + positions[offset + 1] = value.Position.y; + positions[offset + 2] = value.Position.z; + } + + fixed (float* distances = m_Distances) + { + distances[contactIndex] = value.Distance; + } + } + } + + public void Flip() + { + for (int i = 0; i < NumContacts; i++) + { + ContactPoint contact = this[i]; + contact.Position += Normal * contact.Distance; + this[i] = contact; + } + Normal = -Normal; + } + } + + #region Convex vs convex + + // Create a contact point for a pair of spheres in world space. + public static unsafe void SphereSphere( + SphereCollider* sphereA, SphereCollider* sphereB, + MTransform worldFromA, MTransform aFromB, float maxDistance, + out Manifold manifold) + { + DistanceQueries.Result convexDistance = DistanceQueries.SphereSphere(sphereA, sphereB, aFromB); + if (convexDistance.Distance < maxDistance) + { + manifold = new Manifold(convexDistance, worldFromA); + } + else + { + manifold = new Manifold(); + } + } + + // Create a contact point for a box and a sphere in world space. + public static unsafe void BoxSphere( + BoxCollider* boxA, SphereCollider* sphereB, + MTransform worldFromA, MTransform aFromB, float maxDistance, + out Manifold manifold) + { + DistanceQueries.Result convexDistance = DistanceQueries.BoxSphere(boxA, sphereB, aFromB); + if (convexDistance.Distance < maxDistance) + { + manifold = new Manifold(convexDistance, worldFromA); + } + else + { + manifold = new Manifold(); + } + } + + // Create contact points for a pair of boxes in world space. + public static unsafe void BoxBox( + BoxCollider* boxA, BoxCollider* boxB, + MTransform worldFromA, MTransform aFromB, float maxDistance, + out Manifold manifold) + { + manifold = new Manifold(); + + // Get transforms with box center at origin + MTransform bFromBoxB = new MTransform(boxB->Orientation, boxB->Center); + MTransform aFromBoxA = new MTransform(boxA->Orientation, boxA->Center); + MTransform boxAFromBoxB = Mul(Inverse(aFromBoxA), Mul(aFromB, bFromBoxB)); + MTransform boxBFromBoxA = Inverse(boxAFromBoxB); + + float3 halfExtentsA = boxA->Size * 0.5f; + float3 halfExtentsB = boxB->Size * 0.5f; + + // Test planes of each box against the other's vertices + float3 normal; // in BoxA-space + float distance; + { + float3 normalA = new float3(1, 0, 0); + float3 normalB = new float3(1, 0, 0); + float distA = 0.0f; + float distB = 0.0f; + if (!PointPlanes(boxAFromBoxB, halfExtentsA, halfExtentsB, maxDistance, ref normalA, ref distA) || + !PointPlanes(boxBFromBoxA, halfExtentsB, halfExtentsA, maxDistance, ref normalB, ref distB)) + { + return; + } + + normalB = math.mul(boxAFromBoxB.Rotation, normalB); + bool aGreater = distA > distB; + normal = math.select(-normalB, normalA, (bool3)aGreater); + distance = math.select(distB, distA, aGreater); + } + + // Test edge pairs + { + float3 edgeA = new float3(1.0f, 0.0f, 0.0f); + for (int i = 0; i < 3; i++) + { + for (int j = 0; j < 3; j++) + { + float3 edgeB; + switch (j) + { + case 0: edgeB = boxAFromBoxB.Rotation.c0; break; + case 1: edgeB = boxAFromBoxB.Rotation.c1; break; + case 2: edgeB = boxAFromBoxB.Rotation.c2; break; + default: edgeB = new float3(0.0f); break; + } + float3 dir = math.cross(edgeA, edgeB); + + // hack around parallel edges + if (math.all(math.abs(dir) < new float3(1e-5f))) + { + continue; + } + + float3 edgeNormal = math.normalize(dir); + float3 supportA = math.select(halfExtentsA, -halfExtentsA, dir < new float3(0.0f)); + float maxA = math.abs(math.dot(supportA, edgeNormal)); + float minA = -maxA; + float3 dirInB = math.mul(boxBFromBoxA.Rotation, dir); + float3 supportBinB = math.select(halfExtentsB, -halfExtentsB, dirInB < new float3(0.0f)); + float3 supportB = math.mul(boxAFromBoxB.Rotation, supportBinB); + float offsetB = math.abs(math.dot(supportB, edgeNormal)); + float centerB = math.dot(boxAFromBoxB.Translation, edgeNormal); + float maxB = centerB + offsetB; + float minB = centerB - offsetB; + + float2 diffs = new float2(minB - maxA, minA - maxB); // positive normal, negative normal + if (math.all(diffs > new float2(maxDistance))) + { + return; + } + + if (diffs.x > distance) + { + distance = diffs.x; + normal = -edgeNormal; + } + + if (diffs.y > distance) + { + distance = diffs.y; + normal = edgeNormal; + } + } + + edgeA = edgeA.zxy; + } + } + + if (distance < maxDistance) + { + // Get the normal and supporting faces + float3 normalInA = math.mul(boxA->Orientation, normal); + manifold.Normal = math.mul(worldFromA.Rotation, normalInA); + int faceIndexA = boxA->ConvexHull.GetSupportingFace(-normalInA); + int faceIndexB = boxB->ConvexHull.GetSupportingFace(math.mul(math.transpose(aFromB.Rotation), normalInA)); + + // Build manifold + if (!FaceFace(ref boxA->ConvexHull, ref boxB->ConvexHull, faceIndexA, faceIndexB, worldFromA, aFromB, normalInA, distance, ref manifold)) + { + // TODO.ma - the closest points are vertices, we need GJK to find them. + // We could call GJK here, or always use GJK instead of BoxTriangle(), or do nothing and live with possible tunneling. + // This method is ~2.5x faster than GJK, so if it gets the right result without calling GJK most of the time, it's worth keeping even if we call GJK when it fails. + } + } + } + + // Create a single point manifold between a capsule and a sphere in world space. + public static unsafe void CapsuleSphere( + CapsuleCollider* capsuleA, SphereCollider* sphereB, + MTransform worldFromA, MTransform aFromB, float maxDistance, + out Manifold manifold) + { + DistanceQueries.Result convexDistance = DistanceQueries.CapsuleSphere( + capsuleA->Vertex0, capsuleA->Vertex1, capsuleA->Radius, sphereB->Center, sphereB->Radius, aFromB); + if (convexDistance.Distance < maxDistance) + { + manifold = new Manifold(convexDistance, worldFromA); + } + else + { + manifold = new Manifold(); + } + } + + // Create a contact point for a pair of capsules in world space. + public static unsafe void CapsuleCapsule( + CapsuleCollider* capsuleA, CapsuleCollider* capsuleB, + MTransform worldFromA, MTransform aFromB, float maxDistance, + out Manifold manifold) + { + // TODO: Should produce a multi-point manifold + DistanceQueries.Result convexDistance = DistanceQueries.CapsuleCapsule(capsuleA, capsuleB, aFromB); + if (convexDistance.Distance < maxDistance) + { + manifold = new Manifold(convexDistance, worldFromA); + } + else + { + manifold = new Manifold(); + } + } + + // Create contact points for a box and triangle in world space. + public static unsafe void BoxTriangle( + BoxCollider* boxA, PolygonCollider* triangleB, + MTransform worldFromA, MTransform aFromB, float maxDistance, + out Manifold manifold) + { + Assert.IsTrue(triangleB->Vertices.Length == 3); + + // Get triangle in box space + MTransform aFromBoxA = new MTransform(boxA->Orientation, boxA->Center); + MTransform boxAFromB = Mul(Inverse(aFromBoxA), aFromB); + float3 t0 = Mul(boxAFromB, triangleB->ConvexHull.Vertices[0]); + float3 t1 = Mul(boxAFromB, triangleB->ConvexHull.Vertices[1]); + float3 t2 = Mul(boxAFromB, triangleB->ConvexHull.Vertices[2]); + + Plane triPlane = triangleB->ConvexHull.Planes[0]; + float3 triangleNormal = math.mul(boxAFromB.Rotation, triPlane.Normal); + FourTransposedPoints vertsB; + FourTransposedPoints edgesB; + FourTransposedPoints perpsB; + CalcTrianglePlanes(t0, t1, t2, triangleNormal, out vertsB, out edgesB, out perpsB); + + float3 halfExtents = boxA->Size * 0.5f + maxDistance; + + // find the closest minkowski plane + float4 plane; + { + // Box face vs triangle vertex + float4 planeFaceVertex; + { + // get aabb of minkowski diff + float3 tMin = math.min(math.min(t0, t1), t2) - halfExtents; + float3 tMax = math.max(math.max(t0, t1), t2) + halfExtents; + + // find the aabb face closest to the origin + float3 axis0 = new float3(1, 0, 0); + float3 axis1 = axis0.zxy; // 010 + float3 axis2 = axis0.yzx; // 001 + + float4 planeX = SelectMaxW(new float4(axis0, -tMax.x), new float4(-axis0, tMin.x)); + float4 planeY = SelectMaxW(new float4(axis1, -tMax.y), new float4(-axis1, tMin.y)); + float4 planeZ = SelectMaxW(new float4(axis2, -tMax.z), new float4(-axis2, tMin.z)); + + planeFaceVertex = SelectMaxW(planeX, planeY); + planeFaceVertex = SelectMaxW(planeFaceVertex, planeZ); + } + + // Box vertex vs triangle face + float4 planeVertexFace; + { + // Calculate the triangle normal + float triangleOffset = math.dot(triangleNormal, t0); + float expansionOffset = math.dot(math.abs(triangleNormal), halfExtents); + planeVertexFace = SelectMaxW( + new float4(triangleNormal, -triangleOffset - expansionOffset), + new float4(-triangleNormal, triangleOffset - expansionOffset)); + } + + // Edge planes + float4 planeEdgeEdge = new float4(0, 0, 0, -float.MaxValue); + { + // Test the planes from crossing axis i with each edge of the triangle, for example if i = 1 then n0 is from (0, 1, 0) x (t1 - t0). + for (int i = 0, j = 1, k = 2; i < 3; j = k, k = i, i++) + { + // Normalize the cross product and flip it to point outward from the edge + float4 lengthsSq = edgesB.GetComponent(j) * edgesB.GetComponent(j) + edgesB.GetComponent(k) * edgesB.GetComponent(k); + float4 invLengths = math.rsqrt(lengthsSq); + float4 dots = edgesB.GetComponent(j) * perpsB.GetComponent(k) - edgesB.GetComponent(k) * perpsB.GetComponent(j); + float4 factors = invLengths * math.sign(dots); + + float4 nj = -edgesB.GetComponent(k) * factors; + float4 nk = edgesB.GetComponent(j) * factors; + float4 distances = -nj * vertsB.GetComponent(j) - nk * vertsB.GetComponent(k) - math.abs(nj) * halfExtents[j] - math.abs(nk) * halfExtents[k]; + + // If the box edge is parallel to the triangle face then skip it, the plane is redundant with a vertex-face plane + bool4 valid = dots != float4.zero; + distances = math.select(Constants.Min4F, distances, valid); + + float3 n0 = new float3(); n0[i] = 0.0f; n0[j] = nj[0]; n0[k] = nk[0]; + float3 n1 = new float3(); n1[i] = 0.0f; n1[j] = nj[1]; n1[k] = nk[1]; + float3 n2 = new float3(); n2[i] = 0.0f; n2[j] = nj[2]; n2[k] = nk[2]; + float4 temp = SelectMaxW(SelectMaxW(new float4(n0, distances.x), new float4(n1, distances.y)), new float4(n2, distances.z)); + planeEdgeEdge = SelectMaxW(planeEdgeEdge, temp); + } + } + + plane = SelectMaxW(SelectMaxW(planeFaceVertex, planeVertexFace), planeEdgeEdge); + } + + manifold = new Manifold(); + + // Check for a separating plane TODO.ma could early out as soon as any plane with w>0 is found + if (plane.w <= 0.0f) + { + // Get the normal and supporting faces + float3 normalInA = math.mul(boxA->Orientation, plane.xyz); + manifold.Normal = math.mul(worldFromA.Rotation, normalInA); + int faceIndexA = boxA->ConvexHull.GetSupportingFace(-normalInA); + int faceIndexB = triangleB->ConvexHull.GetSupportingFace(math.mul(math.transpose(aFromB.Rotation), normalInA)); + + // Build manifold + if (!FaceFace(ref boxA->ConvexHull, ref triangleB->ConvexHull, faceIndexA, faceIndexB, worldFromA, aFromB, normalInA, float.MaxValue, ref manifold)) + { + // TODO.ma - the closest points are vertices, we need GJK to find them. + // We could call GJK here, or always use GJK instead of BoxTriangle(), or do nothing and live with possible tunneling. + // This method is ~2x faster than GJK, so if it gets the right result without calling GJK most of the time, it's worth keeping even if we call GJK when it fails. + } + } + } + + // Create a single point manifold between a triangle and sphere in world space. + public static unsafe void TriangleSphere( + PolygonCollider* triangleA, SphereCollider* sphereB, + MTransform worldFromA, MTransform aFromB, float maxDistance, + out Manifold manifold) + { + Assert.IsTrue(triangleA->Vertices.Length == 3); + + DistanceQueries.Result convexDistance = DistanceQueries.TriangleSphere( + triangleA->Vertices[0], triangleA->Vertices[1], triangleA->Vertices[2], triangleA->Planes[0].Normal, + sphereB->Center, sphereB->Radius, aFromB); + if (convexDistance.Distance < maxDistance) + { + manifold = new Manifold(convexDistance, worldFromA); + } + else + { + manifold = new Manifold(); + } + } + + // Create contact points for a capsule and triangle in world space. + public static unsafe void CapsuleTriangle( + CapsuleCollider* capsuleA, PolygonCollider* triangleB, + MTransform worldFromA, MTransform aFromB, float maxDistance, + out Manifold manifold) + { + Assert.IsTrue(triangleB->Vertices.Length == 3); + + DistanceQueries.Result convexDistance = DistanceQueries.CapsuleTriangle(capsuleA, triangleB, aFromB); + if (convexDistance.Distance < maxDistance) + { + // Build manifold + manifold = new Manifold + { + Normal = math.mul(worldFromA.Rotation, -convexDistance.NormalInA) // negate the normal because we are temporarily flipping to triangle A capsule B + }; + MTransform worldFromB = Mul(worldFromA, aFromB); + MTransform bFromA = Inverse(aFromB); + float3 normalInB = math.mul(bFromA.Rotation, convexDistance.NormalInA); + int faceIndexB = triangleB->ConvexHull.GetSupportingFace(normalInB); + if (FaceEdge(ref triangleB->ConvexHull, ref capsuleA->ConvexHull, faceIndexB, worldFromB, bFromA, -normalInB, convexDistance.Distance + capsuleA->Radius, ref manifold)) + { + manifold.Flip(); + } + else + { + manifold = new Manifold(convexDistance, worldFromA); + } + } + else + { + manifold = new Manifold(); + } + } + + // Create contact points for a pair of generic convex hulls in world space. + public static unsafe void ConvexConvex( + ref ConvexHull hullA, ref ConvexHull hullB, + MTransform worldFromA, MTransform aFromB, float maxDistance, + out Manifold manifold) + { + // Get closest points on the hulls + ConvexConvexDistanceQueries.Result result = ConvexConvexDistanceQueries.ConvexConvex( + hullA.VerticesPtr, hullA.NumVertices, hullB.VerticesPtr, hullB.NumVertices, aFromB, ConvexConvexDistanceQueries.PenetrationHandling.Exact3D); + + float sumRadii = hullB.ConvexRadius + hullA.ConvexRadius; + if (result.ClosestPoints.Distance < maxDistance + sumRadii) + { + float3 normal = result.ClosestPoints.NormalInA; + + manifold = new Manifold + { + Normal = math.mul(worldFromA.Rotation, normal) + }; + + if (hullA.NumFaces > 0) + { + int faceIndexA = hullA.GetSupportingFace(-normal, result.SimplexVertexA(0)); + if (hullB.NumFaces > 0) + { + // Convex vs convex + int faceIndexB = hullB.GetSupportingFace(math.mul(math.transpose(aFromB.Rotation), normal), result.SimplexVertexB(0)); + if (FaceFace(ref hullA, ref hullB, faceIndexA, faceIndexB, worldFromA, aFromB, normal, result.ClosestPoints.Distance, ref manifold)) + { + return; + } + } + else if (hullB.NumVertices == 2) + { + // Convex vs capsule + if (FaceEdge(ref hullA, ref hullB, faceIndexA, worldFromA, aFromB, normal, result.ClosestPoints.Distance, ref manifold)) + { + return; + } + } // Else convex vs sphere + } + else if (hullA.NumVertices == 2) + { + if (hullB.NumFaces > 0) + { + // Capsule vs convex + manifold.Normal = math.mul(worldFromA.Rotation, -normal); // negate the normal because we are temporarily flipping to triangle A capsule B + MTransform worldFromB = Mul(worldFromA, aFromB); + MTransform bFromA = Inverse(aFromB); + float3 normalInB = math.mul(bFromA.Rotation, normal); + int faceIndexB = hullB.GetSupportingFace(normalInB, result.SimplexVertexB(0)); + bool foundClosestPoint = FaceEdge(ref hullB, ref hullA, faceIndexB, worldFromB, bFromA, -normalInB, result.ClosestPoints.Distance, ref manifold); + manifold.Flip(); + if (foundClosestPoint) + { + return; + } + } // Else capsule vs capsule or sphere + } // Else sphere vs something + + // Either one of the shapes is a sphere, or both of the shapes are capsules, or both of the closest features are nearly perpendicular to the contact normal, + // or FaceFace()/FaceEdge() missed the closest point due to numerical error. In these cases, add the closest point directly to the manifold. + if (manifold.NumContacts < Manifold.k_MaxNumContacts) + { + DistanceQueries.Result convexDistance = result.ClosestPoints; + manifold[manifold.NumContacts++] = new ContactPoint + { + Position = Mul(worldFromA, convexDistance.PositionOnAinA) - manifold.Normal * (convexDistance.Distance - hullB.ConvexRadius), + Distance = convexDistance.Distance - sumRadii + }; + } + } + else + { + manifold = new Manifold(); + } + } + + #endregion + + #region Helpers + + // BoxBox() helper + private static bool PointPlanes(MTransform aFromB, float3 halfExtA, float3 halfExtB, float maxDistance, ref float3 normalOut, ref float distanceOut) + { + // Calculate the AABB of box B in A-space + Aabb aabbBinA; + { + Aabb aabbBinB = new Aabb { Min = -halfExtB, Max = halfExtB }; + aabbBinA = Math.TransformAabb(aFromB, aabbBinB); + } + + // Check for a miss + float3 toleranceHalfExt = halfExtA + maxDistance; + bool3 miss = (aabbBinA.Min > toleranceHalfExt) | (-toleranceHalfExt > aabbBinA.Max); + if (math.any(miss)) + { + return false; + } + + // Return the normal with minimum separating distance + float3 diff0 = aabbBinA.Min - halfExtA; // positive normal + float3 diff1 = -aabbBinA.Max - halfExtA; // negative normal + bool3 greater01 = diff0 > diff1; + float3 max01 = math.select(diff1, diff0, greater01); + distanceOut = math.cmax(max01); + + int axis = IndexOfMaxComponent(max01); + if (axis == 0) + { + normalOut = new float3(1.0f, 0.0f, 0.0f); + } + else if (axis == 1) + { + normalOut = new float3(0.0f, 1.0f, 0.0f); + } + else + { + normalOut = new float3(0.0f, 0.0f, 1.0f); + } + normalOut = math.select(normalOut, -normalOut, greater01); + + return true; + } + + // returns the argument with greater w component + private static float4 SelectMaxW(float4 a, float4 b) + { + return math.select(b, a, a.w > b.w); + } + + private static void CalcTrianglePlanes(float3 v0, float3 v1, float3 v2, float3 normalDirection, + out FourTransposedPoints verts, out FourTransposedPoints edges, out FourTransposedPoints perps) + { + verts = new FourTransposedPoints(v0, v1, v2, v0); + edges = verts.V1230 - verts; + perps = edges.Cross(new FourTransposedPoints(normalDirection)); + } + + #endregion + + #region Multiple contact generation + + // Iterates over the edges of a face + private unsafe struct EdgeIterator + { + // Current edge + public float3 Vertex0 { get; private set; } + public float3 Vertex1 { get; private set; } + public float3 Edge { get; private set; } + public float3 Perp { get; private set; } + public float Offset { get; private set; } + public int Index { get; private set; } + + // Face description + private float3* vertices; + private byte* indices; + private float3 normal; + private int count; + + public static unsafe EdgeIterator Begin(float3* vertices, byte* indices, float3 normal, int count) + { + EdgeIterator iterator = new EdgeIterator(); + iterator.vertices = vertices; + iterator.indices = indices; + iterator.normal = normal; + iterator.count = count; + + iterator.Vertex1 = (indices == null) ? vertices[count - 1] : vertices[indices[count - 1]]; + iterator.update(); + return iterator; + } + + public bool Valid() + { + return Index < count; + } + + public void Advance() + { + Index++; + update(); + } + + private void update() + { + Vertex0 = Vertex1; + Vertex1 = (indices == null) ? vertices[Index] : vertices[indices[Index]]; + + Edge = Vertex1 - Vertex0; + Perp = math.cross(Edge, normal); // points outwards from face + Offset = math.dot(Perp, Vertex1); + } + } + + // Cast ray originA, directionA against plane normalB, offsetB and update the ray hit fractions + private static void castRayPlane(float3 originA, float3 directionA, float3 normalB, float offsetB, ref float fracEnter, ref float fracExit) + { + // Cast edge A against plane B + float start = math.dot(originA, normalB) - offsetB; + float diff = math.dot(directionA, normalB); + float end = start + diff; + float frac = math.select(-start / diff, 0.0f, diff == 0.0f); + + bool startInside = (start <= 0.0f); + bool endInside = (end <= 0.0f); + + bool enter = !startInside & (frac > fracEnter); + fracEnter = math.select(fracEnter, frac, enter); + + bool exit = !endInside & (frac < fracExit); + fracExit = math.select(fracExit, frac, exit); + + bool hit = startInside | endInside; + fracEnter = math.select(fracExit, fracEnter, hit); // mark invalid with enter <= exit in case of a miss + } + + // If the rejections of the faces from the contact normal are just barely touching, then FaceFace() might miss the closest points because of numerical error. + // FaceFace() and FaceEdge() check if they found a point as close as the closest, and if not they return false so that the caller can add it. + private const float closestDistanceTolerance = 1e-4f; + + // Tries to generate a manifold between a pair of faces. It can fail in some cases due to numerical accuracy: + // 1) both faces are nearly perpendicular to the normal + // 2) the closest features on the shapes are vertices, so that the intersection of the projection of the faces to the plane perpendicular to the normal contains only one point + // In those cases, FaceFace() returns false and the caller should generate a contact from the closest points on the shapes. + private static unsafe bool FaceFace( + ref ConvexHull convexA, ref ConvexHull convexB, int faceIndexA, int faceIndexB, MTransform worldFromA, MTransform aFromB, + float3 normal, float distance, ref Manifold manifold) + { + // Get the plane of each face + Plane planeA = convexA.Planes[faceIndexA]; + Plane planeB = TransformPlane(aFromB, convexB.Planes[faceIndexB]); + + // Handle cases where one of the faces is nearly perpendicular to the contact normal + // This gets around divide by zero / numerical problems from dividing collider planes which often contain some error by a very small number, amplifying that error + const float cosMaxAngle = 0.05f; + float dotA = math.dot(planeA.Normal, normal); + float dotB = math.dot(planeB.Normal, normal); + bool acceptB = true; // true if vertices of B projected onto the face of A are accepted + if (dotA > -cosMaxAngle) + { + // Handle cases where both faces are nearly perpendicular to the contact normal. + if (dotB < cosMaxAngle) + { + // Both faces are nearly perpendicular to the contact normal, let the caller generate a single contact + return false; + } + + // Face of A is nearly perpendicular to the contact normal, don't try to project vertices onto it + acceptB = false; + } + else if (dotB < cosMaxAngle) + { + // Face of B is nearly perpendicular to the normal, so we need to clip the edges of B against face A instead + MTransform bFromA = Inverse(aFromB); + float3 normalInB = math.mul(bFromA.Rotation, -normal); + MTransform worldFromB = Mul(worldFromA, aFromB); + bool result = FaceFace(ref convexB, ref convexA, faceIndexB, faceIndexA, worldFromB, bFromA, normalInB, distance, ref manifold); + manifold.Normal = -manifold.Normal; + manifold.Flip(); + return result; + } + + // Check if the manifold gets a point roughly as close as the closest + distance += closestDistanceTolerance; + bool foundClosestPoint = false; + + // Transform vertices of B into A-space + // Initialize validB, which is true for each vertex of B that is inside face A + ConvexHull.Face faceA = convexA.Faces[faceIndexA]; + ConvexHull.Face faceB = convexB.Faces[faceIndexB]; + bool* validB = stackalloc bool[faceB.NumVertices]; + float3* verticesBinA = stackalloc float3[faceB.NumVertices]; + { + byte* indicesB = convexB.FaceVertexIndicesPtr + faceB.FirstIndex; + float3* verticesB = convexB.VerticesPtr; + for (int i = 0; i < faceB.NumVertices; i++) + { + validB[i] = acceptB; + verticesBinA[i] = Mul(aFromB, verticesB[indicesB[i]]); + } + } + + // For each edge of A + float invDotB = math.rcp(dotB); + float sumRadii = convexA.ConvexRadius + convexB.ConvexRadius; + byte* indicesA = convexA.FaceVertexIndicesPtr + faceA.FirstIndex; + float3* verticesA = convexA.VerticesPtr; + for (EdgeIterator edgeA = EdgeIterator.Begin(verticesA, indicesA, -normal, faceA.NumVertices); edgeA.Valid(); edgeA.Advance()) + { + float fracEnterA = 0.0f; + float fracExitA = 1.0f; + + // For each edge of B + for (EdgeIterator edgeB = EdgeIterator.Begin(verticesBinA, null, normal, faceB.NumVertices); edgeB.Valid(); edgeB.Advance()) + { + // Cast edge A against plane B and test if vertex B is inside plane A + castRayPlane(edgeA.Vertex0, edgeA.Edge, edgeB.Perp, edgeB.Offset, ref fracEnterA, ref fracExitA); + validB[edgeB.Index] &= (math.dot(edgeB.Vertex1, edgeA.Perp) < edgeA.Offset); + } + + // If edge A hits B, add a contact points + if (fracEnterA < fracExitA) + { + float distance0 = (math.dot(edgeA.Vertex0, planeB.Normal) + planeB.Distance) * invDotB; + float deltaDistance = math.dot(edgeA.Edge, planeB.Normal) * invDotB; + float3 vertexAOnB = edgeA.Vertex0 - normal * distance0; + float3 edgeAOnB = edgeA.Edge - normal * deltaDistance; + foundClosestPoint |= AddEdgeContact(vertexAOnB, edgeAOnB, distance0, deltaDistance, fracEnterA, normal, convexB.ConvexRadius, sumRadii, worldFromA, distance, ref manifold); + if (fracExitA < 1.0f) // If the exit fraction is 1, then the next edge has the same contact point with enter fraction 0 + { + foundClosestPoint |= AddEdgeContact(vertexAOnB, edgeAOnB, distance0, deltaDistance, fracExitA, normal, convexB.ConvexRadius, sumRadii, worldFromA, distance, ref manifold); + } + } + } + + // For each vertex of B + float invDotA = math.rcp(dotA); + for (int i = 0; i < faceB.NumVertices; i++) + { + if (validB[i] && manifold.NumContacts < Manifold.k_MaxNumContacts) + { + float3 vertexB = verticesBinA[i]; + float distanceB = (math.dot(vertexB, planeA.Normal) + planeA.Distance) * -invDotA; + manifold[manifold.NumContacts++] = new ContactPoint + { + Position = Mul(worldFromA, vertexB) + manifold.Normal * convexB.ConvexRadius, + Distance = distanceB - sumRadii + }; + foundClosestPoint |= distanceB <= distance; + } + } + + return foundClosestPoint; + } + + // Tries to generate a manifold between a face and an edge. It can fail for the same reasons as FaceFace(). + // In those cases, FaceEdge() returns false and the caller should generate a contact from the closest points on the shapes. + private static unsafe bool FaceEdge( + ref ConvexHull faceConvexA, ref ConvexHull edgeConvexB, int faceIndexA, MTransform worldFromA, MTransform aFromB, + float3 normal, float distance, ref Manifold manifold) + { + // Check if the face is nearly perpendicular to the normal + const float cosMaxAngle = 0.05f; + Plane planeA = faceConvexA.Planes[faceIndexA]; + float dotA = math.dot(planeA.Normal, normal); + if (math.abs(dotA) < cosMaxAngle) + { + return false; + } + + // Check if the manifold gets a point roughly as close as the closest + distance += closestDistanceTolerance; + bool foundClosestPoint = false; + + // Get the supporting face on A + ConvexHull.Face faceA = faceConvexA.Faces[faceIndexA]; + byte* indicesA = faceConvexA.FaceVertexIndicesPtr + faceA.FirstIndex; + + // Get edge in B + float3 vertexB0 = Math.Mul(aFromB, edgeConvexB.Vertices[0]); + float3 edgeB = math.mul(aFromB.Rotation, edgeConvexB.Vertices[1] - edgeConvexB.Vertices[0]); + + // For each edge of A + float3* verticesA = faceConvexA.VerticesPtr; + float fracEnterB = 0.0f; + float fracExitB = 1.0f; + for (EdgeIterator edgeA = EdgeIterator.Begin(verticesA, indicesA, -normal, faceA.NumVertices); edgeA.Valid(); edgeA.Advance()) + { + // Cast edge B against plane A + castRayPlane(vertexB0, edgeB, edgeA.Perp, edgeA.Offset, ref fracEnterB, ref fracExitB); + } + + // If edge B hits A, add a contact points + if (fracEnterB < fracExitB) + { + float invDotA = math.rcp(dotA); + float sumRadii = faceConvexA.ConvexRadius + edgeConvexB.ConvexRadius; + float distance0 = (math.dot(vertexB0, planeA.Normal) + planeA.Distance) * -invDotA; + float deltaDistance = math.dot(edgeB, planeA.Normal) * -invDotA; + foundClosestPoint |= AddEdgeContact(vertexB0, edgeB, distance0, deltaDistance, fracEnterB, normal, edgeConvexB.ConvexRadius, sumRadii, worldFromA, distance, ref manifold); + foundClosestPoint |= AddEdgeContact(vertexB0, edgeB, distance0, deltaDistance, fracExitB, normal, edgeConvexB.ConvexRadius, sumRadii, worldFromA, distance, ref manifold); + } + + return foundClosestPoint; + } + + // Adds a contact to the manifold from an edge and fraction + private static bool AddEdgeContact(float3 vertex0, float3 edge, float distance0, float deltaDistance, float fraction, float3 normalInA, float radiusB, float sumRadii, + MTransform worldFromA, float distanceThreshold, ref Manifold manifold) + { + if (manifold.NumContacts < Manifold.k_MaxNumContacts) + { + float3 position = vertex0 + fraction * edge; + float distance = distance0 + fraction * deltaDistance; + + manifold[manifold.NumContacts++] = new ContactPoint + { + Position = Mul(worldFromA, position + normalInA * radiusB), + Distance = distance - sumRadii + }; + + return distance <= distanceThreshold; + } + return false; + } + + #endregion + } +} diff --git a/package/Unity.Physics/Collision/Queries/ConvexConvexManifold.cs.meta b/package/Unity.Physics/Collision/Queries/ConvexConvexManifold.cs.meta new file mode 100755 index 000000000..fecafac51 --- /dev/null +++ b/package/Unity.Physics/Collision/Queries/ConvexConvexManifold.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 2885c4bd4a7b7c84792883bae410ae3b +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Unity.Physics/Collision/Queries/Distance.cs b/package/Unity.Physics/Collision/Queries/Distance.cs new file mode 100755 index 000000000..95a4a07c0 --- /dev/null +++ b/package/Unity.Physics/Collision/Queries/Distance.cs @@ -0,0 +1,908 @@ +using System; +using Unity.Mathematics; +using UnityEngine.Assertions; +using static Unity.Physics.BoundingVolumeHierarchy; +using static Unity.Physics.Math; + +namespace Unity.Physics +{ + // The input to point distance queries + public struct PointDistanceInput + { + public float3 Position; + public float MaxDistance; + public CollisionFilter Filter; + } + + // The input to collider distance queries + public unsafe struct ColliderDistanceInput + { + public Collider* Collider; + public RigidTransform Transform; + public float MaxDistance; + } + + // A hit from a distance query + public struct DistanceHit : IQueryResult + { + public float Fraction { get; set; } + public float Distance => Fraction; + public float3 Position; + public float3 SurfaceNormal; + public int RigidBodyIndex; + public ColliderKey ColliderKey; + + public void Transform(MTransform transform, uint numSubKeyBits, uint subKey) + { + Position = Mul(transform, Position); + SurfaceNormal = math.mul(transform.Rotation, SurfaceNormal); + ColliderKey.PushSubKey(numSubKeyBits, subKey); + } + + public void Transform(MTransform transform, int rigidBodyIndex) + { + Position = Mul(transform, Position); + SurfaceNormal = math.mul(transform.Rotation, SurfaceNormal); + RigidBodyIndex = rigidBodyIndex; + } + } + + // Distance query implementations + public static class DistanceQueries + { + // Distance queries have edge cases where distance = 0, eg. consider choosing the correct normal for a point that is exactly on a triangle surface. + // Additionally, with floating point numbers there are often numerical accuracy problems near distance = 0. Some routines handle this with special + // cases where distance^2 < distanceEpsSq, which is expected to be rare in normal usage. distanceEpsSq is not an exact value, but chosen to be small + // enough that at typical simulation scale the difference between distance = distanceEps and distance = 0 is negligible. + private const float distanceEpsSq = 1e-8f; + + public struct Result + { + public float3 PositionOnAinA; + public float3 NormalInA; + public float Distance; + + public float3 PositionOnBinA => PositionOnAinA - NormalInA * Distance; + } + + public static unsafe Result ConvexConvex(ref ConvexHull convexA, ref ConvexHull convexB, MTransform aFromB) + { + return ConvexConvex( + convexA.VerticesPtr, convexA.NumVertices, convexA.ConvexRadius, + convexB.VerticesPtr, convexB.NumVertices, convexB.ConvexRadius, + aFromB); + } + + public static unsafe Result ConvexConvex( + float3* verticesA, int numVerticesA, float convexRadiusA, + float3* verticesB, int numVerticesB, float convexRadiusB, + MTransform aFromB) + { + ConvexConvexDistanceQueries.Result result = ConvexConvexDistanceQueries.ConvexConvex( + verticesA, numVerticesA, verticesB, numVerticesB, aFromB, ConvexConvexDistanceQueries.PenetrationHandling.Exact3D); + + // Adjust for convex radii + result.ClosestPoints.Distance -= (convexRadiusA + convexRadiusB); + result.ClosestPoints.PositionOnAinA -= result.ClosestPoints.NormalInA * convexRadiusA; + return result.ClosestPoints; + } + + private static Result PointPoint(float3 pointB, float3 diff, float coreDistanceSq, float radiusA, float sumRadii) + { + bool distanceZero = coreDistanceSq == 0.0f; + float invCoreDistance = math.select(math.rsqrt(coreDistanceSq), 0.0f, distanceZero); + float3 normal = math.select(diff * invCoreDistance, new float3(0, 1, 0), distanceZero); // choose an arbitrary normal when the distance is zero + float distance = coreDistanceSq * invCoreDistance; + return new Result + { + NormalInA = normal, + PositionOnAinA = pointB + normal * (distance - radiusA), + Distance = distance - sumRadii + }; + } + + public static Result PointPoint(float3 pointA, float3 pointB, float radiusA, float sumRadii) + { + float3 diff = pointA - pointB; + float coreDistanceSq = math.lengthsq(diff); + return PointPoint(pointB, diff, coreDistanceSq, radiusA, sumRadii); + } + + public static unsafe Result SphereSphere(SphereCollider* sphereA, SphereCollider* sphereB, MTransform aFromB) + { + float3 posBinA = Mul(aFromB, sphereB->Center); + return PointPoint(sphereA->Center, posBinA, sphereA->Radius, sphereA->Radius + sphereB->Radius); + } + + public static unsafe Result BoxSphere(BoxCollider* boxA, SphereCollider* sphereB, MTransform aFromB) + { + MTransform aFromBoxA = new MTransform(boxA->Orientation, boxA->Center); + float3 posBinA = Mul(aFromB, sphereB->Center); + float3 posBinBoxA = Mul(Inverse(aFromBoxA), posBinA); + float3 innerHalfExtents = boxA->Size * 0.5f - boxA->ConvexRadius; + float3 normalInBoxA; + float distance; + { + // from hkAabb::signedDistanceToPoint(), can optimize a lot + float3 projection = math.min(posBinBoxA, innerHalfExtents); + projection = math.max(projection, -innerHalfExtents); + float3 difference = projection - posBinBoxA; + float distanceSquared = math.lengthsq(difference); + + // Check if the sphere center is inside the box + if (distanceSquared < 1e-6f) + { + float3 projectionLocal = projection; + float3 absProjectionLocal = math.abs(projectionLocal); + float3 del = absProjectionLocal - innerHalfExtents; + int axis = IndexOfMaxComponent(new float4(del, -float.MaxValue)); + switch (axis) + { + case 0: normalInBoxA = new float3(projectionLocal.x < 0.0f ? 1.0f : -1.0f, 0.0f, 0.0f); break; + case 1: normalInBoxA = new float3(0.0f, projectionLocal.y < 0.0f ? 1.0f : -1.0f, 0.0f); break; + case 2: normalInBoxA = new float3(0.0f, 0.0f, projectionLocal.z < 0.0f ? 1.0f : -1.0f); break; + default: + normalInBoxA = new float3(1, 0, 0); + Assert.IsTrue(false); + break; + } + distance = math.max(del.x, math.max(del.y, del.z)); + } + else + { + float invDistance = math.rsqrt(distanceSquared); + normalInBoxA = difference * invDistance; + distance = distanceSquared * invDistance; + } + } + + float3 normalInA = math.mul(aFromBoxA.Rotation, normalInBoxA); + return new Result + { + NormalInA = normalInA, + PositionOnAinA = posBinA + normalInA * (distance - boxA->ConvexRadius), + Distance = distance - (sphereB->Radius + boxA->ConvexRadius) + }; + } + + public static Result CapsuleSphere( + float3 capsuleVertex0, float3 capsuleVertex1, float capsuleRadius, + float3 sphereCenter, float sphereRadius, + MTransform aFromB) + { + // Transform the sphere into capsule space + float3 centerB = Mul(aFromB, sphereCenter); + + // Point-segment distance + float3 edgeA = capsuleVertex1 - capsuleVertex0; + float dot = math.dot(edgeA, centerB - capsuleVertex0); + float edgeLengthSquared = math.lengthsq(edgeA); + dot = math.max(dot, 0.0f); + dot = math.min(dot, edgeLengthSquared); + float invEdgeLengthSquared = 1.0f / edgeLengthSquared; + float frac = dot * invEdgeLengthSquared; + float3 pointOnA = capsuleVertex0 + edgeA * frac; + return PointPoint(pointOnA, centerB, capsuleRadius, capsuleRadius + sphereRadius); + } + + // Find the closest points on a pair of line segments + private static void SegmentSegment(float3 pointA, float3 edgeA, float3 pointB, float3 edgeB, out float3 closestAOut, out float3 closestBOut) + { + // Find the closest point on edge A to the line containing edge B + float3 diff = pointB - pointA; + + float r = math.dot(edgeA, edgeB); + float s1 = math.dot(edgeA, diff); + float s2 = math.dot(edgeB, diff); + float lengthASq = math.lengthsq(edgeA); + float lengthBSq = math.lengthsq(edgeB); + + float invDenom, invLengthASq, invLengthBSq; + { + float denom = lengthASq * lengthBSq - r * r; + float3 inv = 1.0f / new float3(denom, lengthASq, lengthBSq); + invDenom = inv.x; + invLengthASq = inv.y; + invLengthBSq = inv.z; + } + + float fracA = (s1 * lengthBSq - s2 * r) * invDenom; + fracA = math.clamp(fracA, 0.0f, 1.0f); + + // Find the closest point on edge B to the point on A just found + float fracB = fracA * (invLengthBSq * r) - invLengthBSq * s2; + fracB = math.clamp(fracB, 0.0f, 1.0f); + + // If the point on B was clamped then there may be a closer point on A to the edge + fracA = fracB * (invLengthASq * r) + invLengthASq * s1; + fracA = math.clamp(fracA, 0.0f, 1.0f); + + closestAOut = pointA + fracA * edgeA; + closestBOut = pointB + fracB * edgeB; + } + + public static unsafe Result CapsuleCapsule(CapsuleCollider* capsuleA, CapsuleCollider* capsuleB, MTransform aFromB) + { + // Transform capsule B into A-space + float3 pointB = Mul(aFromB, capsuleB->Vertex0); + float3 edgeB = math.mul(aFromB.Rotation, capsuleB->Vertex1 - capsuleB->Vertex0); + + // Get point and edge of A + float3 pointA = capsuleA->Vertex0; + float3 edgeA = capsuleA->Vertex1 - capsuleA->Vertex0; + + // Get the closest points on the capsules + SegmentSegment(pointA, edgeA, pointB, edgeB, out float3 closestA, out float3 closestB); + float3 diff = closestA - closestB; + float coreDistanceSq = math.lengthsq(diff); + if (coreDistanceSq < distanceEpsSq) + { + // Special case for extremely small distances, should be rare + float3 normal = math.cross(edgeA, edgeB); + if (math.lengthsq(normal) < 1e-5f) + { + float3 edge = math.normalizesafe(edgeA, math.normalizesafe(edgeB, new float3(1, 0, 0))); // edges are parallel or one of the capsules is a sphere + Math.CalculatePerpendicularNormalized(edge, out normal, out float3 unused); // normal is anything perpendicular to edge + } + else + { + normal = math.normalize(normal); // normal is cross of edges, sign doesn't matter + } + return new Result + { + NormalInA = normal, + PositionOnAinA = pointA - normal * capsuleA->Radius, + Distance = -capsuleA->Radius - capsuleB->Radius + }; + } + return PointPoint(closestA, closestB, capsuleA->Radius, capsuleA->Radius + capsuleB->Radius); + } + + private static void CalcTrianglePlanes(float3 v0, float3 v1, float3 v2, float3 normalDirection, + out FourTransposedPoints verts, out FourTransposedPoints edges, out FourTransposedPoints perps) + { + verts = new FourTransposedPoints(v0, v1, v2, v0); + edges = verts.V1230 - verts; + perps = edges.Cross(new FourTransposedPoints(normalDirection)); + } + + // Checks if the closest point on the triangle is on its face. If so returns true and sets signedDistance to the distance along the normal, otherwise returns false + private static bool PointTriangleFace(float3 point, float3 v0, float3 normal, + FourTransposedPoints verts, FourTransposedPoints edges, FourTransposedPoints perps, FourTransposedPoints rels, out float signedDistance) + { + float4 dots = perps.Dot(rels); + float4 dotsSq = dots * math.abs(dots); + float4 perpLengthSq = perps.Dot(perps); + if (math.all(dotsSq <= perpLengthSq * distanceEpsSq)) + { + // Closest point on face + signedDistance = math.dot(v0 - point, normal); + return true; + } + + signedDistance = float.MaxValue; + return false; + } + + public static Result TriangleSphere( + float3 vertex0, float3 vertex1, float3 vertex2, float3 normal, + float3 sphereCenter, float sphereRadius, + MTransform aFromB) + { + // Sphere center in A-space (TODO.ma should probably work in sphere space, typical for triangle verts to be far from the origin in its local space) + float3 pointB = Mul(aFromB, sphereCenter); + + // Calculate triangle edges and edge planes + FourTransposedPoints vertsA; + FourTransposedPoints edgesA; + FourTransposedPoints perpsA; + CalcTrianglePlanes(vertex0, vertex1, vertex2, normal, out vertsA, out edgesA, out perpsA); + + // Check if the closest point is on the triangle face + FourTransposedPoints rels = new FourTransposedPoints(pointB) - vertsA; + if (PointTriangleFace(pointB, vertex0, normal, vertsA, edgesA, perpsA, rels, out float signedDistance)) + { + return new Result + { + PositionOnAinA = pointB + normal * signedDistance, + NormalInA = math.select(normal, -normal, signedDistance < 0), + Distance = math.abs(signedDistance) - sphereRadius + }; + } + + // Find the closest point on the triangle edges - project point onto the line through each edge, then clamp to the edge + float4 nums = rels.Dot(edgesA); + float4 dens = edgesA.Dot(edgesA); + float4 sols = math.clamp(nums / dens, 0.0f, 1.0f); // fraction along the edge TODO.ma see how it handles inf/nan from divide by zero + FourTransposedPoints projs = edgesA.MulT(sols) - rels; + float4 distancesSq = projs.Dot(projs); + + float3 proj0 = projs.GetPoint(0); + float3 proj1 = projs.GetPoint(1); + float3 proj2 = projs.GetPoint(2); + + // Find the closest projected point + bool less1 = distancesSq.x < distancesSq.y; + float3 direction = math.select(proj1, proj0, less1); + float distanceSq = math.select(distancesSq.y, distancesSq.x, less1); + + bool less2 = distanceSq < distancesSq.z; + direction = math.select(proj2, direction, less2); + distanceSq = math.select(distancesSq.z, distanceSq, less2); + + const float triangleConvexRadius = 0.0f; + return PointPoint(pointB, direction, distanceSq, triangleConvexRadius, sphereRadius); + } + + public static Result QuadSphere( + float3 vertex0, float3 vertex1, float3 vertex2, float3 vertex3, float3 normalDirection, + float3 sphereCenter, float sphereRadius, + MTransform aFromB) + { + // TODO: Do this in one pass + Result result1 = TriangleSphere(vertex0, vertex1, vertex2, normalDirection, sphereCenter, sphereRadius, aFromB); + Result result2 = TriangleSphere(vertex0, vertex2, vertex3, normalDirection, sphereCenter, sphereRadius, aFromB); + return result1.Distance < result2.Distance ? result1 : result2; + } + + // given two (normal, distance) pairs, select the one with smaller distance + private static void SelectMin(ref float3 dirInOut, ref float distInOut, ref float3 posInOut, float3 newDir, float newDist, float3 newPos) + { + bool less = newDist < distInOut; + dirInOut = math.select(dirInOut, newDir, less); + distInOut = math.select(distInOut, newDist, less); + posInOut = math.select(posInOut, newPos, less); + } + + public static unsafe Result CapsuleTriangle(CapsuleCollider* capsuleA, PolygonCollider* triangleB, MTransform aFromB) + { + // Get vertices + float3 c0 = capsuleA->Vertex0; + float3 c1 = capsuleA->Vertex1; + float3 t0 = Mul(aFromB, triangleB->ConvexHull.Vertices[0]); + float3 t1 = Mul(aFromB, triangleB->ConvexHull.Vertices[1]); + float3 t2 = Mul(aFromB, triangleB->ConvexHull.Vertices[2]); + + float3 direction; + float distanceSq; + float3 pointCapsule; + float sign = 1.0f; // negated if penetrating + { + // Calculate triangle edges and edge planes + float3 faceNormal = math.mul(aFromB.Rotation, triangleB->ConvexHull.Planes[0].Normal); + FourTransposedPoints vertsB; + FourTransposedPoints edgesB; + FourTransposedPoints perpsB; + CalcTrianglePlanes(t0, t1, t2, faceNormal, out vertsB, out edgesB, out perpsB); + + // c0 against triangle face + { + FourTransposedPoints rels = new FourTransposedPoints(c0) - vertsB; + PointTriangleFace(c0, t0, faceNormal, vertsB, edgesB, perpsB, rels, out float signedDistance); + distanceSq = signedDistance * signedDistance; + if (distanceSq > distanceEpsSq) + { + direction = -faceNormal * signedDistance; + } + else + { + direction = math.select(faceNormal, -faceNormal, math.dot(c1 - c0, faceNormal) < 0); // rare case, capsule point is exactly on the triangle face + } + pointCapsule = c0; + } + + // c1 against triangle face + { + FourTransposedPoints rels = new FourTransposedPoints(c1) - vertsB; + PointTriangleFace(c1, t0, faceNormal, vertsB, edgesB, perpsB, rels, out float signedDistance); + float distanceSq1 = signedDistance * signedDistance; + float3 direction1; + if (distanceSq1 > distanceEpsSq) + { + direction1 = -faceNormal * signedDistance; + } + else + { + direction1 = math.select(faceNormal, -faceNormal, math.dot(c0 - c1, faceNormal) < 0); // rare case, capsule point is exactly on the triangle face + } + SelectMin(ref direction, ref distanceSq, ref pointCapsule, direction1, distanceSq1, c1); + } + + // axis against triangle edges + float3 axis = c1 - c0; + for (int i = 0; i < 3; i++) + { + float3 closestOnCapsule, closestOnTriangle; + SegmentSegment(c0, axis, vertsB.GetPoint(i), edgesB.GetPoint(i), out closestOnCapsule, out closestOnTriangle); + float3 edgeDiff = closestOnCapsule - closestOnTriangle; + float edgeDistanceSq = math.lengthsq(edgeDiff); + edgeDiff = math.select(edgeDiff, perpsB.GetPoint(i), edgeDistanceSq < distanceEpsSq); // use edge plane if the capsule axis intersects the edge + SelectMin(ref direction, ref distanceSq, ref pointCapsule, edgeDiff, edgeDistanceSq, closestOnCapsule); + } + + // axis against triangle face + { + // Find the intersection of the axis with the triangle plane + float axisDot = math.dot(axis, faceNormal); + float dist0 = math.dot(t0 - c0, faceNormal); // distance from c0 to the plane along the normal + float t = dist0 * math.select(math.rcp(axisDot), 0.0f, axisDot == 0.0f); + if (t > 0.0f && t < 1.0f) + { + // If they intersect, check if the intersection is inside the triangle + FourTransposedPoints rels = new FourTransposedPoints(c0 + axis * t) - vertsB; + float4 dots = perpsB.Dot(rels); + if (math.all(dots <= float4.zero)) + { + // Axis intersects the triangle, choose the separating direction + float dist1 = axisDot - dist0; + bool use1 = math.abs(dist1) < math.abs(dist0); + float dist = math.select(-dist0, dist1, use1); + float3 closestOnCapsule = math.select(c0, c1, use1); + SelectMin(ref direction, ref distanceSq, ref pointCapsule, dist * faceNormal, dist * dist, closestOnCapsule); + + // Even if the edge is closer than the face, we now know that the edge hit was penetrating + sign = -1.0f; + } + } + } + } + + float invDistance = math.rsqrt(distanceSq); + float distance; + float3 normal; + if (distanceSq < distanceEpsSq) + { + normal = math.normalize(direction); // rare case, capsule axis almost exactly touches the triangle + distance = 0.0f; + } + else + { + normal = direction * invDistance * sign; // common case, distanceSq = lengthsq(direction) + distance = distanceSq * invDistance * sign; + } + return new Result + { + NormalInA = normal, + PositionOnAinA = pointCapsule - normal * capsuleA->Radius, + Distance = distance - capsuleA->Radius + }; + } + + // Dispatch any pair of convex colliders + public static unsafe Result ConvexConvex(Collider* convexA, Collider* convexB, MTransform aFromB) + { + Result result; + bool flip = false; + switch (convexA->Type) + { + case ColliderType.Sphere: + SphereCollider* sphereA = (SphereCollider*)convexA; + switch (convexB->Type) + { + case ColliderType.Sphere: + result = SphereSphere(sphereA, (SphereCollider*)convexB, aFromB); + break; + case ColliderType.Capsule: + CapsuleCollider* capsuleB = (CapsuleCollider*)convexB; + result = CapsuleSphere(capsuleB->Vertex0, capsuleB->Vertex1, capsuleB->Radius, sphereA->Center, sphereA->Radius, Inverse(aFromB)); + flip = true; + break; + case ColliderType.Triangle: + PolygonCollider* triangleB = (PolygonCollider*)convexB; + result = TriangleSphere( + triangleB->Vertices[0], triangleB->Vertices[1], triangleB->Vertices[2], triangleB->Planes[0].Normal, + sphereA->Center, sphereA->Radius, Inverse(aFromB)); + flip = true; + break; + case ColliderType.Quad: + PolygonCollider* quadB = (PolygonCollider*)convexB; + result = QuadSphere( + quadB->Vertices[0], quadB->Vertices[1], quadB->Vertices[2], quadB->Vertices[3], quadB->Planes[0].Normal, + sphereA->Center, sphereA->Radius, Inverse(aFromB)); + flip = true; + break; + case ColliderType.Box: + result = BoxSphere((BoxCollider*)convexB, sphereA, Inverse(aFromB)); + flip = true; + break; + case ColliderType.Convex: + result = ConvexConvex(ref sphereA->ConvexHull, ref ((ConvexCollider*)convexB)->ConvexHull, aFromB); + break; + default: + throw new NotImplementedException(); + } + break; + case ColliderType.Capsule: + CapsuleCollider* capsuleA = (CapsuleCollider*)convexA; + switch (convexB->Type) + { + case ColliderType.Sphere: + SphereCollider* sphereB = (SphereCollider*)convexB; + result = CapsuleSphere(capsuleA->Vertex0, capsuleA->Vertex1, capsuleA->Radius, sphereB->Center, sphereB->Radius, aFromB); + break; + case ColliderType.Capsule: + result = CapsuleCapsule(capsuleA, (CapsuleCollider*)convexB, aFromB); + break; + case ColliderType.Triangle: + result = CapsuleTriangle(capsuleA, (PolygonCollider*)convexB, aFromB); + break; + case ColliderType.Box: + case ColliderType.Quad: + case ColliderType.Convex: + result = ConvexConvex(ref capsuleA->ConvexHull, ref ((ConvexCollider*)convexB)->ConvexHull, aFromB); + break; + default: + throw new NotImplementedException(); + } + break; + case ColliderType.Triangle: + PolygonCollider* triangleA = (PolygonCollider*)convexA; + switch (convexB->Type) + { + case ColliderType.Sphere: + SphereCollider* sphereB = (SphereCollider*)convexB; + result = TriangleSphere( + triangleA->Vertices[0], triangleA->Vertices[1], triangleA->Vertices[2], triangleA->Planes[0].Normal, + sphereB->Center, sphereB->Radius, aFromB); + break; + case ColliderType.Capsule: + result = CapsuleTriangle((CapsuleCollider*)convexB, triangleA, Inverse(aFromB)); + flip = true; + break; + case ColliderType.Box: + case ColliderType.Triangle: + case ColliderType.Quad: + case ColliderType.Convex: + result = ConvexConvex(ref triangleA->ConvexHull, ref ((ConvexCollider*)convexB)->ConvexHull, aFromB); + break; + default: + throw new NotImplementedException(); + } + break; + case ColliderType.Box: + BoxCollider* boxA = (BoxCollider*)convexA; + switch (convexB->Type) + { + case ColliderType.Sphere: + result = BoxSphere(boxA, (SphereCollider*)convexB, aFromB); + break; + case ColliderType.Capsule: + case ColliderType.Box: + case ColliderType.Triangle: + case ColliderType.Quad: + case ColliderType.Convex: + result = ConvexConvex(ref boxA->ConvexHull, ref ((ConvexCollider*)convexB)->ConvexHull, aFromB); + break; + default: + throw new NotImplementedException(); + } + break; + case ColliderType.Quad: + case ColliderType.Convex: + result = ConvexConvex(ref ((ConvexCollider*)convexA)->ConvexHull, ref ((ConvexCollider*)convexB)->ConvexHull, aFromB); + break; + default: + throw new NotImplementedException(); + } + + if (flip) + { + result.PositionOnAinA = Mul(aFromB, result.PositionOnBinA); + result.NormalInA = math.mul(aFromB.Rotation, -result.NormalInA); + } + + return result; + } + + public static unsafe bool PointCollider(PointDistanceInput input, Collider* target, ref T collector) where T : struct, ICollector + { + if (!CollisionFilter.IsCollisionEnabled(input.Filter, target->Filter)) + { + return false; + } + + Result result; + switch (target->Type) + { + case ColliderType.Sphere: + var sphere = (SphereCollider*)target; + result = PointPoint(sphere->Center, input.Position, sphere->Radius, sphere->Radius); + break; + case ColliderType.Capsule: + var capsule = (CapsuleCollider*)target; + result = CapsuleSphere(capsule->Vertex0, capsule->Vertex1, capsule->Radius, input.Position, 0.0f, MTransform.Identity); + break; + case ColliderType.Triangle: + var triangle = (PolygonCollider*)target; + result = TriangleSphere( + triangle->Vertices[0], triangle->Vertices[1], triangle->Vertices[2], triangle->Planes[0].Normal, + input.Position, 0.0f, MTransform.Identity); + break; + case ColliderType.Quad: + var quad = (PolygonCollider*)target; + result = QuadSphere( + quad->Vertices[0], quad->Vertices[1], quad->Vertices[2], quad->Vertices[3], quad->Planes[0].Normal, + input.Position, 0.0f, MTransform.Identity); + break; + case ColliderType.Convex: + case ColliderType.Box: + ref ConvexHull hull = ref ((ConvexCollider*)target)->ConvexHull; + result = ConvexConvex(hull.VerticesPtr, hull.NumVertices, hull.ConvexRadius, &input.Position, 1, 0.0f, MTransform.Identity); + break; + case ColliderType.Mesh: + return PointMesh(input, (MeshCollider*)target, ref collector); + case ColliderType.Compound: + return PointCompound(input, (CompoundCollider*)target, ref collector); + default: + throw new NotImplementedException(); + } + + if (result.Distance < collector.MaxFraction) + { + collector.AddHit(new DistanceHit + { + Fraction = result.Distance, + SurfaceNormal = -result.NormalInA, + Position = result.PositionOnAinA, + ColliderKey = ColliderKey.Empty + }); + return true; + } + return false; + } + + public static unsafe bool ColliderCollider(ColliderDistanceInput input, Collider* target, ref T collector) where T : struct, ICollector + { + if (!CollisionFilter.IsCollisionEnabled(input.Collider->Filter, target->Filter)) + { + return false; + } + + switch (input.Collider->CollisionType) + { + case CollisionType.Convex: + switch (target->Type) + { + case ColliderType.Convex: + case ColliderType.Sphere: + case ColliderType.Capsule: + case ColliderType.Triangle: + case ColliderType.Quad: + case ColliderType.Box: + MTransform targetFromQuery = new MTransform(input.Transform); + Result result = ConvexConvex(target, input.Collider, targetFromQuery); + if (result.Distance < collector.MaxFraction) + { + collector.AddHit(new DistanceHit + { + Fraction = result.Distance, + SurfaceNormal = -result.NormalInA, + Position = result.PositionOnAinA, + ColliderKey = ColliderKey.Empty + }); + return true; + } + return false; + case ColliderType.Mesh: + return ConvexMesh(input, (MeshCollider*)target, ref collector); + case ColliderType.Compound: + return ConvexCompound(input, (CompoundCollider*)target, ref collector); + default: + throw new NotImplementedException(); + } + case CollisionType.Composite: + // no support for composite query shapes + throw new NotImplementedException(); + default: + throw new NotImplementedException(); + } + } + + private unsafe struct ConvexMeshLeafProcessor : IPointDistanceLeafProcessor, IColliderDistanceLeafProcessor + { + private readonly Mesh* m_Mesh; + private readonly uint m_NumColliderKeyBits; + + public ConvexMeshLeafProcessor(MeshCollider* meshCollider) + { + m_Mesh = &meshCollider->Mesh; + m_NumColliderKeyBits = meshCollider->NumColliderKeyBits; + } + + public bool DistanceLeaf(PointDistanceInput input, int primitiveKey, ref T collector) + where T : struct, ICollector + { + m_Mesh->GetPrimitive(primitiveKey, out float3x4 vertices, out Mesh.PrimitiveFlags flags, out CollisionFilter filter); + + if (!CollisionFilter.IsCollisionEnabled(input.Filter, filter)) // TODO: could do this check within GetPrimitive() + { + return false; + } + + int numPolygons = Mesh.GetNumPolygonsInPrimitive(flags); + bool isQuad = Mesh.IsPrimitveFlagSet(flags, Mesh.PrimitiveFlags.IsQuad); + + float3 normalDirection = math.cross(vertices[1] - vertices[0], vertices[2] - vertices[0]); + bool acceptHit = false; + + for (int polygonIndex = 0; polygonIndex < numPolygons; polygonIndex++) + { + Result result; + if (isQuad) + { + result = QuadSphere( + vertices[0], vertices[1], vertices[2], vertices[3], normalDirection, + input.Position, 0.0f, MTransform.Identity); + } + else + { + result = TriangleSphere( + vertices[0], vertices[1], vertices[2], normalDirection, + input.Position, 0.0f, MTransform.Identity); + } + + if (result.Distance < collector.MaxFraction) + { + acceptHit |= collector.AddHit(new DistanceHit + { + Fraction = result.Distance, + Position = result.PositionOnAinA, + SurfaceNormal = -result.NormalInA, + ColliderKey = new ColliderKey(m_NumColliderKeyBits, (uint)(primitiveKey << 1 | polygonIndex)) + }); + } + } + + return acceptHit; + } + + public bool DistanceLeaf(ColliderDistanceInput input, int primitiveKey, ref T collector) + where T : struct, ICollector + { + m_Mesh->GetPrimitive(primitiveKey, out float3x4 vertices, out Mesh.PrimitiveFlags flags, out CollisionFilter filter); + + if (!CollisionFilter.IsCollisionEnabled(input.Collider->Filter, filter)) // TODO: could do this check within GetPrimitive() + { + return false; + } + + int numPolygons = Mesh.GetNumPolygonsInPrimitive(flags); + bool isQuad = Mesh.IsPrimitveFlagSet(flags, Mesh.PrimitiveFlags.IsQuad); + + float3* v = stackalloc float3[4]; + bool acceptHit = false; + + ref ConvexHull inputHull = ref ((ConvexCollider*)input.Collider)->ConvexHull; + MTransform targetFromQuery = new MTransform(input.Transform); + + for (int polygonIndex = 0; polygonIndex < numPolygons; polygonIndex++) + { + int numVertices; + if (isQuad) + { + v[0] = vertices[0]; + v[1] = vertices[1]; + v[2] = vertices[2]; + v[3] = vertices[3]; + numVertices = 4; + } + else + { + v[0] = vertices[0]; + v[1] = vertices[1 + polygonIndex]; + v[2] = vertices[2 + polygonIndex]; + numVertices = 3; + } + + Result result = ConvexConvex(v, numVertices, 0.0f, inputHull.VerticesPtr, inputHull.NumVertices, inputHull.ConvexRadius, targetFromQuery); + if (result.Distance < collector.MaxFraction) + { + acceptHit |= collector.AddHit(new DistanceHit + { + Fraction = result.Distance, + Position = result.PositionOnAinA, + SurfaceNormal = -result.NormalInA, + ColliderKey = new ColliderKey(m_NumColliderKeyBits, (uint)(primitiveKey << 1 | polygonIndex)) + }); + } + } + + return acceptHit; + } + } + + public static unsafe bool PointMesh(PointDistanceInput input, MeshCollider* meshCollider, ref T collector) + where T : struct, ICollector + { + var leafProcessor = new ConvexMeshLeafProcessor(meshCollider); + return meshCollider->Mesh.BoundingVolumeHierarchy.Distance(input, ref leafProcessor, ref collector); + } + + public static unsafe bool ConvexMesh(ColliderDistanceInput input, MeshCollider* meshCollider, ref T collector) + where T : struct, ICollector + { + var leafProcessor = new ConvexMeshLeafProcessor(meshCollider); + return meshCollider->Mesh.BoundingVolumeHierarchy.Distance(input, ref leafProcessor, ref collector); + } + + private unsafe struct ConvexCompoundLeafProcessor : IPointDistanceLeafProcessor, IColliderDistanceLeafProcessor + { + private readonly CompoundCollider* m_CompoundCollider; + + public ConvexCompoundLeafProcessor(CompoundCollider* compoundCollider) + { + m_CompoundCollider = compoundCollider; + } + + public bool DistanceLeaf(PointDistanceInput input, int leafData, ref T collector) + where T : struct, ICollector + { + ref CompoundCollider.Child child = ref m_CompoundCollider->Children[leafData]; + + if (!CollisionFilter.IsCollisionEnabled(input.Filter, child.Collider->Filter)) + { + return false; + } + + // Transform the point into child space + MTransform compoundFromChild = new MTransform(child.CompoundFromChild); + PointDistanceInput inputLs = input; + { + MTransform childFromCompound = Inverse(compoundFromChild); + inputLs.Position = Math.Mul(childFromCompound, input.Position); + } + + int numHits = collector.NumHits; + float fraction = collector.MaxFraction; + + if (child.Collider->CalculateDistance(inputLs, ref collector)) + { + // Transform results back to compound space + collector.TransformNewHits(numHits, fraction, compoundFromChild, m_CompoundCollider->NumColliderKeyBits, (uint)leafData); + return true; + } + return false; + } + + public bool DistanceLeaf(ColliderDistanceInput input, int leafData, ref T collector) + where T : struct, ICollector + { + ref CompoundCollider.Child child = ref m_CompoundCollider->Children[leafData]; + + if (!CollisionFilter.IsCollisionEnabled(input.Collider->Filter, child.Collider->Filter)) + { + return false; + } + + // Transform the query into child space + ColliderDistanceInput inputLs = input; + inputLs.Transform = math.mul(math.inverse(child.CompoundFromChild), input.Transform); + + int numHits = collector.NumHits; + float fraction = collector.MaxFraction; + + if (child.Collider->CalculateDistance(inputLs, ref collector)) + { + // Transform results back to compound space + collector.TransformNewHits(numHits, fraction, new MTransform(child.CompoundFromChild), m_CompoundCollider->NumColliderKeyBits, (uint)leafData); + return true; + } + return false; + } + } + + public static unsafe bool PointCompound(PointDistanceInput input, CompoundCollider* compoundCollider, ref T collector) + where T : struct, ICollector + { + var leafProcessor = new ConvexCompoundLeafProcessor(compoundCollider); + return compoundCollider->BoundingVolumeHierarchy.Distance(input, ref leafProcessor, ref collector); + } + + public static unsafe bool ConvexCompound(ColliderDistanceInput input, CompoundCollider* compoundCollider, ref T collector) + where T : struct, ICollector + { + var leafProcessor = new ConvexCompoundLeafProcessor(compoundCollider); + return compoundCollider->BoundingVolumeHierarchy.Distance(input, ref leafProcessor, ref collector); + } + } +} diff --git a/package/Unity.Physics/Collision/Queries/Distance.cs.meta b/package/Unity.Physics/Collision/Queries/Distance.cs.meta new file mode 100755 index 000000000..179be33d2 --- /dev/null +++ b/package/Unity.Physics/Collision/Queries/Distance.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 5a84c8eace8256047b5d285f712fee6b +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Unity.Physics/Collision/Queries/Manifold.cs b/package/Unity.Physics/Collision/Queries/Manifold.cs new file mode 100755 index 000000000..122baa37f --- /dev/null +++ b/package/Unity.Physics/Collision/Queries/Manifold.cs @@ -0,0 +1,552 @@ +using System; +using Unity.Collections; +using Unity.Mathematics; +using static Unity.Physics.Math; + +namespace Unity.Physics +{ + // A header preceding a number of contact points in a stream. + public struct ContactHeader + { + public BodyIndexPair BodyPair; + public CustomDataPair BodyCustomDatas; + public JacobianFlags JacobianFlags; + public int NumContacts; + public float3 Normal; + public float CoefficientOfFriction; + public float CoefficientOfRestitution; + public ColliderKeyPair ColliderKeys; + + // followed by NumContacts * ContactPoint + } + + // A contact point in a manifold. All contacts share the same normal. + public struct ContactPoint + { + public float3 Position; // world space position on object A + public float Distance; // separating distance along the manifold normal + } + + + // Contact manifold stream generation functions + public static class ManifoldQueries + { + // A context passed through the manifold generation functions + private struct Context + { + public BodyIndexPair BodyIndices; + public CustomDataPair BodyCustomDatas; + } + + // Write a set of contact manifolds for a pair of bodies to the given stream. + public static unsafe void BodyBody(ref PhysicsWorld world, BodyIndexPair pair, float timeStep, ref BlockStream.Writer contactWriter) + { + RigidBody rigidBodyA = world.Bodies[pair.BodyAIndex]; + RigidBody rigidBodyB = world.Bodies[pair.BodyBIndex]; + + Collider* colliderA = rigidBodyA.Collider; + Collider* colliderB = rigidBodyB.Collider; + + if (colliderA == null || colliderB == null || !CollisionFilter.IsCollisionEnabled(colliderA->Filter, colliderB->Filter)) + { + return; + } + + // Build combined motion expansion + MotionExpansion expansion; + { + MotionExpansion GetBodyExpansion(int bodyIndex, NativeSlice mvs) + { + return bodyIndex < mvs.Length ? mvs[bodyIndex].CalculateExpansion(timeStep) : MotionExpansion.Zero; + } + MotionExpansion expansionA = GetBodyExpansion(pair.BodyAIndex, world.MotionVelocities); + MotionExpansion expansionB = GetBodyExpansion(pair.BodyBIndex, world.MotionVelocities); + expansion = new MotionExpansion + { + Linear = expansionA.Linear - expansionB.Linear, + Uniform = expansionA.Uniform + expansionB.Uniform + world.CollisionTolerance + }; + } + + var context = new Context + { + BodyIndices = pair, + BodyCustomDatas = new CustomDataPair { CustomDataA = rigidBodyA.CustomData, CustomDataB = rigidBodyB.CustomData } + }; + + var worldFromA = new MTransform(rigidBodyA.WorldFromBody); + var worldFromB = new MTransform(rigidBodyB.WorldFromBody); + + // Dispatch to appropriate manifold generator + switch (colliderA->CollisionType) + { + case CollisionType.Convex: + switch (colliderB->CollisionType) + { + case CollisionType.Convex: + ConvexConvex(context, ColliderKeyPair.Empty, colliderA, colliderB, worldFromA, worldFromB, expansion.MaxDistance, false, ref contactWriter); + break; + case CollisionType.Composite: + ConvexComposite(context, ColliderKey.Empty, colliderA, colliderB, worldFromA, worldFromB, expansion, false, ref contactWriter); + break; + } + break; + case CollisionType.Composite: + switch (colliderB->CollisionType) + { + case CollisionType.Convex: + CompositeConvex(context, colliderA, colliderB, worldFromA, worldFromB, expansion, false, ref contactWriter); + break; + case CollisionType.Composite: + CompositeComposite(context, colliderA, colliderB, worldFromA, worldFromB, expansion, false, ref contactWriter); + break; + } + break; + } + } + + private static unsafe void ConvexConvex( + Context context, ColliderKeyPair colliderKeys, + Collider* convexColliderA, Collider* convexColliderB, MTransform worldFromA, MTransform worldFromB, + float maxDistance, bool flipped, ref BlockStream.Writer contactWriter) + { + MTransform aFromB = Mul(Inverse(worldFromA), worldFromB); + + ConvexConvexManifoldQueries.Manifold contactManifold; + switch (convexColliderA->Type) + { + case ColliderType.Sphere: + switch (convexColliderB->Type) + { + case ColliderType.Sphere: + ConvexConvexManifoldQueries.SphereSphere( + (SphereCollider*)convexColliderA, (SphereCollider*)convexColliderB, + worldFromA, aFromB, maxDistance, out contactManifold); + break; + case ColliderType.Capsule: + ConvexConvexManifoldQueries.CapsuleSphere( + (CapsuleCollider*)convexColliderB, (SphereCollider*)convexColliderA, + worldFromB, Inverse(aFromB), maxDistance, out contactManifold); + flipped = !flipped; + break; + case ColliderType.Triangle: + ConvexConvexManifoldQueries.TriangleSphere( + (PolygonCollider*)convexColliderB, (SphereCollider*)convexColliderA, + worldFromB, Inverse(aFromB), maxDistance, out contactManifold); + flipped = !flipped; + break; + case ColliderType.Box: + ConvexConvexManifoldQueries.BoxSphere( + (BoxCollider*)convexColliderB, (SphereCollider*)convexColliderA, + worldFromB, Inverse(aFromB), maxDistance, out contactManifold); + flipped = !flipped; + break; + case ColliderType.Quad: + case ColliderType.Convex: + ConvexConvexManifoldQueries.ConvexConvex( + ref ((SphereCollider*)convexColliderA)->ConvexHull, ref ((ConvexCollider*)convexColliderB)->ConvexHull, + worldFromA, aFromB, maxDistance, out contactManifold); + break; + default: + throw new NotImplementedException(); + } + break; + case ColliderType.Box: + switch (convexColliderB->Type) + { + case ColliderType.Sphere: + ConvexConvexManifoldQueries.BoxSphere( + (BoxCollider*)convexColliderA, (SphereCollider*)convexColliderB, + worldFromA, aFromB, maxDistance, out contactManifold); + break; + case ColliderType.Triangle: + ConvexConvexManifoldQueries.BoxTriangle( + (BoxCollider*)convexColliderA, (PolygonCollider*)convexColliderB, + worldFromA, aFromB, maxDistance, out contactManifold); + break; + case ColliderType.Box: + ConvexConvexManifoldQueries.BoxBox( + (BoxCollider*)convexColliderA, (BoxCollider*)convexColliderB, + worldFromA, aFromB, maxDistance, out contactManifold); + break; + case ColliderType.Capsule: + case ColliderType.Quad: + case ColliderType.Convex: + ConvexConvexManifoldQueries.ConvexConvex( + ref ((BoxCollider*)convexColliderA)->ConvexHull, ref ((ConvexCollider*)convexColliderB)->ConvexHull, + worldFromA, aFromB, maxDistance, out contactManifold); + break; + default: + throw new NotImplementedException(); + } + break; + case ColliderType.Capsule: + switch (convexColliderB->Type) + { + case ColliderType.Sphere: + ConvexConvexManifoldQueries.CapsuleSphere( + (CapsuleCollider*)convexColliderA, (SphereCollider*)convexColliderB, + worldFromA, aFromB, maxDistance, out contactManifold); + break; + case ColliderType.Capsule: + ConvexConvexManifoldQueries.CapsuleCapsule( + (CapsuleCollider*)convexColliderA, (CapsuleCollider*)convexColliderB, + worldFromA, aFromB, maxDistance, out contactManifold); + break; + case ColliderType.Triangle: + ConvexConvexManifoldQueries.CapsuleTriangle( + (CapsuleCollider*)convexColliderA, (PolygonCollider*)convexColliderB, + worldFromA, aFromB, maxDistance, out contactManifold); + break; + case ColliderType.Quad: + case ColliderType.Box: + case ColliderType.Convex: + ConvexConvexManifoldQueries.ConvexConvex( + ref ((CapsuleCollider*)convexColliderA)->ConvexHull, ref ((ConvexCollider*)convexColliderB)->ConvexHull, + worldFromA, aFromB, maxDistance, out contactManifold); + break; + default: + throw new NotImplementedException(); + } + break; + case ColliderType.Triangle: + switch (convexColliderB->Type) + { + case ColliderType.Sphere: + ConvexConvexManifoldQueries.TriangleSphere( + (PolygonCollider*)convexColliderA, (SphereCollider*)convexColliderB, + worldFromA, aFromB, maxDistance, out contactManifold); + break; + case ColliderType.Capsule: + ConvexConvexManifoldQueries.CapsuleTriangle( + (CapsuleCollider*)convexColliderB, (PolygonCollider*)convexColliderA, + worldFromB, Inverse(aFromB), maxDistance, out contactManifold); + flipped = !flipped; + break; + case ColliderType.Box: + ConvexConvexManifoldQueries.BoxTriangle( + (BoxCollider*)convexColliderB, (PolygonCollider*)convexColliderA, + worldFromB, Inverse(aFromB), maxDistance, out contactManifold); + flipped = !flipped; + break; + case ColliderType.Triangle: + case ColliderType.Quad: + case ColliderType.Convex: + ConvexConvexManifoldQueries.ConvexConvex( + ref ((PolygonCollider*)convexColliderA)->ConvexHull, ref ((ConvexCollider*)convexColliderB)->ConvexHull, + worldFromA, aFromB, maxDistance, out contactManifold); + break; + default: + throw new NotImplementedException(); + } + break; + case ColliderType.Quad: + case ColliderType.Convex: + ConvexConvexManifoldQueries.ConvexConvex( + ref ((ConvexCollider*)convexColliderA)->ConvexHull, ref ((ConvexCollider*)convexColliderB)->ConvexHull, + worldFromA, aFromB, maxDistance, out contactManifold); + break; + default: + throw new NotImplementedException(); + } + + // Write results to stream + if (contactManifold.NumContacts > 0) + { + if (flipped) + { + contactManifold.Flip(); + } + + var header = new ContactHeader + { + BodyPair = context.BodyIndices, + BodyCustomDatas = context.BodyCustomDatas, + NumContacts = contactManifold.NumContacts, + Normal = contactManifold.Normal, + ColliderKeys = colliderKeys + }; + + // Apply materials + { + Material materialA = ((ConvexColliderHeader*)convexColliderA)->Material; + Material materialB = ((ConvexColliderHeader*)convexColliderB)->Material; + Material.MaterialFlags combinedFlags = materialA.Flags | materialB.Flags; + if ((combinedFlags & Material.MaterialFlags.IsTrigger) != 0) + { + header.JacobianFlags |= JacobianFlags.IsTrigger; + } + else + { + if ((combinedFlags & Material.MaterialFlags.EnableCollisionEvents) != 0) + { + header.JacobianFlags |= JacobianFlags.EnableCollisionEvents; + } + if ((combinedFlags & Material.MaterialFlags.EnableMassFactors) != 0) + { + header.JacobianFlags |= JacobianFlags.EnableMassFactors; + } + if ((combinedFlags & Material.MaterialFlags.EnableSurfaceVelocity) != 0) + { + header.JacobianFlags |= JacobianFlags.EnableSurfaceVelocity; + } + if ((combinedFlags & Material.MaterialFlags.EnableMaxImpulse) != 0) + { + header.JacobianFlags |= JacobianFlags.EnableMaxImpulse; + } + + header.CoefficientOfFriction = Material.GetCombinedFriction(materialA, materialB); + header.CoefficientOfRestitution = Material.GetCombinedRestitution(materialA, materialB); + } + } + + contactWriter.Write(header); + + for (int contactIndex = 0; contactIndex < header.NumContacts; contactIndex++) + { + contactWriter.Write(contactManifold[contactIndex]); + } + } + } + + private static unsafe void ConvexComposite( + Context context, ColliderKey convexKeyA, + Collider* convexColliderA, Collider* compositeColliderB, MTransform worldFromA, MTransform worldFromB, + MotionExpansion expansion, bool flipped, ref BlockStream.Writer contactWriter) + { + // Calculate AABB of A in B + MTransform bFromWorld = Inverse(worldFromB); + MTransform bFromA = Mul(bFromWorld, worldFromA); + var transform = new RigidTransform(new quaternion(bFromA.Rotation), bFromA.Translation); // TODO: avoid this conversion to and back from float3x3 + Aabb aabbAinB = expansion.ExpandAabb(convexColliderA->CalculateAabb(transform)); + + // Do the midphase query and build manifolds for any overlapping leaf colliders + var input = new OverlapAabbInput { Aabb = aabbAinB, Filter = convexColliderA->Filter }; + var collector = new ConvexCompositeOverlapCollector( + context, + convexColliderA, convexKeyA, compositeColliderB, + worldFromA, worldFromB, expansion.MaxDistance, flipped, + contactWriter); + OverlapQueries.AabbCollider(input, compositeColliderB, ref collector); + + // Keep updated writer state + contactWriter = collector.m_ContactWriter; + } + + private static unsafe void CompositeConvex( + Context context, + Collider* compositeColliderA, Collider* convexColliderB, MTransform worldFromA, MTransform worldFromB, + MotionExpansion expansion, bool flipped, ref BlockStream.Writer contactWriter) + { + // Flip the relevant inputs and call convex-vs-composite + expansion.Linear *= -1.0f; + ConvexComposite(context, ColliderKey.Empty, + convexColliderB, compositeColliderA, worldFromB, worldFromA, expansion, !flipped, + ref contactWriter); + } + + private unsafe struct ConvexCompositeOverlapCollector : IOverlapCollector + { + // Inputs + readonly Context m_Context; + readonly Collider* m_ConvexColliderA; + readonly ColliderKey m_ConvexColliderKey; + readonly Collider* m_CompositeColliderB; + readonly MTransform m_WorldFromA; + readonly MTransform m_WorldFromB; + readonly float m_CollisionTolerance; + readonly bool m_Flipped; + + ColliderKeyPath m_CompositeColliderKeyPath; + + // Output + internal BlockStream.Writer m_ContactWriter; + + public ConvexCompositeOverlapCollector( + Context context, + Collider* convexCollider, ColliderKey convexColliderKey, Collider* compositeCollider, + MTransform worldFromA, MTransform worldFromB, float collisionTolerance, bool flipped, + BlockStream.Writer contactWriter) + { + m_Context = context; + m_ConvexColliderA = convexCollider; + m_ConvexColliderKey = convexColliderKey; + m_CompositeColliderB = compositeCollider; + m_CompositeColliderKeyPath = ColliderKeyPath.Empty; + m_WorldFromA = worldFromA; + m_WorldFromB = worldFromB; + m_CollisionTolerance = collisionTolerance; + m_Flipped = flipped; + m_ContactWriter = contactWriter; + } + + public void AddRigidBodyIndices(int* indices, int count) + { + throw new NotSupportedException(); + } + + public void AddColliderKeys(ColliderKey* keys, int count) + { + ColliderKeyPair colliderKeys = new ColliderKeyPair { ColliderKeyA = m_ConvexColliderKey, ColliderKeyB = m_ConvexColliderKey }; + CollisionFilter filter = m_ConvexColliderA->Filter; + + // Collide the convex A with all overlapping leaves of B + switch (m_CompositeColliderB->Type) + { + // Special case meshes (since we know all polygons will be built on the fly) + case ColliderType.Mesh: + { + Mesh* mesh = &((MeshCollider*)m_CompositeColliderB)->Mesh; + uint numMeshKeyBits = mesh->NumColliderKeyBits; + var polygon = new PolygonCollider(); + polygon.InitEmpty(); + for (int i = 0; i < count; i++) + { + ColliderKey compositeKey = m_CompositeColliderKeyPath.GetLeafKey(keys[i]); + uint meshKey = compositeKey.Value >> (32 - (int)numMeshKeyBits); + if (mesh->GetPolygon(meshKey, filter, ref polygon)) + { + if (m_Flipped) + { + colliderKeys.ColliderKeyA = compositeKey; + } + else + { + colliderKeys.ColliderKeyB = compositeKey; + } + + ConvexConvex( + m_Context, colliderKeys, m_ConvexColliderA, (Collider*)&polygon, + m_WorldFromA, m_WorldFromB, m_CollisionTolerance, m_Flipped, ref m_ContactWriter); + } + } + } + break; + + // General case for all other composites (compounds, compounds of meshes, etc) + default: + { + for (int i = 0; i < count; i++) + { + ColliderKey compositeKey = m_CompositeColliderKeyPath.GetLeafKey(keys[i]); + m_CompositeColliderB->GetLeaf(compositeKey, out ChildCollider leaf); + if (CollisionFilter.IsCollisionEnabled(filter, leaf.Collider->Filter)) // TODO: shouldn't be needed if/when filtering is done fully by the BVH query + { + if (m_Flipped) + { + colliderKeys.ColliderKeyA = compositeKey; + } + else + { + colliderKeys.ColliderKeyB = compositeKey; + } + + MTransform worldFromLeafB = Mul(m_WorldFromB, new MTransform(leaf.TransformFromChild)); + ConvexConvex( + m_Context, colliderKeys, m_ConvexColliderA, leaf.Collider, + m_WorldFromA, worldFromLeafB, m_CollisionTolerance, m_Flipped, ref m_ContactWriter); + } + } + } + break; + } + } + + public void PushCompositeCollider(ColliderKeyPath compositeKey) + { + m_CompositeColliderKeyPath.PushChildKey(compositeKey); + } + + public void PopCompositeCollider(uint numCompositeKeyBits) + { + m_CompositeColliderKeyPath.PopChildKey(numCompositeKeyBits); + } + } + + private static unsafe void CompositeComposite( + Context context, + Collider* compositeColliderA, Collider* compositeColliderB, MTransform worldFromA, MTransform worldFromB, + MotionExpansion expansion, bool flipped, ref BlockStream.Writer contactWriter) + { + // Flip the order if necessary, so that A has fewer leaves than B + if (compositeColliderA->NumColliderKeyBits > compositeColliderB->NumColliderKeyBits) + { + Collider* c = compositeColliderA; + compositeColliderA = compositeColliderB; + compositeColliderB = c; + + MTransform t = worldFromA; + worldFromA = worldFromB; + worldFromB = t; + + expansion.Linear *= -1.0f; + flipped = !flipped; + } + + var collector = new CompositeCompositeLeafCollector( + context, + compositeColliderA, compositeColliderB, + worldFromA, worldFromB, expansion, flipped, + contactWriter); + compositeColliderA->GetLeaves(ref collector); + + // Keep updated writer state + contactWriter = collector.m_ContactWriter; + } + + private unsafe struct CompositeCompositeLeafCollector : ILeafColliderCollector + { + // Inputs + readonly Context m_Context; + readonly Collider* m_CompositeColliderA; + readonly Collider* m_CompositeColliderB; + MTransform m_WorldFromA; + readonly MTransform m_WorldFromB; + readonly MotionExpansion m_Expansion; + readonly bool m_Flipped; + + ColliderKeyPath m_KeyPath; + + // Output + internal BlockStream.Writer m_ContactWriter; + + public CompositeCompositeLeafCollector( + Context context, + Collider* compositeColliderA, Collider* compositeColliderB, + MTransform worldFromA, MTransform worldFromB, MotionExpansion expansion, bool flipped, + BlockStream.Writer contactWriter) + { + m_Context = context; + m_CompositeColliderA = compositeColliderA; + m_CompositeColliderB = compositeColliderB; + m_WorldFromA = worldFromA; + m_WorldFromB = worldFromB; + m_Expansion = expansion; + m_Flipped = flipped; + m_ContactWriter = contactWriter; + m_KeyPath = ColliderKeyPath.Empty; + } + + public void AddLeaf(ColliderKey key, ref ChildCollider leaf) + { + MTransform worldFromLeafA = Mul(m_WorldFromA, new MTransform(leaf.TransformFromChild)); + ConvexComposite( + m_Context, m_KeyPath.GetLeafKey(key), leaf.Collider, m_CompositeColliderB, + worldFromLeafA, m_WorldFromB, m_Expansion, m_Flipped, ref m_ContactWriter); + } + + public void PushCompositeCollider(ColliderKeyPath compositeKey, MTransform parentFromComposite, out MTransform worldFromParent) + { + m_KeyPath.PushChildKey(compositeKey); + worldFromParent = m_WorldFromA; + m_WorldFromA = Math.Mul(worldFromParent, parentFromComposite); + } + + public void PopCompositeCollider(uint numCompositeKeyBits, MTransform worldFromParent) + { + m_WorldFromA = worldFromParent; + m_KeyPath.PopChildKey(numCompositeKeyBits); + } + } + } +} diff --git a/package/Unity.Physics/Collision/Queries/Manifold.cs.meta b/package/Unity.Physics/Collision/Queries/Manifold.cs.meta new file mode 100755 index 000000000..096e40609 --- /dev/null +++ b/package/Unity.Physics/Collision/Queries/Manifold.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 7af89d6754d0f5a4eaf4dc05dfd11c49 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Unity.Physics/Collision/Queries/Overlap.cs b/package/Unity.Physics/Collision/Queries/Overlap.cs new file mode 100755 index 000000000..7f7e1bc5e --- /dev/null +++ b/package/Unity.Physics/Collision/Queries/Overlap.cs @@ -0,0 +1,175 @@ +using System; +using Unity.Mathematics; + +namespace Unity.Physics +{ + // The input to AABB overlap queries + public struct OverlapAabbInput + { + public Aabb Aabb; + public CollisionFilter Filter; + } + + // A hit from an overlap query + public struct OverlapAabbHit + { + public int RigidBodyIndex; + public ColliderKey ColliderKey; + } + + // Interface for collecting hits from overlap queries + public interface IOverlapCollector + { + unsafe void AddRigidBodyIndices(int* indices, int count); + unsafe void AddColliderKeys(ColliderKey* keys, int count); + void PushCompositeCollider(ColliderKeyPath compositeKey); + void PopCompositeCollider(uint numCompositeKeyBits); + } + + // Overlap query implementations + public static class OverlapQueries + { + #region AABB vs colliders + + public static unsafe void AabbCollider(OverlapAabbInput input, Collider* collider, ref T collector) + where T : struct, IOverlapCollector + { + if (!CollisionFilter.IsCollisionEnabled(input.Filter, collider->Filter)) + { + return; + } + + switch (collider->Type) + { + case ColliderType.Mesh: + AabbMesh(input, (MeshCollider*)collider, ref collector); + break; + case ColliderType.Compound: + AabbCompound(input, (CompoundCollider*)collider, ref collector); + break; + default: + throw new NotImplementedException(); + } + } + + // Mesh + private unsafe struct MeshLeafProcessor : BoundingVolumeHierarchy.IAabbOverlapLeafProcessor + { + readonly Mesh* m_Mesh; + readonly uint m_NumColliderKeyBits; + + const int k_MaxKeys = 512; + fixed uint m_Keys[k_MaxKeys]; // actually ColliderKeys, but C# doesn't allow fixed arrays of structs + int m_NumKeys; + + public MeshLeafProcessor(MeshCollider* mesh) + { + m_Mesh = &mesh->Mesh; + m_NumColliderKeyBits = mesh->NumColliderKeyBits; + m_NumKeys = 0; + } + + public void AabbLeaf(OverlapAabbInput input, int primitiveKey, ref T collector) where T : struct, IOverlapCollector + { + fixed (uint* keys = m_Keys) + { + keys[m_NumKeys++] = new ColliderKey(m_NumColliderKeyBits, (uint)(primitiveKey << 1)).Value; + + Mesh.PrimitiveFlags flags = m_Mesh->GetPrimitiveFlags(primitiveKey); + if (Mesh.IsPrimitveFlagSet(flags, Mesh.PrimitiveFlags.IsTrianglePair) && + !Mesh.IsPrimitveFlagSet(flags, Mesh.PrimitiveFlags.IsQuad)) + { + keys[m_NumKeys++] = new ColliderKey(m_NumColliderKeyBits, (uint)(primitiveKey << 1) | 1).Value; + } + } + + if (m_NumKeys > k_MaxKeys - 8) + { + Flush(ref collector); + } + } + + // Flush keys to collector + internal void Flush(ref T collector) where T : struct, IOverlapCollector + { + fixed (uint* keys = m_Keys) + { + collector.AddColliderKeys((ColliderKey*)keys, m_NumKeys); + } + m_NumKeys = 0; + } + } + + private static unsafe void AabbMesh(OverlapAabbInput input, MeshCollider* mesh, ref T collector) + where T : struct, IOverlapCollector + { + var leafProcessor = new MeshLeafProcessor(mesh); + mesh->Mesh.BoundingVolumeHierarchy.AabbOverlap(input, ref leafProcessor, ref collector); + leafProcessor.Flush(ref collector); + } + + + // Compound + private unsafe struct CompoundLeafProcessor : BoundingVolumeHierarchy.IAabbOverlapLeafProcessor + { + readonly CompoundCollider* m_CompoundCollider; + readonly uint m_NumColliderKeyBits; + + const int k_MaxKeys = 512; + fixed uint m_Keys[k_MaxKeys]; // actually ColliderKeys, but C# doesn't allow fixed arrays of structs + int m_NumKeys; + + public CompoundLeafProcessor(CompoundCollider* compound) + { + m_CompoundCollider = compound; + m_NumColliderKeyBits = compound->NumColliderKeyBits; + m_NumKeys = 0; + } + + public void AabbLeaf(OverlapAabbInput input, int childIndex, ref T collector) where T : struct, IOverlapCollector + { + ColliderKey childKey = new ColliderKey(m_NumColliderKeyBits, (uint)(childIndex)); + + // Recurse if child is a composite + ref CompoundCollider.Child child = ref m_CompoundCollider->Children[childIndex]; + if (child.Collider->CollisionType == CollisionType.Composite) + { + OverlapAabbInput childInput = input; + childInput.Aabb = Math.TransformAabb(math.inverse(child.CompoundFromChild), input.Aabb); + + collector.PushCompositeCollider(new ColliderKeyPath(childKey, m_NumColliderKeyBits)); + AabbCollider(childInput, child.Collider, ref collector); + collector.PopCompositeCollider(m_NumColliderKeyBits); + } + else + { + m_Keys[m_NumKeys++] = childKey.Value; + if (m_NumKeys > k_MaxKeys - 8) + { + Flush(ref collector); + } + } + } + + // Flush keys to collector + internal void Flush(ref T collector) where T : struct, IOverlapCollector + { + fixed (uint* keys = m_Keys) + { + collector.AddColliderKeys((ColliderKey*)keys, m_NumKeys); + } + m_NumKeys = 0; + } + } + + private static unsafe void AabbCompound(OverlapAabbInput input, CompoundCollider* compound, ref T collector) + where T : struct, IOverlapCollector + { + var leafProcessor = new CompoundLeafProcessor(compound); + compound->BoundingVolumeHierarchy.AabbOverlap(input, ref leafProcessor, ref collector); + leafProcessor.Flush(ref collector); + } + + #endregion + } +} diff --git a/package/Unity.Physics/Collision/Queries/Overlap.cs.meta b/package/Unity.Physics/Collision/Queries/Overlap.cs.meta new file mode 100755 index 000000000..1b843f07b --- /dev/null +++ b/package/Unity.Physics/Collision/Queries/Overlap.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 32c964965c6520341a464e63284a6514 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Unity.Physics/Collision/Queries/Raycast.cs b/package/Unity.Physics/Collision/Queries/Raycast.cs new file mode 100755 index 000000000..3cd42220f --- /dev/null +++ b/package/Unity.Physics/Collision/Queries/Raycast.cs @@ -0,0 +1,505 @@ +using System; +using Unity.Mathematics; +using UnityEngine.Assertions; +using static Unity.Physics.Math; + +namespace Unity.Physics +{ + // A ray + public struct Ray + { + public float3 Origin; + private float3 m_Direction; + public float3 ReciprocalDirection { get; private set; } + + public float3 Direction + { + get => m_Direction; + set + { + m_Direction = value; + Assert.IsFalse(math.all(m_Direction == float3.zero)); + ReciprocalDirection = math.rcp(m_Direction); + } + } + + public Ray(float3 origin, float3 direction) + { + Assert.IsFalse(math.all(direction == float3.zero)); + + Origin = origin; + m_Direction = direction; + ReciprocalDirection = math.rcp(direction); + } + } + + // The input to ray cast queries + public struct RaycastInput + { + public Ray Ray; + public CollisionFilter Filter; + } + + // A hit from a ray cast query + public struct RaycastHit : IQueryResult + { + public float Fraction { get; set; } + + public float3 Position; + public float3 SurfaceNormal; + public int RigidBodyIndex; + public ColliderKey ColliderKey; + + public void Transform(MTransform transform, uint numSubKeyBits, uint subKey) + { + Position = Mul(transform, Position); + SurfaceNormal = math.mul(transform.Rotation, SurfaceNormal); + ColliderKey.PushSubKey(numSubKeyBits, subKey); + } + + public void Transform(MTransform transform, int rigidBodyIndex) + { + Position = Mul(transform, Position); + SurfaceNormal = math.mul(transform.Rotation, SurfaceNormal); + RigidBodyIndex = rigidBodyIndex; + } + } + + // Raycast query implementations + public static class RaycastQueries + { + #region Ray vs primitives + + public static bool RaySphere( + float3 rayOrigin, float3 rayDirection, + float3 sphereCenter, float sphereRadius, + ref float fraction, out float3 normal) + { + normal = float3.zero; + + // TODO.ma lots of float inaccuracy problems with this + float3 diff = rayOrigin - sphereCenter; + float a = math.dot(rayDirection, rayDirection); + float b = 2.0f * math.dot(rayDirection, diff); + float c = math.dot(diff, diff) - sphereRadius * sphereRadius; + float discriminant = b * b - 4.0f * a * c; + + if (c < 0) + { + // Inside hit. + fraction = 0; + normal = math.normalize(-rayDirection); + return true; + } + + if (discriminant < 0) + { + return false; + } + + float sqrtDiscriminant = math.sqrt(discriminant); + float invDenom = 0.5f / a; + + float t0 = (sqrtDiscriminant - b) * invDenom; + float t1 = (-sqrtDiscriminant - b) * invDenom; + float tMin = math.min(t0, t1); + + if (tMin >= 0 && tMin < fraction) + { + fraction = tMin; + normal = (rayOrigin + rayDirection * fraction - sphereCenter) / sphereRadius; + + return true; + } + + return false; + } + + public static bool RayCapsule( + float3 rayOrigin, float3 rayDirection, + float3 vertex0, float3 vertex1, float radius, + ref float fraction, out float3 normal) + { + float axisLength = NormalizeWithLength(vertex1 - vertex0, out float3 axis); + + // Ray vs infinite cylinder + { + float directionDotAxis = math.dot(rayDirection, axis); + float originDotAxis = math.dot(rayOrigin - vertex0, axis); + float3 rayDirection2D = rayDirection - axis * directionDotAxis; + float3 rayOrigin2D = rayOrigin - axis * originDotAxis; + float cylinderFraction = fraction; + + if (RaySphere(rayOrigin2D, rayDirection2D, vertex0, radius, ref cylinderFraction, out normal)) + { + float t = originDotAxis + cylinderFraction * directionDotAxis; // distance of the hit from Vertex0 along axis + if (t >= 0.0f && t <= axisLength) + { + if (cylinderFraction == 0) + { + // Inside hit + normal = math.normalize(-rayDirection); + } + + fraction = cylinderFraction; + return true; + } + } + } + + // Ray vs caps + { + bool hadHit = false; + float3 capNormal; + if (RaySphere(rayOrigin, rayDirection, vertex0, radius, ref fraction, out capNormal)) + { + hadHit = true; + normal = capNormal; + } + if (RaySphere(rayOrigin, rayDirection, vertex1, radius, ref fraction, out capNormal)) + { + hadHit = true; + normal = capNormal; + } + return hadHit; + } + } + + public static bool RayTriangle( + float3 rayOrigin, float3 rayDirection, + float3 a, float3 b, float3 c, // TODO: float3x3? + ref float fraction, out float3 unnormalizedNormal) + { + float3 vAb = b - a; + float3 vCa = a - c; + + float3 vN = math.cross(vAb, vCa); + float3 vAp = rayOrigin - a; + float3 end0 = vAp + rayDirection * fraction; + + float d = math.dot(vN, vAp); + float e = math.dot(vN, end0); + + if (d * e >= 0) + { + unnormalizedNormal = float3.zero; + return false; + } + + float3 vBc = c - b; + fraction *= d / (d - e); + unnormalizedNormal = vN * math.sign(d); + + // edge normals + float3 c0 = math.cross(vAb, rayDirection); + float3 c1 = math.cross(vBc, rayDirection); + float3 c2 = math.cross(vCa, rayDirection); + + float3 dots; + { + float3 o2 = rayOrigin + rayOrigin; + float3 r0 = o2 - (a + b); + float3 r1 = o2 - (b + c); + float3 r2 = o2 - (c + a); + + dots.x = math.dot(r0, c0); + dots.y = math.dot(r1, c1); + dots.z = math.dot(r2, c2); + } + + bool3 notOutSide = dots < 0; + // hit if all bits have the same sign + return math.all(notOutSide) || !math.any(notOutSide); + } + + public static bool RayQuad( + float3 rayOrigin, float3 rayDirection, + float3 a, float3 b, float3 c, float3 d, // TODO: float3x4? + ref float fraction, out float3 unnormalizedNormal) + { + float3 vAb = b - a; + float3 vCa = a - c; + + float3 vN = math.cross(vAb, vCa); + float3 vAp = rayOrigin - a; + float3 end0 = vAp + rayDirection * fraction; + + float nDotAp = math.dot(vN, vAp); + float e = math.dot(vN, end0); + + if (nDotAp * e >= 0) + { + unnormalizedNormal = float3.zero; + return false; + } + + float3 vBc = c - b; + float3 vDa = a - d; + float3 vCd = d - c; + fraction *= nDotAp / (nDotAp - e); + unnormalizedNormal = vN * math.sign(nDotAp); + + // edge normals + float3 c0 = math.cross(vAb, rayDirection); + float3 c1 = math.cross(vBc, rayDirection); + float3 c2 = math.cross(vCd, rayDirection); + float3 c3 = math.cross(vDa, rayDirection); + + float4 dots; + { + float3 o2 = rayOrigin + rayOrigin; + float3 r0 = o2 - (a + b); + float3 r1 = o2 - (b + c); + float3 r2 = o2 - (c + d); + float3 r3 = o2 - (d + a); + + dots.x = math.dot(r0, c0); + dots.y = math.dot(r1, c1); + dots.z = math.dot(r2, c2); + dots.w = math.dot(r3, c3); + } + + bool4 notOutSide = dots < 0; + // hit if all bits have the same sign + return math.all(notOutSide) || !math.any(notOutSide); + } + + public static bool RayConvex( + float3 rayOrigin, float3 rayDirection, ref ConvexHull hull, + ref float fraction, out float3 normal) + { + // TODO: Call RaySphere/Capsule/Triangle() if num vertices <= 3 ? + + float convexRadius = hull.ConvexRadius; + float fracEnter = -1.0f; + float fracExit = 2.0f; + float3 start = rayOrigin; + float3 end = start + rayDirection * fraction; + normal = new float3(1, 0, 0); + for (int i = 0; i < hull.NumFaces; i++) // TODO.ma vectorize + { + // Calculate the plane's hit fraction + Plane plane = hull.Planes[i]; + float startDistance = math.dot(start, plane.Normal) + plane.Distance - convexRadius; + float endDistance = math.dot(end, plane.Normal) + plane.Distance - convexRadius; + float newFraction = startDistance / (startDistance - endDistance); + bool startInside = (startDistance < 0); + bool endInside = (endDistance < 0); + + // If the ray is entirely outside of any plane, then it misses + if (!(startInside || endInside)) + { + return false; + } + + // If the ray crosses the plane, update the enter or exit fraction + bool enter = !startInside && newFraction > fracEnter; + bool exit = !endInside && newFraction < fracExit; + fracEnter = math.select(fracEnter, newFraction, enter); + normal = math.select(normal, plane.Normal, enter); + fracExit = math.select(fracExit, newFraction, exit); + } + + if (fracEnter < 0) + { + // Inside hit. + fraction = 0; + normal = math.normalize(-rayDirection); + return true; + } + + if (fracEnter < fracExit) + { + fraction *= fracEnter; + return true; + } + + // miss + return false; + } + + #endregion + + #region Ray vs colliders + + public static unsafe bool RayCollider(RaycastInput input, Collider* collider, ref T collector) where T : struct, ICollector + { + if (!CollisionFilter.IsCollisionEnabled(input.Filter, collider->Filter)) + { + return false; + } + + float fraction = collector.MaxFraction; + float3 normal; + bool hadHit; + switch (collider->Type) + { + case ColliderType.Sphere: + var sphere = (SphereCollider*)collider; + hadHit = RaySphere(input.Ray.Origin, input.Ray.Direction, sphere->Center, sphere->Radius, ref fraction, out normal); + break; + case ColliderType.Capsule: + var capsule = (CapsuleCollider*)collider; + hadHit = RayCapsule(input.Ray.Origin, input.Ray.Direction, capsule->Vertex0, capsule->Vertex1, capsule->Radius, ref fraction, out normal); + break; + case ColliderType.Triangle: + { + var triangle = (PolygonCollider*)collider; + hadHit = RayTriangle(input.Ray.Origin, input.Ray.Direction, triangle->Vertices[0], triangle->Vertices[1], triangle->Vertices[2], ref fraction, out float3 unnormalizedNormal); + normal = hadHit ? math.normalize(unnormalizedNormal) : float3.zero; + break; + } + case ColliderType.Quad: + { + var quad = (PolygonCollider*)collider; + hadHit = RayQuad(input.Ray.Origin, input.Ray.Direction, quad->Vertices[0], quad->Vertices[1], quad->Vertices[2], quad->Vertices[3], ref fraction, out float3 unnormalizedNormal); + normal = hadHit ? math.normalize(unnormalizedNormal) : float3.zero; + break; + } + case ColliderType.Box: + case ColliderType.Convex: + hadHit = RayConvex(input.Ray.Origin, input.Ray.Direction, ref ((ConvexCollider*)collider)->ConvexHull, ref fraction, out normal); + break; + case ColliderType.Mesh: + return RayMesh(input, (MeshCollider*)collider, ref collector); + case ColliderType.Compound: + return RayCompound(input, (CompoundCollider*)collider, ref collector); + default: + throw new NotImplementedException(); + } + + if (hadHit) + { + return collector.AddHit(new RaycastHit + { + Fraction = fraction, + Position = input.Ray.Origin + input.Ray.Direction * fraction, + SurfaceNormal = normal, + RigidBodyIndex = -1, + ColliderKey = ColliderKey.Empty + }); + } + return false; + } + + // Mesh + private unsafe struct RayMeshLeafProcessor : BoundingVolumeHierarchy.IRaycastLeafProcessor + { + private readonly Mesh* m_Mesh; + private readonly uint m_NumColliderKeyBits; + + public RayMeshLeafProcessor(MeshCollider* meshCollider) + { + m_Mesh = &meshCollider->Mesh; + m_NumColliderKeyBits = meshCollider->NumColliderKeyBits; + } + + public bool RayLeaf(RaycastInput input, int primitiveKey, ref T collector) where T : struct, ICollector + { + m_Mesh->GetPrimitive(primitiveKey, out float3x4 vertices, out Mesh.PrimitiveFlags flags, out CollisionFilter filter); + + if (!CollisionFilter.IsCollisionEnabled(input.Filter, filter)) // TODO: could do this check within GetPrimitive() + { + return false; + } + + int numPolygons = Mesh.GetNumPolygonsInPrimitive(flags); + bool isQuad = Mesh.IsPrimitveFlagSet(flags, Mesh.PrimitiveFlags.IsQuad); + + bool acceptHit = false; + float3 unnormalizedNormal; + + for (int polygonIndex = 0; polygonIndex < numPolygons; polygonIndex++) + { + float fraction = collector.MaxFraction; + bool hadHit; + if (isQuad) + { + hadHit = RayQuad(input.Ray.Origin, input.Ray.Direction, vertices[0], vertices[1], vertices[2], vertices[3], ref fraction, out unnormalizedNormal); + } + else + { + hadHit = RayTriangle(input.Ray.Origin, input.Ray.Direction, vertices[0], vertices[polygonIndex + 1], vertices[polygonIndex + 2], ref fraction, out unnormalizedNormal); + } + + if (hadHit && fraction < collector.MaxFraction) + { + acceptHit |= collector.AddHit(new RaycastHit + { + Fraction = fraction, + Position = input.Ray.Origin + input.Ray.Direction * fraction, + SurfaceNormal = math.normalize(unnormalizedNormal), + RigidBodyIndex = -1, + ColliderKey = new ColliderKey(m_NumColliderKeyBits, (uint)(primitiveKey << 1 | polygonIndex)) + }); + } + } + + return acceptHit; + } + } + + private static unsafe bool RayMesh(RaycastInput input, MeshCollider* meshCollider, ref T collector) where T : struct, ICollector + { + var leafProcessor = new RayMeshLeafProcessor(meshCollider); + return meshCollider->Mesh.BoundingVolumeHierarchy.Raycast(input, ref leafProcessor, ref collector); + } + + // Compound + + private unsafe struct RayCompoundLeafProcessor : BoundingVolumeHierarchy.IRaycastLeafProcessor + { + private readonly CompoundCollider* m_CompoundCollider; + + public RayCompoundLeafProcessor(CompoundCollider* compoundCollider) + { + m_CompoundCollider = compoundCollider; + } + + public bool RayLeaf(RaycastInput input, int leafData, ref T collector) where T : struct, ICollector + { + ref CompoundCollider.Child child = ref m_CompoundCollider->Children[leafData]; + + if (!CollisionFilter.IsCollisionEnabled(input.Filter, child.Collider->Filter)) + { + return false; + } + + MTransform compoundFromChild = new MTransform(child.CompoundFromChild); + + // Transform the ray into child space + RaycastInput inputLs = input; + { + MTransform childFromCompound = Inverse(compoundFromChild); + float3 originLs = Mul(childFromCompound, input.Ray.Origin); + float3 directionLs = math.mul(childFromCompound.Rotation, input.Ray.Direction); + inputLs.Ray = new Ray(originLs, directionLs); + } + + int numHits = collector.NumHits; + float fraction = collector.MaxFraction; + + if (child.Collider->CastRay(inputLs, ref collector)) + { + // Transform results back to compound space + collector.TransformNewHits(numHits, fraction, compoundFromChild, m_CompoundCollider->NumColliderKeyBits, (uint)leafData); + return true; + } + return false; + } + } + + private static unsafe bool RayCompound(RaycastInput input, CompoundCollider* compoundCollider, ref T collector) where T : struct, ICollector + { + if (!CollisionFilter.IsCollisionEnabled(input.Filter, compoundCollider->Filter)) + { + return false; + } + + var leafProcessor = new RayCompoundLeafProcessor(compoundCollider); + return compoundCollider->BoundingVolumeHierarchy.Raycast(input, ref leafProcessor, ref collector); + } + + #endregion + } +} diff --git a/package/Unity.Physics/Collision/Queries/Raycast.cs.meta b/package/Unity.Physics/Collision/Queries/Raycast.cs.meta new file mode 100755 index 000000000..d1ae732eb --- /dev/null +++ b/package/Unity.Physics/Collision/Queries/Raycast.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 926cb3d162207f649bd8ae9326fe6f8e +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Unity.Physics/Collision/RigidBody.meta b/package/Unity.Physics/Collision/RigidBody.meta new file mode 100755 index 000000000..a6baa2538 --- /dev/null +++ b/package/Unity.Physics/Collision/RigidBody.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: d8f9a1bb6c3efc647af89ad4aa86d3f4 +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Unity.Physics/Collision/RigidBody/RigidBody.cs b/package/Unity.Physics/Collision/RigidBody/RigidBody.cs new file mode 100755 index 000000000..ad515cd25 --- /dev/null +++ b/package/Unity.Physics/Collision/RigidBody/RigidBody.cs @@ -0,0 +1,103 @@ +using Unity.Collections; +using Unity.Entities; +using Unity.Mathematics; + +namespace Unity.Physics +{ + // An instance of a collider in a physics world. + public unsafe struct RigidBody : ICollidable + { + // The rigid body's transform in world space + public RigidTransform WorldFromBody; + + // The rigid body's collider (allowed to be null) + public Collider* Collider; // Todo: use BlobAssetReference? + + // The entity that rigid body represents + public Entity Entity; + + // Arbitrary custom data. + // Gets copied into contact manifolds and can be used to inform contact modifiers. + public byte CustomData; + + public static readonly RigidBody Zero = new RigidBody + { + WorldFromBody = RigidTransform.identity, + Collider = null, + Entity = Entity.Null, + CustomData = 0 + }; + + #region ICollidable implementation + + public Aabb CalculateAabb() + { + if (Collider != null) + { + return Collider->CalculateAabb(WorldFromBody); + } + return new Aabb { Min = WorldFromBody.pos, Max = WorldFromBody.pos }; + } + + public Aabb CalculateAabb(RigidTransform transform) + { + if (Collider != null) + { + return Collider->CalculateAabb(math.mul(transform, WorldFromBody)); + } + return new Aabb { Min = WorldFromBody.pos, Max = WorldFromBody.pos }; + } + + public bool CastRay(RaycastInput input) => QueryWrappers.RayCast(ref this, input); + public bool CastRay(RaycastInput input, out RaycastHit closestHit) => QueryWrappers.RayCast(ref this, input, out closestHit); + public bool CastRay(RaycastInput input, ref NativeList allHits) => QueryWrappers.RayCast(ref this, input, ref allHits); + public bool CastRay(RaycastInput input, ref T collector) where T : struct, ICollector + { + return Collider != null && Collider->CastRay(input, ref collector); + } + + public bool CastCollider(ColliderCastInput input) => QueryWrappers.ColliderCast(ref this, input); + public bool CastCollider(ColliderCastInput input, out ColliderCastHit closestHit) => QueryWrappers.ColliderCast(ref this, input, out closestHit); + public bool CastCollider(ColliderCastInput input, ref NativeList allHits) => QueryWrappers.ColliderCast(ref this, input, ref allHits); + public bool CastCollider(ColliderCastInput input, ref T collector) where T : struct, ICollector + { + return Collider != null && Collider->CastCollider(input, ref collector); + } + + public bool CalculateDistance(PointDistanceInput input) => QueryWrappers.CalculateDistance(ref this, input); + public bool CalculateDistance(PointDistanceInput input, out DistanceHit closestHit) => QueryWrappers.CalculateDistance(ref this, input, out closestHit); + public bool CalculateDistance(PointDistanceInput input, ref NativeList allHits) => QueryWrappers.CalculateDistance(ref this, input, ref allHits); + public bool CalculateDistance(PointDistanceInput input, ref T collector) where T : struct, ICollector + { + return Collider != null && Collider->CalculateDistance(input, ref collector); + } + + public bool CalculateDistance(ColliderDistanceInput input) => QueryWrappers.CalculateDistance(ref this, input); + public bool CalculateDistance(ColliderDistanceInput input, out DistanceHit closestHit) => QueryWrappers.CalculateDistance(ref this, input, out closestHit); + public bool CalculateDistance(ColliderDistanceInput input, ref NativeList allHits) => QueryWrappers.CalculateDistance(ref this, input, ref allHits); + public bool CalculateDistance(ColliderDistanceInput input, ref T collector) where T : struct, ICollector + { + return Collider != null && Collider->CalculateDistance(input, ref collector); + } + + #endregion + } + + // A pair of rigid body indices + public struct BodyIndexPair + { + // B before A to match Havok + public int BodyBIndex; + public int BodyAIndex; + + public static BodyIndexPair Invalid => new BodyIndexPair() { BodyBIndex = -1, BodyAIndex = -1 }; + } + + // A pair of custom rigid body datas + public struct CustomDataPair + { + // B before A for consistency with other pairs + public byte CustomDataB; + public byte CustomDataA; + } +} diff --git a/package/Unity.Physics/Collision/RigidBody/RigidBody.cs.meta b/package/Unity.Physics/Collision/RigidBody/RigidBody.cs.meta new file mode 100755 index 000000000..98a70ec5f --- /dev/null +++ b/package/Unity.Physics/Collision/RigidBody/RigidBody.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 2399ddd9d733dbc48bfd114f8c1779f4 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Unity.Physics/Collision/World.meta b/package/Unity.Physics/Collision/World.meta new file mode 100755 index 000000000..042c72efa --- /dev/null +++ b/package/Unity.Physics/Collision/World.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: 4592463c2e408b74f9aedc284dd9c037 +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Unity.Physics/Collision/World/Broadphase.cs b/package/Unity.Physics/Collision/World/Broadphase.cs new file mode 100755 index 000000000..8a2af518d --- /dev/null +++ b/package/Unity.Physics/Collision/World/Broadphase.cs @@ -0,0 +1,889 @@ +using System; +using System.Runtime.CompilerServices; +using Unity.Burst; +using Unity.Collections; +using Unity.Collections.LowLevel.Unsafe; +using Unity.Jobs; +using Unity.Mathematics; +using UnityEngine.Assertions; +using static Unity.Physics.BoundingVolumeHierarchy; +using static Unity.Physics.Math; + +namespace Unity.Physics +{ + // A bounding volume around a collection of rigid bodies + public struct Broadphase : IDisposable, ICloneable + { + private Tree m_StaticTree; // The tree of static rigid bodies + private Tree m_DynamicTree; // The tree of dynamic rigid bodies + private NativeArray m_BodyFilters; // A copy of the rigid body's filters + + public Tree StaticTree => m_StaticTree; + public Tree DynamicTree => m_DynamicTree; + public Aabb Domain => Aabb.Union(m_StaticTree.BoundingVolumeHierarchy.Domain, m_DynamicTree.BoundingVolumeHierarchy.Domain); + + public void Init() + { + m_StaticTree.Init(); + m_DynamicTree.Init(); + m_BodyFilters = new NativeArray(0, Allocator.Persistent, NativeArrayOptions.ClearMemory); + } + + public void Dispose() + { + m_StaticTree.Dispose(); + m_DynamicTree.Dispose(); + if (m_BodyFilters.IsCreated) + { + m_BodyFilters.Dispose(); + } + } + + public object Clone() + { + var clone = new Broadphase + { + m_StaticTree = (Tree)m_StaticTree.Clone(), + m_DynamicTree = (Tree)m_DynamicTree.Clone(), + m_BodyFilters = new NativeArray(m_BodyFilters.Length, Allocator.Persistent, NativeArrayOptions.UninitializedMemory) + }; + clone.m_BodyFilters.CopyFrom(m_BodyFilters); + return clone; + } + + public unsafe JobHandle ScheduleStaticTreeBuildJobs( + ref PhysicsWorld world, int numThreadsHint, bool haveStaticBodiesChanged, + ref NativeArray previousFrameBodyFilters, JobHandle inputDeps) + { + JobHandle handle = inputDeps; + + if (!haveStaticBodiesChanged) + { + int dynamicBodyDiffFromPreviousFrame = world.NumDynamicBodies - m_DynamicTree.BodyCount; + if (dynamicBodyDiffFromPreviousFrame != 0) + { + // Fix up the static tree. + JobHandle adjustBodyIndices = new AdjustBodyIndicesJob + { + StaticNodes = (Node*)m_StaticTree.Nodes.GetUnsafePtr(), + NumStaticNodes = m_StaticTree.Nodes.Length, + NumDynamicBodiesDiff = dynamicBodyDiffFromPreviousFrame, + }.Schedule(inputDeps); + + // Fix up body filters for static bodies. + JobHandle adjustStaticBodyFilters = new AdjustStaticBodyFilters + { + NumDynamicBodiesDiff = dynamicBodyDiffFromPreviousFrame, + PreviousFrameBodyFilters = previousFrameBodyFilters, + BodyFilters = m_BodyFilters, + NumStaticBodies = world.NumStaticBodies, + NumDynamicBodies = world.NumDynamicBodies + }.Schedule(inputDeps); + + handle = JobHandle.CombineDependencies(adjustBodyIndices, adjustStaticBodyFilters); + } + + return handle; + } + + var aabbs = new NativeArray(world.NumBodies, Allocator.TempJob, NativeArrayOptions.UninitializedMemory); + var lookup = new NativeArray(world.NumStaticBodies, Allocator.TempJob, NativeArrayOptions.UninitializedMemory); + + handle = new BuildStaticBodyDataJob + { + RigidBodies = world.StaticBodies, + Aabbs = aabbs, + FiltersOut = new NativeSlice(m_BodyFilters, world.NumDynamicBodies, world.NumStaticBodies), + Lookup = lookup, + Offset = world.NumDynamicBodies, + AabbMargin = world.CollisionTolerance / 2.0f // each body contributes half + }.Schedule(world.NumStaticBodies, 32, handle); + + m_StaticTree.NodeCount = world.NumStaticBodies + BoundingVolumeHierarchy.Constants.MaxNumTreeBranches; + + return m_StaticTree.BoundingVolumeHierarchy.ScheduleBuildJobs( + lookup, aabbs, m_BodyFilters, numThreadsHint, handle, + m_StaticTree.NodeCount, m_StaticTree.Ranges, m_StaticTree.m_BranchCount); + } + + private JobHandle ScheduleDynamicTreeBuildJobs(ref PhysicsWorld world, float timeStep, int numThreadsHint, JobHandle inputDeps) + { + JobHandle handle = inputDeps; + var aabbs = new NativeArray(world.NumBodies, Allocator.TempJob, NativeArrayOptions.UninitializedMemory); + var lookup = new NativeArray(world.NumDynamicBodies, Allocator.TempJob, NativeArrayOptions.UninitializedMemory); + + handle = new BuildDynamicBodyDataJob + { + RigidBodies = world.DynamicBodies, + MotionVelocities = world.MotionVelocities, + Aabbs = aabbs, + FiltersOut = new NativeSlice(m_BodyFilters, 0, world.NumDynamicBodies), + Lookup = lookup, + AabbMargin = world.CollisionTolerance / 2.0f, // each body contributes half + TimeStep = timeStep + }.Schedule(world.NumDynamicBodies, 32, handle); + + m_DynamicTree.NodeCount = world.NumDynamicBodies + BoundingVolumeHierarchy.Constants.MaxNumTreeBranches; + + return m_DynamicTree.BoundingVolumeHierarchy.ScheduleBuildJobs( + lookup, aabbs, m_BodyFilters, numThreadsHint, handle, + m_DynamicTree.NodeCount, m_DynamicTree.Ranges, m_DynamicTree.m_BranchCount); + } + + // Schedule a set of jobs to build the broadphase based on the given world. + public JobHandle ScheduleBuildJobs(ref PhysicsWorld world, float timeStep, int numThreadsHint, bool haveStaticBodiesChanged, JobHandle inputDeps) + { + JobHandle handle = inputDeps; + + NativeArray previousFrameBodyFilters = new NativeArray(); + if (m_DynamicTree.BodyCount != world.NumDynamicBodies && !haveStaticBodiesChanged) + { + previousFrameBodyFilters = new NativeArray(m_BodyFilters, Allocator.TempJob); + } + + if (world.NumBodies > m_BodyFilters.Length) + { + m_BodyFilters.Dispose(); + m_BodyFilters = new NativeArray(world.NumBodies, Allocator.Persistent, NativeArrayOptions.UninitializedMemory); + } + + m_StaticTree.BodyCount = world.NumStaticBodies; + JobHandle staticTree = ScheduleStaticTreeBuildJobs(ref world, numThreadsHint, haveStaticBodiesChanged, ref previousFrameBodyFilters, inputDeps); + + m_DynamicTree.BodyCount = world.NumDynamicBodies; + JobHandle dynamicTree = inputDeps; + if (world.NumDynamicBodies > 0) + { + dynamicTree = ScheduleDynamicTreeBuildJobs(ref world, timeStep, numThreadsHint, inputDeps); + } + + return JobHandle.CombineDependencies(staticTree, dynamicTree); + } + + // Schedule a set of jobs which will write all overlapping body pairs to the given steam, + // where at least one of the bodies is dynamic. The results are unsorted. + public JobHandle ScheduleFindOverlapsJobs( + out BlockStream dynamicVsDynamicPairsStream, out BlockStream staticVsDynamicPairsStream, JobHandle inputDeps) + { + int numDynamicVsDynamicBranchOverlapPairs = m_DynamicTree.BranchCount * (m_DynamicTree.BranchCount + 1) / 2; + dynamicVsDynamicPairsStream = new BlockStream(numDynamicVsDynamicBranchOverlapPairs, 0xa542b34); + + int numStaticVsDynamicBranchOverlapPairs = m_StaticTree.BranchCount * m_DynamicTree.BranchCount; + staticVsDynamicPairsStream = new BlockStream(numStaticVsDynamicBranchOverlapPairs, 0xa542b34); + + // Build pairs of branch node indices + var dynamicVsDynamicNodePairIndices = new NativeArray(numDynamicVsDynamicBranchOverlapPairs, Allocator.TempJob, NativeArrayOptions.UninitializedMemory); + JobHandle dynamicVsDynamicPairs = new DynamicVsDynamicBuildBranchNodePairsJob + { + Ranges = m_DynamicTree.Ranges, + NumBranches = m_DynamicTree.BranchCount, + NodePairIndices = dynamicVsDynamicNodePairIndices + }.Schedule(inputDeps); + + var staticVsDynamicNodePairIndices = new NativeArray(numStaticVsDynamicBranchOverlapPairs, Allocator.TempJob, NativeArrayOptions.UninitializedMemory); + JobHandle staticVsDynamicPairs = new StaticVsDynamicBuildBranchNodePairsJob + { + DynamicRanges = m_DynamicTree.Ranges, + StaticRanges = m_StaticTree.Ranges, + NumStaticBranches = m_StaticTree.BranchCount, + NumDynamicBranches = m_DynamicTree.BranchCount, + NodePairIndices = staticVsDynamicNodePairIndices + }.Schedule(inputDeps); + + // Write all overlaps to the stream (also deallocates nodePairIndices) + JobHandle dynamicVsDynamicHandle = new DynamicVsDynamicFindOverlappingPairsJob + { + DynamicNodes = m_DynamicTree.Nodes, + BodyFilters = m_BodyFilters, + DynamicNodeFilters = m_DynamicTree.NodeFilters, + PairWriter = dynamicVsDynamicPairsStream, + NodePairIndices = dynamicVsDynamicNodePairIndices + }.Schedule(numDynamicVsDynamicBranchOverlapPairs, 1, dynamicVsDynamicPairs); + + // Write all overlaps to the stream (also deallocates nodePairIndices) + JobHandle staticVsDynamicHandle = new StaticVsDynamicFindOverlappingPairsJob + { + StaticNodes = m_StaticTree.Nodes, + DynamicNodes = m_DynamicTree.Nodes, + BodyFilters = m_BodyFilters, + StaticNodeFilters = m_StaticTree.NodeFilters, + DynamicNodeFilters = m_DynamicTree.NodeFilters, + PairWriter = staticVsDynamicPairsStream, + NodePairIndices = staticVsDynamicNodePairIndices + }.Schedule(numStaticVsDynamicBranchOverlapPairs, 1, staticVsDynamicPairs); + + return JobHandle.CombineDependencies(dynamicVsDynamicHandle, staticVsDynamicHandle); + } + + #region Tree + + // A tree of rigid bodies + public struct Tree : IDisposable, ICloneable + { + public NativeArray Nodes; // The nodes of the bounding volume + public NativeArray NodeFilters; // The collision filter for each node (a union of all its children) + public NativeArray Ranges; + public int BodyCount; + internal NativeArray m_BranchCount; + + public BoundingVolumeHierarchy BoundingVolumeHierarchy => new BoundingVolumeHierarchy(Nodes, NodeFilters); + + public int NodeCount + { + get => Nodes.Length; + set + { + if (value != Nodes.Length) + { + Nodes.Dispose(); + Nodes = new NativeArray(value, Allocator.Persistent, NativeArrayOptions.UninitializedMemory); + NodeFilters.Dispose(); + NodeFilters = new NativeArray(value, Allocator.Persistent, NativeArrayOptions.UninitializedMemory); + } + } + } + + public int BranchCount { get => m_BranchCount[0]; set => m_BranchCount[0] = value; } + + public void Init() + { + // Need minimum of 2 empty nodes, to gracefully return from queries on an empty tree + Nodes = new NativeArray(2, Allocator.Persistent, NativeArrayOptions.ClearMemory) + { + [0] = BoundingVolumeHierarchy.Node.Empty, + [1] = BoundingVolumeHierarchy.Node.Empty + }; + NodeFilters = new NativeArray(2, Allocator.Persistent, NativeArrayOptions.ClearMemory) + { + [0] = CollisionFilter.Default, + [1] = CollisionFilter.Default + }; + Ranges = new NativeArray( + BoundingVolumeHierarchy.Constants.MaxNumTreeBranches, Allocator.Persistent, NativeArrayOptions.UninitializedMemory); + + m_BranchCount = new NativeArray(1, Allocator.Persistent, NativeArrayOptions.ClearMemory); + BodyCount = 0; + } + + public object Clone() + { + var clone = new Tree(); + clone.Nodes = new NativeArray(Nodes.Length, Allocator.Persistent, NativeArrayOptions.UninitializedMemory); + clone.Nodes.CopyFrom(Nodes); + clone.NodeFilters = new NativeArray(NodeFilters.Length, Allocator.Persistent, NativeArrayOptions.UninitializedMemory); + clone.NodeFilters.CopyFrom(NodeFilters); + clone.Ranges = new NativeArray(Ranges.Length, Allocator.Persistent, NativeArrayOptions.UninitializedMemory); + clone.Ranges.CopyFrom(Ranges); + clone.m_BranchCount = new NativeArray(1, Allocator.Persistent, NativeArrayOptions.UninitializedMemory); + clone.BranchCount = BranchCount; + clone.BodyCount = BodyCount; + return clone; + } + + public void Dispose() + { + if (Nodes.IsCreated) + { + Nodes.Dispose(); + } + + if (NodeFilters.IsCreated) + { + NodeFilters.Dispose(); + } + + if (Ranges.IsCreated) + { + Ranges.Dispose(); + } + + if (m_BranchCount.IsCreated) + { + m_BranchCount.Dispose(); + } + } + } + + #endregion + + #region Queries + + private struct RigidBodyOverlapsCollector : IOverlapCollector + { + public NativeList RigidBodyIndices; + + public unsafe void AddRigidBodyIndices(int* indices, int count) + { + RigidBodyIndices.AddRange(indices, count); + } + + public unsafe void AddColliderKeys(ColliderKey* keys, int count) + { + throw new NotSupportedException(); + } + + public void PushCompositeCollider(ColliderKeyPath compositeKey) + { + throw new NotSupportedException(); + } + + public void PopCompositeCollider(uint numCompositeKeyBits) + { + throw new NotSupportedException(); + } + } + + // Test broadphase nodes against the aabb in input. For any overlapping + // tree leaves, put the body indices into the output leafIndices. + public void OverlapAabb(OverlapAabbInput input, NativeSlice rigidBodies, ref NativeList rigidBodyIndices) + { + Assert.IsTrue(input.Filter.IsValid); + var leafProcessor = new BvhLeafProcessor(rigidBodies); + var leafCollector = new RigidBodyOverlapsCollector { RigidBodyIndices = rigidBodyIndices }; + m_StaticTree.BoundingVolumeHierarchy.AabbOverlap(input, ref leafProcessor, ref leafCollector); + m_DynamicTree.BoundingVolumeHierarchy.AabbOverlap(input, ref leafProcessor, ref leafCollector); + } + + public bool CastRay(RaycastInput input, NativeSlice rigidBodies, ref T collector) + where T : struct, ICollector + { + Assert.IsTrue(input.Filter.IsValid); + var leafProcessor = new BvhLeafProcessor(rigidBodies); + bool hasHit = m_StaticTree.BoundingVolumeHierarchy.Raycast(input, ref leafProcessor, ref collector); + hasHit |= m_DynamicTree.BoundingVolumeHierarchy.Raycast(input, ref leafProcessor, ref collector); + return hasHit; + } + + public unsafe bool CastCollider(ColliderCastInput input, NativeSlice rigidBodies, ref T collector) + where T : struct, ICollector + { + Assert.IsTrue(input.Collider != null && input.Collider->Filter.IsValid); + var leafProcessor = new BvhLeafProcessor(rigidBodies); + bool hasHit = m_StaticTree.BoundingVolumeHierarchy.ColliderCast(input, ref leafProcessor, ref collector); + hasHit |= m_DynamicTree.BoundingVolumeHierarchy.ColliderCast(input, ref leafProcessor, ref collector); + return hasHit; + } + + public bool CalculateDistance(PointDistanceInput input, NativeSlice rigidBodies, ref T collector) + where T : struct, ICollector + { + Assert.IsTrue(input.Filter.IsValid); + var leafProcessor = new BvhLeafProcessor(rigidBodies); + bool hasHit = m_StaticTree.BoundingVolumeHierarchy.Distance(input, ref leafProcessor, ref collector); + hasHit |= m_DynamicTree.BoundingVolumeHierarchy.Distance(input, ref leafProcessor, ref collector); + return hasHit; + } + + public unsafe bool CalculateDistance(ColliderDistanceInput input, NativeSlice rigidBodies, ref T collector) + where T : struct, ICollector + { + Assert.IsTrue(input.Collider != null && input.Collider->Filter.IsValid); + var leafProcessor = new BvhLeafProcessor(rigidBodies); + bool hasHit = m_StaticTree.BoundingVolumeHierarchy.Distance(input, ref leafProcessor, ref collector); + hasHit |= m_DynamicTree.BoundingVolumeHierarchy.Distance(input, ref leafProcessor, ref collector); + return hasHit; + } + + private struct BvhLeafProcessor : + BoundingVolumeHierarchy.IRaycastLeafProcessor, + BoundingVolumeHierarchy.IColliderCastLeafProcessor, + BoundingVolumeHierarchy.IPointDistanceLeafProcessor, + BoundingVolumeHierarchy.IColliderDistanceLeafProcessor, + BoundingVolumeHierarchy.IAabbOverlapLeafProcessor + { + [Obsolete("Do not call this method. It is only included to hint AOT compilation", true)] + static void AOTHint() + { + var p = new BvhLeafProcessor(); + var collector = new ClosestHitCollector(); + p.RayLeaf(default(RaycastInput), 0, ref collector); + } + + private readonly NativeSlice m_Bodies; + + public BvhLeafProcessor(NativeSlice bodies) + { + m_Bodies = bodies; + } + + public bool AabbOverlap(int rigidBodyIndex, ref NativeList allHits) + { + allHits.Add(rigidBodyIndex); + return true; + } + + public bool RayLeaf(RaycastInput input, int rigidBodyIndex, ref T collector) where T : struct, ICollector + { + RigidBody body = m_Bodies[rigidBodyIndex]; + + var worldFromBody = new MTransform(body.WorldFromBody); + + // Transform the ray into body space + RaycastInput inputLs = input; + { + MTransform bodyFromWorld = Inverse(worldFromBody); + float3 originLs = Mul(bodyFromWorld, input.Ray.Origin); + float3 directionLs = math.mul(bodyFromWorld.Rotation, input.Ray.Direction); + inputLs.Ray = new Ray(originLs, directionLs); + } + + float fraction = collector.MaxFraction; + int numHits = collector.NumHits; + + if (body.CastRay(inputLs, ref collector)) + { + // Transform results back into world space + collector.TransformNewHits(numHits, fraction, worldFromBody, rigidBodyIndex); + return true; + } + return false; + } + + public unsafe bool ColliderCastLeaf(ColliderCastInput input, int rigidBodyIndex, ref T collector) + where T : struct, ICollector + { + RigidBody body = m_Bodies[rigidBodyIndex]; + + // Transform the input into body space + MTransform worldFromBody = new MTransform(body.WorldFromBody); + MTransform bodyFromWorld = Inverse(worldFromBody); + ColliderCastInput inputLs = new ColliderCastInput + { + Collider = input.Collider, + Position = Mul(bodyFromWorld, input.Position), + Orientation = math.mul(math.inverse(body.WorldFromBody.rot), input.Orientation), + Direction = math.mul(bodyFromWorld.Rotation, input.Direction) + }; + + float fraction = collector.MaxFraction; + int numHits = collector.NumHits; + + if (body.CastCollider(inputLs, ref collector)) + { + // Transform results back into world space + collector.TransformNewHits(numHits, fraction, worldFromBody, rigidBodyIndex); + return true; + } + return false; + } + + public bool DistanceLeaf(PointDistanceInput input, int rigidBodyIndex, ref T collector) + where T : struct, ICollector + { + RigidBody body = m_Bodies[rigidBodyIndex]; + + // Transform the input into body space + MTransform worldFromBody = new MTransform(body.WorldFromBody); + MTransform bodyFromWorld = Inverse(worldFromBody); + PointDistanceInput inputLs = new PointDistanceInput + { + Position = Mul(bodyFromWorld, input.Position), + MaxDistance = input.MaxDistance, + Filter = input.Filter + }; + + float fraction = collector.MaxFraction; + int numHits = collector.NumHits; + + if (body.CalculateDistance(inputLs, ref collector)) + { + // Transform results back into world space + collector.TransformNewHits(numHits, fraction, worldFromBody, rigidBodyIndex); + return true; + } + return false; + } + + public unsafe bool DistanceLeaf(ColliderDistanceInput input, int rigidBodyIndex, ref T collector) + where T : struct, ICollector + { + RigidBody body = m_Bodies[rigidBodyIndex]; + + // Transform the input into body space + MTransform worldFromBody = new MTransform(body.WorldFromBody); + MTransform bodyFromWorld = Inverse(worldFromBody); + ColliderDistanceInput inputLs = new ColliderDistanceInput + { + Collider = input.Collider, + Transform = new RigidTransform( + math.mul(math.inverse(body.WorldFromBody.rot), input.Transform.rot), + Mul(bodyFromWorld, input.Transform.pos)), + MaxDistance = input.MaxDistance + }; + + float fraction = collector.MaxFraction; + int numHits = collector.NumHits; + + if (body.CalculateDistance(inputLs, ref collector)) + { + // Transform results back into world space + collector.TransformNewHits(numHits, fraction, worldFromBody, rigidBodyIndex); + return true; + } + return false; + } + + public unsafe void AabbLeaf(OverlapAabbInput input, int rigidBodyIndex, ref T collector) + where T : struct, IOverlapCollector + { + RigidBody body = m_Bodies[rigidBodyIndex]; + if (body.Collider != null && CollisionFilter.IsCollisionEnabled(input.Filter, body.Collider->Filter)) + { + collector.AddRigidBodyIndices(&rigidBodyIndex, 1); + } + } + } + + #endregion + + #region Jobs + + // Reads broadphase data from dynamic rigid bodies + [BurstCompile] + internal struct BuildDynamicBodyDataJob : IJobParallelFor + { + [ReadOnly] public NativeSlice RigidBodies; + [ReadOnly] public NativeSlice MotionVelocities; + [ReadOnly] public float TimeStep; + [ReadOnly] public float AabbMargin; + + public NativeArray Lookup; + public NativeArray Aabbs; + [NativeDisableContainerSafetyRestriction] + public NativeSlice FiltersOut; + + public unsafe void Execute(int index) + { + RigidBody body = RigidBodies[index]; + + Aabb aabb; + if (body.Collider != null) + { + MotionExpansion expansion = MotionVelocities[index].CalculateExpansion(TimeStep); + aabb = expansion.ExpandAabb(body.Collider->CalculateAabb(body.WorldFromBody)); + aabb.Expand(AabbMargin); + + FiltersOut[index] = body.Collider->Filter; + } + else + { + aabb.Min = body.WorldFromBody.pos; + aabb.Max = body.WorldFromBody.pos; + + FiltersOut[index] = CollisionFilter.Zero; + } + + Aabbs[index] = aabb; + Lookup[index] = new BoundingVolumeHierarchy.PointAndIndex + { + Position = aabb.Center, + Index = index + }; + } + } + + // Reads broadphase data from static rigid bodies + [BurstCompile] + internal struct BuildStaticBodyDataJob : IJobParallelFor + { + [ReadOnly] public NativeSlice RigidBodies; + [ReadOnly] public int Offset; + [ReadOnly] public float AabbMargin; + + [NativeDisableContainerSafetyRestriction] + public NativeArray Aabbs; + public NativeArray Lookup; + [NativeDisableContainerSafetyRestriction] + public NativeSlice FiltersOut; + + public unsafe void Execute(int index) + { + int staticBodyIndex = index + Offset; + RigidBody body = RigidBodies[index]; + + Aabb aabb; + if (body.Collider != null) + { + aabb = body.Collider->CalculateAabb(body.WorldFromBody); + aabb.Expand(AabbMargin); + + FiltersOut[index] = RigidBodies[index].Collider->Filter; + } + else + { + aabb.Min = body.WorldFromBody.pos; + aabb.Max = body.WorldFromBody.pos; + + FiltersOut[index] = CollisionFilter.Default; + } + + Aabbs[staticBodyIndex] = aabb; + Lookup[index] = new BoundingVolumeHierarchy.PointAndIndex + { + Position = aabb.Center, + Index = staticBodyIndex + }; + } + } + + // Builds a list of branch node index pairs (an input to FindOverlappingPairsJob) + [BurstCompile] + public struct DynamicVsDynamicBuildBranchNodePairsJob : IJob + { + [ReadOnly] public NativeArray Ranges; + [ReadOnly] public int NumBranches; + public NativeArray NodePairIndices; + + public void Execute() + { + int arrayIndex = 0; + for (int i = 0; i < NumBranches; i++) + { + for (int j = i; j < NumBranches; j++) + { + int2 pair = new int2 { x = Ranges[i].Root, y = Ranges[j].Root }; + NodePairIndices[arrayIndex++] = pair; + } + } + } + } + + // Builds a list of branch node index pairs (an input to FindOverlappingPairsJob) + [BurstCompile] + public struct StaticVsDynamicBuildBranchNodePairsJob : IJob + { + [ReadOnly] public NativeArray StaticRanges; + [ReadOnly] public NativeArray DynamicRanges; + [ReadOnly] public int NumStaticBranches; + [ReadOnly] public int NumDynamicBranches; + public NativeArray NodePairIndices; + + public void Execute() + { + int arrayIndex = 0; + for (int i = 0; i < NumStaticBranches; i++) + { + for (int j = 0; j < NumDynamicBranches; j++) + { + int2 pair = new int2 { x = StaticRanges[i].Root, y = DynamicRanges[j].Root }; + NodePairIndices[arrayIndex++] = pair; + } + } + } + } + + // An implementation of IOverlapCollector which filters and writes body pairs to a block stream + public unsafe struct BodyPairWriter : BoundingVolumeHierarchy.ITreeOverlapCollector + { + const int k_Capacity = 256; + const int k_Margin = 64; + const int k_Threshold = k_Capacity - k_Margin; + + fixed int m_PairsLeft[k_Capacity]; + fixed int m_PairsRight[k_Capacity]; + + private BlockStream.Writer* m_CollidingPairs; + private CollisionFilter* m_BodyFilters; + private int m_Count; + + public BodyPairWriter(ref BlockStream.Writer collidingPairs, CollisionFilter* bodyFilters) + { + m_CollidingPairs = (BlockStream.Writer*)UnsafeUtility.AddressOf(ref collidingPairs); + m_BodyFilters = bodyFilters; + m_Count = 0; + } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public void AddPairs(int4 pairsLeft, int4 pairsRight, int count) + { + for (int i = 0; i < count; i++) + { + if (CollisionFilter.IsCollisionEnabled(m_BodyFilters[pairsLeft[i]], m_BodyFilters[pairsRight[i]])) + { + fixed (int* l = m_PairsLeft) + { + l[m_Count] = pairsLeft[i]; + } + fixed (int* r = m_PairsRight) + { + r[m_Count] = pairsRight[i]; + } + m_Count++; + } + } + } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public void AddPairs(int pairLeft, int4 pairsRight, int countR) + { + for (int i = 0; i < countR; i++) + { + if (CollisionFilter.IsCollisionEnabled(m_BodyFilters[pairLeft], m_BodyFilters[pairsRight[i]])) + { + fixed (int* l = m_PairsLeft) + { + l[m_Count] = pairLeft; + } + fixed (int* r = m_PairsRight) + { + r[m_Count] = pairsRight[i]; + } + m_Count++; + } + } + } + + public void Close() + { + Flush(); + } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public void FlushIfNeeded() + { + if (m_Count >= k_Threshold) + { + Flush(); + } + } + + private void Flush() + { + if (m_Count != 0) + { + fixed (int* l = m_PairsLeft) + { + fixed (int* r = m_PairsRight) + { + for (int i = 0; i < m_Count; i++) + { + m_CollidingPairs->Write(new BodyIndexPair { BodyAIndex = l[i], BodyBIndex = r[i] }); + } + } + } + + m_Count = 0; + } + } + } + + // Writes pairs of overlapping broadphase AABBs to a stream. + [BurstCompile] + public unsafe struct DynamicVsDynamicFindOverlappingPairsJob : IJobParallelFor + { + [ReadOnly] public NativeArray DynamicNodes; + [ReadOnly] public NativeArray DynamicNodeFilters; + [ReadOnly] public NativeArray BodyFilters; + [ReadOnly] [DeallocateOnJobCompletion] public NativeArray NodePairIndices; + + public BlockStream.Writer PairWriter; + + public void Execute(int index) + { + PairWriter.BeginForEachIndex(index); + + int2 pair = NodePairIndices[index]; + + CollisionFilter* bodyFiltersPtr = (CollisionFilter*)BodyFilters.GetUnsafeReadOnlyPtr(); + var bufferedPairs = new BodyPairWriter(ref PairWriter, bodyFiltersPtr); + + new BoundingVolumeHierarchy(DynamicNodes, DynamicNodeFilters).SelfBvhOverlap(ref bufferedPairs, pair.x, pair.y); + + bufferedPairs.Close(); + PairWriter.EndForEachIndex(); + } + } + + [BurstCompile] + public unsafe struct StaticVsDynamicFindOverlappingPairsJob : IJobParallelFor + { + [ReadOnly] public NativeArray StaticNodes; + [ReadOnly] public NativeArray DynamicNodes; + + [ReadOnly] public NativeArray StaticNodeFilters; + [ReadOnly] public NativeArray DynamicNodeFilters; + [ReadOnly] public NativeArray BodyFilters; + + [ReadOnly] [DeallocateOnJobCompletion] public NativeArray NodePairIndices; + + public BlockStream.Writer PairWriter; + + public void Execute(int index) + { + PairWriter.BeginForEachIndex(index); + + int2 pair = NodePairIndices[index]; + + CollisionFilter* bodyFiltersPtr = (CollisionFilter*)BodyFilters.GetUnsafeReadOnlyPtr(); + var bufferedPairs = new BodyPairWriter(ref PairWriter, bodyFiltersPtr); + + var staticBvh = new BoundingVolumeHierarchy(StaticNodes, StaticNodeFilters); + var dynamicBvh = new BoundingVolumeHierarchy(DynamicNodes, DynamicNodeFilters); + + staticBvh.BvhOverlap(ref bufferedPairs, dynamicBvh, pair.x, pair.y); + + bufferedPairs.Close(); + PairWriter.EndForEachIndex(); + } + + } + + // In case static bodies haven't changed, but dynamic bodies did + // we need to move body filter info by an offset defined by + // number of dynamic bodies count diff. + [BurstCompile] + public struct AdjustStaticBodyFilters : IJob + { + [ReadOnly] public int NumDynamicBodiesDiff; + [ReadOnly] public int NumStaticBodies; + [ReadOnly] public int NumDynamicBodies; + [ReadOnly] [DeallocateOnJobCompletion] public NativeArray PreviousFrameBodyFilters; + + [NativeDisableContainerSafetyRestriction] public NativeArray BodyFilters; + + public void Execute() + { + int previousFrameNumDynamicBodies = NumDynamicBodies - NumDynamicBodiesDiff; + + for (int i = 0; i < NumStaticBodies; i++) + { + BodyFilters[NumDynamicBodies + i] = PreviousFrameBodyFilters[previousFrameNumDynamicBodies + i]; + } + } + } + + // In case static bodies haven't changed, but dynamic bodies did + // we can keep the static tree as it is, we just need to update + // indices of static rigid bodies by fixed offset in all leaf nodes. + [BurstCompile] + public unsafe struct AdjustBodyIndicesJob : IJob + { + [ReadOnly] public int NumDynamicBodiesDiff; + [ReadOnly] public int NumStaticNodes; + + [NativeDisableUnsafePtrRestriction] + public Node* StaticNodes; + + public void Execute() + { + for (int nodeIndex = 0; nodeIndex < NumStaticNodes; nodeIndex++) + { + if (StaticNodes[nodeIndex].IsLeaf) + { + StaticNodes[nodeIndex].Data[0] += NumDynamicBodiesDiff; + + for (int i = 1; i < 4; i++) + { + if (StaticNodes[nodeIndex].IsLeafValid(i)) + { + StaticNodes[nodeIndex].Data[i] += NumDynamicBodiesDiff; + } + } + } + } + } + } + + #endregion + } +} diff --git a/package/Unity.Physics/Collision/World/Broadphase.cs.meta b/package/Unity.Physics/Collision/World/Broadphase.cs.meta new file mode 100755 index 000000000..2164b367d --- /dev/null +++ b/package/Unity.Physics/Collision/World/Broadphase.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 290f7d3360f57104bb02d3e7b56f9a30 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Unity.Physics/Collision/World/CollisionWorld.cs b/package/Unity.Physics/Collision/World/CollisionWorld.cs new file mode 100755 index 000000000..e23ede493 --- /dev/null +++ b/package/Unity.Physics/Collision/World/CollisionWorld.cs @@ -0,0 +1,153 @@ +using System; +using Unity.Burst; +using Unity.Collections; +using Unity.Jobs; +using Unity.Mathematics; +using static Unity.Physics.Math; + +namespace Unity.Physics +{ + // A collection of rigid bodies wrapped by a bounding volume hierarchy. + // This allows to do collision queries such as raycasting, overlap testing, etc. + public struct CollisionWorld : ICollidable, IDisposable, ICloneable + { + private NativeArray m_Bodies; // storage for the rigid bodies + private int m_NumBodies; // number of rigid bodies currently in use + + public Broadphase Broadphase; // a bounding volume hierarchy around the rigid bodies + + public NativeSlice Bodies => new NativeSlice(m_Bodies, 0, m_NumBodies); + + public int NumBodies + { + get => m_NumBodies; + set + { + m_NumBodies = value; + if (m_Bodies.Length < m_NumBodies) + { + m_Bodies.Dispose(); + m_Bodies = new NativeArray(m_NumBodies, Allocator.Persistent, NativeArrayOptions.UninitializedMemory); + } + } + } + + // Construct a collision world with the given number of uninitialized rigid bodies + public CollisionWorld(int numBodies) + { + m_Bodies = new NativeArray(numBodies, Allocator.Persistent, NativeArrayOptions.UninitializedMemory); + m_NumBodies = numBodies; + Broadphase = new Broadphase(); + Broadphase.Init(); + } + + // Free internal memory + public void Dispose() + { + m_Bodies.Dispose(); + Broadphase.Dispose(); + } + + // Clone the world (except the colliders) + public object Clone() + { + CollisionWorld clone = new CollisionWorld + { + m_Bodies = new NativeArray(m_Bodies.Length, Allocator.Persistent, NativeArrayOptions.UninitializedMemory), + m_NumBodies = m_NumBodies, + Broadphase = (Broadphase)Broadphase.Clone() + }; + clone.m_Bodies.CopyFrom(m_Bodies); + return clone; + } + + #region Jobs + + // Schedule a set of jobs to synchronize the collision world with the dynamics world. + public JobHandle ScheduleUpdateDynamicLayer(ref PhysicsWorld world, float timeStep, int numThreadsHint, JobHandle inputDeps) + { + JobHandle handle = new UpdateRigidBodyTransformsJob + { + MotionDatas = world.MotionDatas, + MotionVelocities = world.MotionVelocities, + RigidBodies = m_Bodies + }.Schedule(world.MotionDatas.Length, 32, inputDeps); + + // TODO: Instead of a full build we could probably incrementally update the existing broadphase, + // since the number of bodies will be the same and their positions should be similar. + return Broadphase.ScheduleBuildJobs(ref world, timeStep, numThreadsHint, haveStaticBodiesChanged: false, inputDeps: handle); + } + + [BurstCompile] + private struct UpdateRigidBodyTransformsJob : IJobParallelFor + { + [ReadOnly] public NativeSlice MotionDatas; + [ReadOnly] public NativeSlice MotionVelocities; + public NativeSlice RigidBodies; + + public void Execute(int i) + { + RigidBody rb = RigidBodies[i]; + rb.WorldFromBody = math.mul(MotionDatas[i].WorldFromMotion, math.inverse(MotionDatas[i].BodyFromMotion)); + RigidBodies[i] = rb; + } + } + + #endregion + + #region ICollidable implementation + + public Aabb CalculateAabb() + { + return Broadphase.Domain; + } + + public Aabb CalculateAabb(RigidTransform transform) + { + return TransformAabb(transform, Broadphase.Domain); + } + + public bool CastRay(RaycastInput input) => QueryWrappers.RayCast(ref this, input); + public bool CastRay(RaycastInput input, out RaycastHit closestHit) => QueryWrappers.RayCast(ref this, input, out closestHit); + public bool CastRay(RaycastInput input, ref NativeList allHits) => QueryWrappers.RayCast(ref this, input, ref allHits); + public bool CastRay(RaycastInput input, ref T collector) where T : struct, ICollector + { + return Broadphase.CastRay(input, m_Bodies, ref collector); + } + + public bool CastCollider(ColliderCastInput input) => QueryWrappers.ColliderCast(ref this, input); + public bool CastCollider(ColliderCastInput input, out ColliderCastHit closestHit) => QueryWrappers.ColliderCast(ref this, input, out closestHit); + public bool CastCollider(ColliderCastInput input, ref NativeList allHits) => QueryWrappers.ColliderCast(ref this, input, ref allHits); + public bool CastCollider(ColliderCastInput input, ref T collector) where T : struct, ICollector + { + return Broadphase.CastCollider(input, m_Bodies, ref collector); + } + + public bool CalculateDistance(PointDistanceInput input) => QueryWrappers.CalculateDistance(ref this, input); + public bool CalculateDistance(PointDistanceInput input, out DistanceHit closestHit) => QueryWrappers.CalculateDistance(ref this, input, out closestHit); + public bool CalculateDistance(PointDistanceInput input, ref NativeList allHits) => QueryWrappers.CalculateDistance(ref this, input, ref allHits); + public bool CalculateDistance(PointDistanceInput input, ref T collector) where T : struct, ICollector + { + return Broadphase.CalculateDistance(input, m_Bodies, ref collector); + } + + public bool CalculateDistance(ColliderDistanceInput input) => QueryWrappers.CalculateDistance(ref this, input); + public bool CalculateDistance(ColliderDistanceInput input, out DistanceHit closestHit) => QueryWrappers.CalculateDistance(ref this, input, out closestHit); + public bool CalculateDistance(ColliderDistanceInput input, ref NativeList allHits) => QueryWrappers.CalculateDistance(ref this, input, ref allHits); + public bool CalculateDistance(ColliderDistanceInput input, ref T collector) where T : struct, ICollector + { + return Broadphase.CalculateDistance(input, m_Bodies, ref collector); + } + + #endregion + + // Test input against the broadphase tree, filling allHits with the body indices of every overlap. + // Returns true if there was at least overlap. + public bool OverlapAabb(OverlapAabbInput input, ref NativeList allHits) + { + int hitsBefore = allHits.Length; + Broadphase.OverlapAabb(input, m_Bodies, ref allHits); + return allHits.Length > hitsBefore; + } + } +} diff --git a/package/Unity.Physics/Collision/World/CollisionWorld.cs.meta b/package/Unity.Physics/Collision/World/CollisionWorld.cs.meta new file mode 100755 index 000000000..21ad1d90d --- /dev/null +++ b/package/Unity.Physics/Collision/World/CollisionWorld.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 67ee297e4011cb44abd9059ed2790b6a +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Unity.Physics/Dynamics.meta b/package/Unity.Physics/Dynamics.meta new file mode 100755 index 000000000..3b0dbe334 --- /dev/null +++ b/package/Unity.Physics/Dynamics.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: a73b940bb36d7d74a843a6da6b74ab1d +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Unity.Physics/Dynamics/Integrator.meta b/package/Unity.Physics/Dynamics/Integrator.meta new file mode 100755 index 000000000..f8b81efe8 --- /dev/null +++ b/package/Unity.Physics/Dynamics/Integrator.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: 3cc0cca878c14374496004736b408eee +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Unity.Physics/Dynamics/Integrator/Integrator.cs b/package/Unity.Physics/Dynamics/Integrator/Integrator.cs new file mode 100755 index 000000000..5698cf3b5 --- /dev/null +++ b/package/Unity.Physics/Dynamics/Integrator/Integrator.cs @@ -0,0 +1,75 @@ +using Unity.Burst; +using Unity.Collections; +using Unity.Jobs; +using Unity.Mathematics; + +namespace Unity.Physics +{ + public static class Integrator + { + // Schedule a job to integrate the world's motions forward by the given time step. + public static JobHandle ScheduleIntegrateJobs(ref DynamicsWorld world, float timeStep, float3 gravity, JobHandle inputDeps) + { + return new IntegrateMotionsJob + { + MotionDatas = world.MotionDatas, + MotionVelocities = world.MotionVelocities, + Timestep = timeStep, + Gravity = gravity + }.Schedule(world.NumMotions, 64, inputDeps); + } + + [BurstCompile] + private struct IntegrateMotionsJob : IJobParallelFor + { + public NativeSlice MotionDatas; + public NativeSlice MotionVelocities; + public float Timestep; + public float3 Gravity; + + public void Execute(int i) + { + MotionData motionData = MotionDatas[i]; + MotionVelocity motionVelocity = MotionVelocities[i]; + + // Update motion space + { + // center of mass + motionData.WorldFromMotion.pos += motionVelocity.LinearVelocity * Timestep; + + // orientation + IntegrateOrientation(ref motionData.WorldFromMotion.rot, motionVelocity.AngularVelocity, Timestep); + } + + // Update velocities + { + // gravity + motionVelocity.LinearVelocity += Gravity * motionData.GravityFactor * Timestep; + + // damping + motionVelocity.LinearVelocity *= math.clamp(1.0f - motionData.LinearDamping * Timestep, 0.0f, 1.0f); + motionVelocity.AngularVelocity *= math.clamp(1.0f - motionData.AngularDamping * Timestep, 0.0f, 1.0f); + } + + // Write back + MotionDatas[i] = motionData; + MotionVelocities[i] = motionVelocity; + } + } + + public static void IntegrateOrientation(ref quaternion orientation, float3 angularVelocity, float timestep) + { + quaternion dq = IntegrateAngularVelocity(angularVelocity, timestep); + quaternion r = math.mul(orientation, dq); + orientation = math.normalize(r); + } + + // Returns a non-normalized quaternion that approximates the change in angle angularVelocity * timestep. + public static quaternion IntegrateAngularVelocity(float3 angularVelocity, float timestep) + { + float3 halfDeltaTime = new float3(timestep * 0.5f); + float3 halfDeltaAngle = angularVelocity * halfDeltaTime; + return new quaternion(new float4(halfDeltaAngle, 1.0f)); + } + } +} diff --git a/package/Unity.Physics/Dynamics/Integrator/Integrator.cs.meta b/package/Unity.Physics/Dynamics/Integrator/Integrator.cs.meta new file mode 100755 index 000000000..69ea01908 --- /dev/null +++ b/package/Unity.Physics/Dynamics/Integrator/Integrator.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 6b6541aa033c9c649afef26fb467e3eb +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Unity.Physics/Dynamics/Jacobians.meta b/package/Unity.Physics/Dynamics/Jacobians.meta new file mode 100755 index 000000000..1b441cbed --- /dev/null +++ b/package/Unity.Physics/Dynamics/Jacobians.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: f9d102141c6347c4faad2054fb4bbe3e +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Unity.Physics/Dynamics/Jacobians/AngularLimit1DJacobian.cs b/package/Unity.Physics/Dynamics/Jacobians/AngularLimit1DJacobian.cs new file mode 100755 index 000000000..689c981a8 --- /dev/null +++ b/package/Unity.Physics/Dynamics/Jacobians/AngularLimit1DJacobian.cs @@ -0,0 +1,114 @@ +using Unity.Mathematics; +using static Unity.Physics.Math; + +namespace Unity.Physics +{ + // Solve data for a constraint that limits one degree of angular freedom + public struct AngularLimit1DJacobian + { + // Limited axis in motion A space + // TODO could calculate this from AxisIndex and MotionAFromJoint + public float3 AxisInMotionA; + + // Index of the limited axis + public int AxisIndex; + + // Relative angle limits + public float MinAngle; + public float MaxAngle; + + // Relative orientation of the motions before solving + public quaternion MotionBFromA; + + // Rotation to joint space from motion space + public quaternion MotionAFromJoint; + public quaternion MotionBFromJoint; + + // Error before solving + public float InitialError; + + // Fraction of the position error to correct per step + public float Tau; + + // Fraction of the velocity error to correct per step + public float Damping; + + // Build the Jacobian + public void Build( + MTransform aFromConstraint, MTransform bFromConstraint, + MotionVelocity velocityA, MotionVelocity velocityB, + MotionData motionA, MotionData motionB, + Constraint constraint, float tau, float damping) + { + this = default(AngularLimit1DJacobian); + + // Copy the constraint into the jacobian + AxisInMotionA = aFromConstraint.Rotation[constraint.ConstrainedAxis1D]; + MinAngle = constraint.Min; + MaxAngle = constraint.Max; + Tau = tau; + Damping = damping; + MotionBFromA = math.mul(math.inverse(motionB.WorldFromMotion.rot), motionA.WorldFromMotion.rot); + MotionAFromJoint = new quaternion(aFromConstraint.Rotation); + MotionBFromJoint = new quaternion(bFromConstraint.Rotation); + + // Calculate the current error + InitialError = CalculateError(MotionBFromA); + } + + // Solve the Jacobian + public void Solve(ref MotionVelocity velocityA, ref MotionVelocity velocityB, float timestep) + { + // Predict the relative orientation at the end of the step + quaternion futureMotionBFromA = JacobianUtilities.IntegrateOrientationBFromA(MotionBFromA, velocityA.AngularVelocity, velocityB.AngularVelocity, timestep); + + // Calculate the effective mass + float3 axisInMotionB = math.mul(futureMotionBFromA, -AxisInMotionA); + float effectiveMass; + { + float invEffectiveMass = math.csum(AxisInMotionA * AxisInMotionA * velocityA.InverseInertiaAndMass.xyz + + axisInMotionB * axisInMotionB * velocityB.InverseInertiaAndMass.xyz); + effectiveMass = math.select(1.0f / invEffectiveMass, 0.0f, invEffectiveMass == 0.0f); + } + + // Calculate the error, adjust by tau and damping, and apply an impulse to correct it + float futureError = CalculateError(futureMotionBFromA); + float solveError = JacobianUtilities.CalculateCorrection(futureError, InitialError, Tau, Damping); + float impulse = math.mul(effectiveMass, -solveError) * (1.0f / timestep); + velocityA.ApplyAngularImpulse(impulse * AxisInMotionA); + velocityB.ApplyAngularImpulse(impulse * axisInMotionB); + } + + // Helper function + private float CalculateError(quaternion motionBFromA) + { + // Calculate the relative body rotation + quaternion jointBFromA = math.mul(math.mul(math.inverse(MotionBFromJoint), motionBFromA), MotionAFromJoint); + + // Find the twist angle of the rotation. + // + // There is no one correct solution for the twist angle. Suppose the joint models a pair of bodies connected by + // three gimbals, one of which is limited by this jacobian. There are multiple configurations of the gimbals that + // give the bodies the same relative orientation, so it is impossible to determine the configuration from the + // bodies' orientations alone, nor therefore the orientation of the limited gimbal. + // + // This code instead makes a reasonable guess, the twist angle of the swing-twist decomposition of the bodies' + // relative orientation. It always works when the limited axis itself is unable to rotate freely, as in a limited + // hinge. It works fairly well when the limited axis can only rotate a small amount, preferably less than 90 + // degrees. It works poorly at higher angles, especially near 180 degrees where it is not continuous. For systems + // that require that kind of flexibility, the gimbals should be modeled as separate bodies. + float angle = CalculateTwistAngle(jointBFromA, AxisIndex); + + // Angle is in [-2pi, 2pi]. + // For comparison against the limits, find k so that angle + 2k * pi is as close to [min, max] as possible. + float centerAngle = (MinAngle + MaxAngle) / 2.0f; + bool above = angle > (centerAngle + (float)math.PI); + bool below = angle < (centerAngle - (float)math.PI); + angle = math.select(angle, angle - 2.0f * (float)math.PI, above); + angle = math.select(angle, angle + 2.0f * (float)math.PI, below); + + // Calculate the relative angle about the twist axis + return JacobianUtilities.CalculateError(angle, MinAngle, MaxAngle); + } + } +} diff --git a/package/Unity.Physics/Dynamics/Jacobians/AngularLimit1DJacobian.cs.meta b/package/Unity.Physics/Dynamics/Jacobians/AngularLimit1DJacobian.cs.meta new file mode 100755 index 000000000..554715e62 --- /dev/null +++ b/package/Unity.Physics/Dynamics/Jacobians/AngularLimit1DJacobian.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: a34e365e99ef51f4d93fa95efbb69854 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Unity.Physics/Dynamics/Jacobians/AngularLimit2DJacobian.cs b/package/Unity.Physics/Dynamics/Jacobians/AngularLimit2DJacobian.cs new file mode 100755 index 000000000..b5575e400 --- /dev/null +++ b/package/Unity.Physics/Dynamics/Jacobians/AngularLimit2DJacobian.cs @@ -0,0 +1,107 @@ +using Unity.Mathematics; +using static Unity.Physics.Math; + +namespace Unity.Physics +{ + // Solve data for a constraint that limits two degrees of angular freedom + public struct AngularLimit2DJacobian + { + // Free axes in motion space + public float3 AxisAinA; + public float3 AxisBinB; + + // Relative angle limits + public float MinAngle; + public float MaxAngle; + + // Relative orientation before solving + public quaternion BFromA; + + // Error before solving + public float InitialError; + + // Fraction of the position error to correct per step + public float Tau; + + // Fraction of the velocity error to correct per step + public float Damping; + + // Build the Jacobian + public void Build( + MTransform aFromConstraint, MTransform bFromConstraint, + MotionVelocity velocityA, MotionVelocity velocityB, + MotionData motionA, MotionData motionB, + Constraint constraint, float tau, float damping) + { + this = default(AngularLimit2DJacobian); + + // Copy the constraint data + int freeIndex = constraint.FreeAxis2D; + AxisAinA = aFromConstraint.Rotation[freeIndex]; + AxisBinB = bFromConstraint.Rotation[freeIndex]; + MinAngle = constraint.Min; + MaxAngle = constraint.Max; + Tau = tau; + Damping = damping; + BFromA = math.mul(math.inverse(motionB.WorldFromMotion.rot), motionA.WorldFromMotion.rot); + + // Calculate the initial error + { + float3 axisAinB = math.mul(BFromA, AxisAinA); + float sinAngle = math.length(math.cross(axisAinB, AxisBinB)); + float cosAngle = math.dot(axisAinB, AxisBinB); + float angle = math.atan2(sinAngle, cosAngle); + InitialError = JacobianUtilities.CalculateError(angle, MinAngle, MaxAngle); + } + } + + // Solve the Jacobian + public void Solve(ref MotionVelocity velocityA, ref MotionVelocity velocityB, float timestep) + { + // Predict the relative orientation at the end of the step + quaternion futureBFromA = JacobianUtilities.IntegrateOrientationBFromA(BFromA, velocityA.AngularVelocity, velocityB.AngularVelocity, timestep); + + // Calculate the jacobian axis and angle + float3 axisAinB = math.mul(futureBFromA, AxisAinA); + float3 jacB0 = math.cross(axisAinB, AxisBinB); + float3 jacA0 = math.mul(math.inverse(futureBFromA), -jacB0); + float jacLengthSq = math.lengthsq(jacB0); + float invJacLength = Math.RSqrtSafe(jacLengthSq); + float futureAngle; + { + float sinAngle = jacLengthSq * invJacLength; + float cosAngle = math.dot(axisAinB, AxisBinB); + futureAngle = math.atan2(sinAngle, cosAngle); + } + + // Choose a second jacobian axis perpendicular to A + float3 jacB1 = math.cross(jacB0, axisAinB); + float3 jacA1 = math.mul(math.inverse(futureBFromA), -jacB1); + + // Calculate effective mass + float2 effectiveMass; // First column of the 2x2 matrix, we don't need the second column because the second component of error is zero + { + // Calculate the inverse effective mass matrix, then invert it + float invEffMassDiag0 = math.csum(jacA0 * jacA0 * velocityA.InverseInertiaAndMass.xyz + jacB0 * jacB0 * velocityB.InverseInertiaAndMass.xyz); + float invEffMassDiag1 = math.csum(jacA1 * jacA1 * velocityA.InverseInertiaAndMass.xyz + jacB1 * jacB1 * velocityB.InverseInertiaAndMass.xyz); + float invEffMassOffDiag = math.csum(jacA0 * jacA1 * velocityA.InverseInertiaAndMass.xyz + jacB0 * jacB1 * velocityB.InverseInertiaAndMass.xyz); + float det = invEffMassDiag0 * invEffMassDiag1 - invEffMassOffDiag * invEffMassOffDiag; + float invDet = math.select(jacLengthSq / det, 0.0f, det == 0.0f); // scale by jacLengthSq because the jacs were not normalized + effectiveMass = invDet * new float2(invEffMassDiag1, -invEffMassOffDiag); + } + + // Normalize the jacobians + jacA0 *= invJacLength; + jacB0 *= invJacLength; + jacA1 *= invJacLength; + jacB1 *= invJacLength; + + // Calculate the error, adjust by tau and damping, and apply an impulse to correct it + float futureError = JacobianUtilities.CalculateError(futureAngle, MinAngle, MaxAngle); + float solveError = JacobianUtilities.CalculateCorrection(futureError, InitialError, Tau, Damping); + float2 impulse = -effectiveMass * solveError * (1.0f / timestep); + velocityA.ApplyAngularImpulse(impulse.x * jacA0 + impulse.y * jacA1); + velocityB.ApplyAngularImpulse(impulse.x * jacB0 + impulse.y * jacB1); + } + } +} diff --git a/package/Unity.Physics/Dynamics/Jacobians/AngularLimit2DJacobian.cs.meta b/package/Unity.Physics/Dynamics/Jacobians/AngularLimit2DJacobian.cs.meta new file mode 100755 index 000000000..e5f640c67 --- /dev/null +++ b/package/Unity.Physics/Dynamics/Jacobians/AngularLimit2DJacobian.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: a0e5ac4c1f05c904aa3a2aefda712342 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Unity.Physics/Dynamics/Jacobians/AngularLimit3DJacobian.cs b/package/Unity.Physics/Dynamics/Jacobians/AngularLimit3DJacobian.cs new file mode 100755 index 000000000..dbfec6d5f --- /dev/null +++ b/package/Unity.Physics/Dynamics/Jacobians/AngularLimit3DJacobian.cs @@ -0,0 +1,98 @@ +using Unity.Mathematics; +using static Unity.Physics.Math; + +namespace Unity.Physics +{ + // Solve data for a constraint that limits three degrees of angular freedom + public struct AngularLimit3DJacobian + { + // Relative angle limits + public float MinAngle; + public float MaxAngle; + + // Relative orientation before solving + public quaternion BFromA; + + // Rotation to joint space from motion space + public quaternion MotionAFromJoint; + public quaternion MotionBFromJoint; + + // Error before solving + public float InitialError; + + public float Tau; + public float Damping; + + // Build the Jacobian + public void Build( + MTransform aFromConstraint, MTransform bFromConstraint, + MotionVelocity velocityA, MotionVelocity velocityB, + MotionData motionA, MotionData motionB, + Constraint constraint, float tau, float damping) + { + this = default(AngularLimit3DJacobian); + + BFromA = math.mul(math.inverse(motionB.WorldFromMotion.rot), motionA.WorldFromMotion.rot); + MotionAFromJoint = new quaternion(aFromConstraint.Rotation); + MotionBFromJoint = new quaternion(bFromConstraint.Rotation); + MinAngle = constraint.Min; + MaxAngle = constraint.Max; + Tau = tau; + Damping = damping; + + float initialAngle = math.atan2(math.length(BFromA.value.xyz), BFromA.value.w) * 2.0f; + InitialError = JacobianUtilities.CalculateError(initialAngle, MinAngle, MaxAngle); + } + + public void Solve(ref MotionVelocity velocityA, ref MotionVelocity velocityB, float timestep) + { + // Predict the relative orientation at the end of the step + quaternion futureBFromA = JacobianUtilities.IntegrateOrientationBFromA(BFromA, velocityA.AngularVelocity, velocityB.AngularVelocity, timestep); + + // Find the future axis and angle of rotation between the free axes + float3 jacA0, jacA1, jacA2, jacB0, jacB1, jacB2; + float3 effectiveMass; // first column of 3x3 effective mass matrix, don't need the others because only jac0 can have nonzero error + float futureAngle; + { + // Calculate the relative rotation between joint spaces + quaternion motionAFromJointB = math.mul(math.inverse(futureBFromA), MotionBFromJoint); + quaternion jointBFromAInA = math.mul(math.inverse(motionAFromJointB), MotionAFromJoint); + + // Find the axis and angle of rotation + jacA0 = jointBFromAInA.value.xyz; + float sinHalfAngleSq = math.lengthsq(jacA0); + float invSinHalfAngle = Math.RSqrtSafe(sinHalfAngleSq); + float sinHalfAngle = sinHalfAngleSq * invSinHalfAngle; + futureAngle = math.atan2(sinHalfAngle, futureBFromA.value.w) * 2.0f; + + jacA0 = math.select(jacA0 * invSinHalfAngle, new float3(1, 0, 0), invSinHalfAngle == 0.0f); + Math.CalculatePerpendicularNormalized(jacA0, out jacA1, out jacA2); + + jacB0 = math.mul(futureBFromA, -jacA0); + jacB1 = math.mul(futureBFromA, -jacA1); + jacB2 = math.mul(futureBFromA, -jacA2); + + // Calculate the effective mass + float3 invEffectiveMassDiag = new float3( + math.csum(jacA0 * jacA0 * velocityA.InverseInertiaAndMass.xyz + jacB0 * jacB0 * velocityB.InverseInertiaAndMass.xyz), + math.csum(jacA1 * jacA1 * velocityA.InverseInertiaAndMass.xyz + jacB1 * jacB1 * velocityB.InverseInertiaAndMass.xyz), + math.csum(jacA2 * jacA2 * velocityA.InverseInertiaAndMass.xyz + jacB2 * jacB2 * velocityB.InverseInertiaAndMass.xyz)); + float3 invEffectiveMassOffDiag = new float3( + math.csum(jacA0 * jacA1 * velocityA.InverseInertiaAndMass.xyz + jacB0 * jacB1 * velocityB.InverseInertiaAndMass.xyz), + math.csum(jacA0 * jacA2 * velocityA.InverseInertiaAndMass.xyz + jacB0 * jacB2 * velocityB.InverseInertiaAndMass.xyz), + math.csum(jacA1 * jacA2 * velocityA.InverseInertiaAndMass.xyz + jacB1 * jacB2 * velocityB.InverseInertiaAndMass.xyz)); + JacobianUtilities.InvertSymmetricMatrix(invEffectiveMassDiag, invEffectiveMassOffDiag, out float3 effectiveMassDiag, out float3 effectiveMassOffDiag); + effectiveMass = JacobianUtilities.BuildSymmetricMatrix(effectiveMassDiag, effectiveMassOffDiag).c0; + } + + // Calculate the error, adjust by tau and damping, and apply an impulse to correct it + float futureError = JacobianUtilities.CalculateError(futureAngle, MinAngle, MaxAngle); + float solveError = JacobianUtilities.CalculateCorrection(futureError, InitialError, Tau, Damping); + float solveVelocity = -solveError * (1.0f / timestep); + float3 impulseA = solveVelocity * (jacA0 * effectiveMass.x + jacA1 * effectiveMass.y + jacA2 * effectiveMass.z); + float3 impulseB = solveVelocity * (jacB0 * effectiveMass.x + jacB1 * effectiveMass.y + jacB2 * effectiveMass.z); + velocityA.ApplyAngularImpulse(impulseA); + velocityB.ApplyAngularImpulse(impulseB); + } + } +} diff --git a/package/Unity.Physics/Dynamics/Jacobians/AngularLimit3DJacobian.cs.meta b/package/Unity.Physics/Dynamics/Jacobians/AngularLimit3DJacobian.cs.meta new file mode 100755 index 000000000..5ffa66e89 --- /dev/null +++ b/package/Unity.Physics/Dynamics/Jacobians/AngularLimit3DJacobian.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: f3a650279a219ff48afd0f45ac019834 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Unity.Physics/Dynamics/Jacobians/ContactJacobian.cs b/package/Unity.Physics/Dynamics/Jacobians/ContactJacobian.cs new file mode 100755 index 000000000..695d6f78e --- /dev/null +++ b/package/Unity.Physics/Dynamics/Jacobians/ContactJacobian.cs @@ -0,0 +1,256 @@ +using Unity.Collections; +using Unity.Mathematics; + +namespace Unity.Physics +{ + public struct ContactJacobianAngular + { + public float3 AngularA; + public float3 AngularB; + public float EffectiveMass; + public float Impulse; // Accumulated impulse + } + + public struct ContactJacAngAndVelToReachCp + { + public ContactJacobianAngular Jac; + + // Velocity needed to reach the contact plane in one frame, + // both if approaching (negative) and depenetrating (positive) + public float VelToReachCp; + } + + public struct SurfaceVelocity + { + // 0 and 1 for linear, 2 for angular + public float3 ExtraFrictionDv; + } + + public struct MassFactors + { + // TODO: mark these internal, add separate properties for InvInertiaFactor and InvMassFactor instead? + public float4 InvInertiaAndMassFactorA; + public float4 InvInertiaAndMassFactorB; + + public static MassFactors Default => new MassFactors + { + InvInertiaAndMassFactorA = new float4(1.0f), + InvInertiaAndMassFactorB = new float4(1.0f) + }; + } + + public struct BaseContactJacobian + { + public int NumContacts; + public float3 Normal; + + internal static float GetJacVelocity(float3 linear, ContactJacobianAngular jacAngular, MotionVelocity velocityA, MotionVelocity velocityB) + { + float3 temp = (velocityA.LinearVelocity - velocityB.LinearVelocity) * linear; + temp += velocityA.AngularVelocity * jacAngular.AngularA; + temp += velocityB.AngularVelocity * jacAngular.AngularB; + return math.csum(temp); + } + } + + // A Jacobian representing a set of contact points that apply impulses + public struct ContactJacobian + { + public BaseContactJacobian BaseJacobian; + public float CoefficientOfFriction; + public float CoefficientOfRestitution; + + // Linear friction jacobians. Only store the angular part, linear part can be recalculated from BaseJacobian.Normal + public ContactJacobianAngular Friction0; // EffectiveMass stores friction effective mass matrix element (0, 0) + public ContactJacobianAngular Friction1; // EffectiveMass stores friction effective mass matrix element (1, 1) + + // Angular friction about the contact normal, no linear part + public ContactJacobianAngular AngularFriction; // EffectiveMass stores friction effective mass matrix element (2, 2) + public float3 FrictionEffectiveMassOffDiag; // Effective mass matrix (0, 1), (0, 2), (1, 2) == (1, 0), (2, 0), (2, 1) + + // Solve the Jacobian + public void Solve( + ref JacobianHeader jacHeader, ref MotionVelocity velocityA, ref MotionVelocity velocityB, + Solver.StepInput stepInput, ref BlockStream.Writer collisionEventsWriter) + { + // Copy velocity data + MotionVelocity tempVelocityA = velocityA; + MotionVelocity tempVelocityB = velocityB; + if (jacHeader.HasMassFactors) + { + MassFactors jacMod = jacHeader.AccessMassFactors(); + tempVelocityA.InverseInertiaAndMass *= jacMod.InvInertiaAndMassFactorA; + tempVelocityB.InverseInertiaAndMass *= jacMod.InvInertiaAndMassFactorB; + } + + // Calculate maximum impulse per sub step + float maxImpulseToApply; + if (jacHeader.HasMaxImpulse) + maxImpulseToApply = jacHeader.AccessMaxImpulse() * stepInput.Timestep * stepInput.InvNumSolverIterations; + else + maxImpulseToApply = float.MaxValue; + + // Solve normal impulses + float sumImpulses = 0.0f; + float frictionFactor = 1.0f; + float4 totalAccumulatedImpulses = float4.zero; + bool forceCollisionEvent = false; + for (int j = 0; j < BaseJacobian.NumContacts; j++) + { + ref ContactJacAngAndVelToReachCp jacAngular = ref jacHeader.AccessAngularJacobian(j); + + // Solve velocity so that predicted contact distance is greater than or equal to zero + float relativeVelocity = BaseContactJacobian.GetJacVelocity(BaseJacobian.Normal, jacAngular.Jac, tempVelocityA, tempVelocityB); + float dv = jacAngular.VelToReachCp - relativeVelocity; + + // Restitution (typically set to zero) + if (dv > 0.0f && CoefficientOfRestitution > 0.0f) + { + float negContactRestingVelocity = -stepInput.GravityLength * stepInput.Timestep * 1.5f; + if (relativeVelocity < negContactRestingVelocity) + { + float invMassA = tempVelocityA.InverseInertiaAndMass.w; + float invMassB = tempVelocityB.InverseInertiaAndMass.w; + float effInvMassAtCenter = invMassA + invMassB; + jacAngular.VelToReachCp = -(CoefficientOfRestitution * (relativeVelocity - negContactRestingVelocity)) * + (jacAngular.Jac.EffectiveMass * effInvMassAtCenter); + dv = jacAngular.VelToReachCp - relativeVelocity; + + // reduce friction to 1/4 of the impulse + frictionFactor = 0.25f; + } + } + + float impulse = dv * jacAngular.Jac.EffectiveMass; + bool clipped = impulse > maxImpulseToApply; + impulse = math.min(impulse, maxImpulseToApply); + float accumulatedImpulse = math.max(jacAngular.Jac.Impulse + impulse, 0.0f); + if (accumulatedImpulse != jacAngular.Jac.Impulse) + { + float deltaImpulse = accumulatedImpulse - jacAngular.Jac.Impulse; + ApplyImpulse(deltaImpulse, BaseJacobian.Normal, jacAngular.Jac, ref tempVelocityA, ref tempVelocityB); + } + + jacAngular.Jac.Impulse = accumulatedImpulse; + sumImpulses += accumulatedImpulse; + + // If there are more than 4 contacts, accumulate their impulses to the last contact point + totalAccumulatedImpulses[math.min(j, 3)] += jacAngular.Jac.Impulse; + + // Force contact event even when no impulse is applied, but there is penetration. + // Also force when impulse was clipped, even if clipped to 0. + forceCollisionEvent |= jacAngular.VelToReachCp > 0.0f || clipped; + } + + // Export collision event + if (stepInput.IsLastIteration && (math.any(totalAccumulatedImpulses > 0.0f) || forceCollisionEvent) && jacHeader.HasColliderKeys) + { + collisionEventsWriter.Write(new CollisionEvent + { + BodyIndices = jacHeader.BodyPair, + ColliderKeys = jacHeader.AccessColliderKeys(), + Normal = BaseJacobian.Normal, + AccumulatedImpulses = totalAccumulatedImpulses + }); + } + + // Solve friction + if (sumImpulses > 0.0f) + { + // Choose friction axes + Math.CalculatePerpendicularNormalized(BaseJacobian.Normal, out float3 frictionDir0, out float3 frictionDir1); + + // Calculate impulses for full stop + float3 imp; + { + // Calculate the jacobian dot velocity for each of the friction jacobians + float3 extraFrictionDv = jacHeader.HasSurfaceVelocity ? jacHeader.AccessSurfaceVelocity().ExtraFrictionDv : float3.zero; + float dv0 = extraFrictionDv.x - BaseContactJacobian.GetJacVelocity(frictionDir0, Friction0, tempVelocityA, tempVelocityB); + float dv1 = extraFrictionDv.y - BaseContactJacobian.GetJacVelocity(frictionDir1, Friction1, tempVelocityA, tempVelocityB); + float dva = extraFrictionDv.z - math.csum(AngularFriction.AngularA * tempVelocityA.AngularVelocity + AngularFriction.AngularB * tempVelocityB.AngularVelocity); + + // Reassemble the effective mass matrix + float3 effectiveMassDiag = new float3(Friction0.EffectiveMass, Friction1.EffectiveMass, AngularFriction.EffectiveMass); + float3x3 effectiveMass = JacobianUtilities.BuildSymmetricMatrix(effectiveMassDiag, FrictionEffectiveMassOffDiag); + + // Calculate the impulse + imp = math.mul(effectiveMass, new float3(dv0, dv1, dva)); + imp *= frictionFactor; + } + + // Clip TODO.ma calculate some contact radius and use it to influence balance between linear and angular friction + float maxImpulse = sumImpulses * CoefficientOfFriction * stepInput.InvNumSolverIterations; + float frictionImpulseSquared = math.lengthsq(imp); + imp *= math.min(1.0f, maxImpulse * math.rsqrt(frictionImpulseSquared)); + + // Apply impulses + ApplyImpulse(imp.x, frictionDir0, Friction0, ref tempVelocityA, ref tempVelocityB); + ApplyImpulse(imp.y, frictionDir1, Friction1, ref tempVelocityA, ref tempVelocityB); + + tempVelocityA.ApplyAngularImpulse(imp.z * AngularFriction.AngularA); + tempVelocityB.ApplyAngularImpulse(imp.z * AngularFriction.AngularB); + + // Accumulate them + Friction0.Impulse += imp.x; + Friction1.Impulse += imp.y; + AngularFriction.Impulse += imp.z; + } + + // Write back + velocityA = tempVelocityA; + velocityB = tempVelocityB; + } + + // Helper function + private static void ApplyImpulse( + float impulse, float3 linear, ContactJacobianAngular jacAngular, + ref MotionVelocity velocityA, ref MotionVelocity velocityB) + { + velocityA.ApplyLinearImpulse(impulse * linear); + velocityB.ApplyLinearImpulse(-impulse * linear); + velocityA.ApplyAngularImpulse(impulse * jacAngular.AngularA); + velocityB.ApplyAngularImpulse(impulse * jacAngular.AngularB); + } + } + + // A Jacobian representing a set of contact points that export trigger events + public struct TriggerJacobian + { + public BaseContactJacobian BaseJacobian; + public ColliderKeyPair ColliderKeys; + + // Solve the Jacobian + public void Solve( + ref JacobianHeader jacHeader, ref MotionVelocity velocityA, ref MotionVelocity velocityB, Solver.StepInput stepInput, + ref BlockStream.Writer triggerEventsWriter) + { + // Export trigger events only in last iteration + if (!stepInput.IsLastIteration) + { + return; + } + + for (int j = 0; j < BaseJacobian.NumContacts; j++) + { + ref ContactJacAngAndVelToReachCp jacAngular = ref jacHeader.AccessAngularJacobian(j); + + // Solve velocity so that predicted contact distance is greater than or equal to zero + float relativeVelocity = BaseContactJacobian.GetJacVelocity(BaseJacobian.Normal, jacAngular.Jac, velocityA, velocityB); + float dv = jacAngular.VelToReachCp - relativeVelocity; + float impulse = dv * jacAngular.Jac.EffectiveMass; + float accumulatedImpulse = math.max(jacAngular.Jac.Impulse + impulse, 0.0f); + if (accumulatedImpulse != jacAngular.Jac.Impulse || jacAngular.VelToReachCp > 0.0f) + { + // Export trigger event only if impulse is applied, or objects are penetrating + triggerEventsWriter.Write(new TriggerEvent + { + BodyIndices = jacHeader.BodyPair, + ColliderKeys = ColliderKeys, + }); + + return; + } + } + } + } +} diff --git a/package/Unity.Physics/Dynamics/Jacobians/ContactJacobian.cs.meta b/package/Unity.Physics/Dynamics/Jacobians/ContactJacobian.cs.meta new file mode 100755 index 000000000..656af20be --- /dev/null +++ b/package/Unity.Physics/Dynamics/Jacobians/ContactJacobian.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: e841cafb297d7264b96b8666fcf50b55 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Unity.Physics/Dynamics/Jacobians/ContactJacobianMemoryLayout.md b/package/Unity.Physics/Dynamics/Jacobians/ContactJacobianMemoryLayout.md new file mode 100755 index 000000000..81be19043 --- /dev/null +++ b/package/Unity.Physics/Dynamics/Jacobians/ContactJacobianMemoryLayout.md @@ -0,0 +1,42 @@ +# Contact jacobian memory layout + +| **Start byte** | **End byte** | **Member name** | +----------------------------------|--------------|---------------------------------------------| + **JacobianHeader** | | | + 0 | 7 | BodyPair (2 ints) | + 8 | 8 | Type (byte) | + 9 | 9 | JacModFlags (byte) | + **BaseContactJacobian** | | | + 0 | 3 | NumContacts (int) | + 4 | 15 | Normal (float3) | + **ContactJacobianAngular** | | | + 0 | 11 | AngularA (float3) | + 12 | 23 | AngularB (float3) | + 24 | 27 | EffectiveMass (float) | + 28 | 31 | Impulse (float) | + **ContactJacAngAndVelToReachCP** | | | + 0 | 31 | Jac (ContactJacobianAngular) | + 32 | 35 | VelToReachCP (float) | + **ContactJacobian** | | | + 0 | 15 | BaseJac (BaseContactJacobian) | + 16 | 19 | CoefficientOfFriction (float) | + 20 | 23 | CoefficientOfRestitution (float) | + 24 | 55 | Friction0 (ContactJacobianAngular) | + 56 | 87 | Friction1 (ContactJacobianAngular) | + 88 | 91 | FrictionEffectiveMassNonDiag (float) | + 92 | 123 | AngularFriction (ContactJacobianAngular) | + **TriggerJacobian** | | | + 0 | 15 | BaseJac (BaseContactJacobian) | + 16 | 23 | ColliderKeys (2 ints) | + + **Contact jacobian types** | +-------------------------------------------------| + **JacobianType.Contact** | + JacobianHeader | + ContactJacobian | + Modifier data (based on JacModFlags) | + NumContacts * ContactJacAngAndVelToReachCP | + **JacobianType.Trigger** | + JacobianHeader | + TriggerJacobian | + NumContacts * ContactJacAngAndVelToReachCP | \ No newline at end of file diff --git a/package/Unity.Physics/Dynamics/Jacobians/ContactJacobianMemoryLayout.md.meta b/package/Unity.Physics/Dynamics/Jacobians/ContactJacobianMemoryLayout.md.meta new file mode 100755 index 000000000..cf4ae152b --- /dev/null +++ b/package/Unity.Physics/Dynamics/Jacobians/ContactJacobianMemoryLayout.md.meta @@ -0,0 +1,7 @@ +fileFormatVersion: 2 +guid: 49d4596482a51304aa5667cdb92739b2 +TextScriptImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Unity.Physics/Dynamics/Jacobians/Jacobian.cs b/package/Unity.Physics/Dynamics/Jacobians/Jacobian.cs new file mode 100755 index 000000000..a07ce9971 --- /dev/null +++ b/package/Unity.Physics/Dynamics/Jacobians/Jacobian.cs @@ -0,0 +1,470 @@ +using System; +using Unity.Collections; +using Unity.Collections.LowLevel.Unsafe; +using Unity.Mathematics; +using UnityEngine.Assertions; + +namespace Unity.Physics +{ + public enum JacobianType : byte + { + // Contact Jacobians + Contact, + Trigger, + + // Joint Jacobians + LinearLimit, + AngularLimit1D, + AngularLimit2D, + AngularLimit3D + } + + // Flags which enable optional Jacobian behaviors + [Flags] + public enum JacobianFlags : byte + { + // These flags apply to all Jacobians + Disabled = 1 << 0, + EnableMassFactors = 1 << 1, + UserFlag0 = 1 << 2, + UserFlag1 = 1 << 3, + + // These flags apply only to contact Jacobians + IsTrigger = 1 << 4, + EnableCollisionEvents = 1 << 5, + EnableSurfaceVelocity = 1 << 6, + EnableMaxImpulse = 1 << 7 + } + + // Jacobian header, first part of each Jacobian in the stream + public struct JacobianHeader + { + public BodyIndexPair BodyPair { get; internal set; } + public JacobianType Type { get; internal set; } + public JacobianFlags Flags { get; internal set; } + + // Whether the Jacobian should be solved or not + public bool Enabled + { + get => ((Flags & JacobianFlags.Disabled) == 0); + set => Flags = value ? (Flags & ~JacobianFlags.Disabled) : (Flags | JacobianFlags.Disabled); + } + + // Collider keys for the collision events + public bool HasColliderKeys => (Flags & JacobianFlags.EnableCollisionEvents) != 0; + public ColliderKeyPair ColliderKeys + { + get => HasColliderKeys ? AccessColliderKeys() : ColliderKeyPair.Empty; + set + { + if (HasColliderKeys) + AccessColliderKeys() = value; + else + throw new NotSupportedException("Jacobian does not have collision events enabled"); + } + } + + // Overrides for the mass properties of the pair of bodies + public bool HasMassFactors => (Flags & JacobianFlags.EnableMassFactors) != 0; + public MassFactors MassFactors + { + get => HasMassFactors ? AccessMassFactors() : MassFactors.Default; + set + { + if (HasMassFactors) + AccessMassFactors() = value; + else + throw new NotSupportedException("Jacobian does not have mass factors enabled"); + } + } + + // The surface velocity to apply to contact points + public bool HasSurfaceVelocity => (Flags & JacobianFlags.EnableSurfaceVelocity) != 0; + public SurfaceVelocity SurfaceVelocity + { + get => HasSurfaceVelocity ? AccessSurfaceVelocity() : new SurfaceVelocity(); + set + { + if (HasSurfaceVelocity) + AccessSurfaceVelocity() = value; + else + throw new NotSupportedException("Jacobian does not have surface velocity enabled"); + } + } + + // The maximum impulse that can be applied by contact points + public bool HasMaxImpulse => (Flags & JacobianFlags.EnableMaxImpulse) != 0; + public float MaxImpulse + { + get => HasMaxImpulse ? AccessMaxImpulse() : float.MaxValue; + set + { + if (HasMaxImpulse) + AccessMaxImpulse() = value; + else + throw new NotSupportedException("Jacobian does not have max impulse enabled"); + } + } + + // Solve the Jacobian + public void Solve(ref MotionVelocity velocityA, ref MotionVelocity velocityB, Solver.StepInput stepInput, + ref BlockStream.Writer collisionEventsWriter, ref BlockStream.Writer triggerEventsWriter) + { + if (Enabled) + { + switch (Type) + { + case JacobianType.Contact: + AccessBaseJacobian().Solve(ref this, ref velocityA, ref velocityB, stepInput, ref collisionEventsWriter); + break; + case JacobianType.Trigger: + AccessBaseJacobian().Solve(ref this, ref velocityA, ref velocityB, stepInput, ref triggerEventsWriter); + break; + case JacobianType.LinearLimit: + AccessBaseJacobian().Solve(ref velocityA, ref velocityB, stepInput.Timestep); + break; + case JacobianType.AngularLimit1D: + AccessBaseJacobian().Solve(ref velocityA, ref velocityB, stepInput.Timestep); + break; + case JacobianType.AngularLimit2D: + AccessBaseJacobian().Solve(ref velocityA, ref velocityB, stepInput.Timestep); + break; + case JacobianType.AngularLimit3D: + AccessBaseJacobian().Solve(ref velocityA, ref velocityB, stepInput.Timestep); + break; + default: + throw new NotImplementedException(); + } + } + } + + #region Helpers + + public static int CalculateSize(JacobianType type, JacobianFlags flags, int numContactPoints = 0) + { + return UnsafeUtility.SizeOf() + + SizeOfBaseJacobian(type) + SizeOfModifierData(type, flags) + + numContactPoints * UnsafeUtility.SizeOf(); + } + + private static int SizeOfColliderKeys(JacobianType type, JacobianFlags flags) + { + return (type == JacobianType.Contact && (flags & JacobianFlags.EnableCollisionEvents) != 0) ? + UnsafeUtility.SizeOf() : 0; + } + + private static int SizeOfSurfaceVelocity(JacobianType type, JacobianFlags flags) + { + return (type == JacobianType.Contact && (flags & JacobianFlags.EnableSurfaceVelocity) != 0) ? + UnsafeUtility.SizeOf() : 0; + } + + private static int SizeOfMaxImpulse(JacobianType type, JacobianFlags flags) + { + return (type == JacobianType.Contact && (flags & JacobianFlags.EnableMaxImpulse) != 0) ? + UnsafeUtility.SizeOf() : 0; + } + + private static int SizeOfMassFactors(JacobianType type, JacobianFlags flags) + { + return (type == JacobianType.Contact && (flags & JacobianFlags.EnableMassFactors) != 0) ? + UnsafeUtility.SizeOf() : 0; + } + + private static int SizeOfModifierData(JacobianType type, JacobianFlags flags) + { + return SizeOfColliderKeys(type, flags) + SizeOfSurfaceVelocity(type, flags) + + SizeOfMaxImpulse(type, flags) + SizeOfMassFactors(type, flags); + } + + private static int SizeOfBaseJacobian(JacobianType type) + { + switch (type) + { + case JacobianType.Contact: + return UnsafeUtility.SizeOf(); + case JacobianType.Trigger: + return UnsafeUtility.SizeOf(); + case JacobianType.LinearLimit: + return UnsafeUtility.SizeOf(); + case JacobianType.AngularLimit1D: + return UnsafeUtility.SizeOf(); + case JacobianType.AngularLimit2D: + return UnsafeUtility.SizeOf(); + case JacobianType.AngularLimit3D: + return UnsafeUtility.SizeOf(); + default: + throw new NotImplementedException(); + } + } + + // Access to "base" jacobian - a jacobian that comes after the header + public unsafe ref T AccessBaseJacobian() where T : struct + { + byte* ptr = (byte*)UnsafeUtility.AddressOf(ref this); + ptr += UnsafeUtility.SizeOf(); + return ref UnsafeUtilityEx.AsRef(ptr); + } + + public unsafe ref ColliderKeyPair AccessColliderKeys() + { + Assert.IsTrue((Flags & JacobianFlags.EnableCollisionEvents) != 0); + byte* ptr = (byte*)UnsafeUtility.AddressOf(ref this); + ptr += UnsafeUtility.SizeOf() + SizeOfBaseJacobian(Type); + return ref UnsafeUtilityEx.AsRef(ptr); + } + + public unsafe ref SurfaceVelocity AccessSurfaceVelocity() + { + Assert.IsTrue((Flags & JacobianFlags.EnableSurfaceVelocity) != 0); + byte* ptr = (byte*)UnsafeUtility.AddressOf(ref this); + ptr += UnsafeUtility.SizeOf() + SizeOfBaseJacobian(Type) + + SizeOfColliderKeys(Type, Flags); + return ref UnsafeUtilityEx.AsRef(ptr); + } + + public unsafe ref float AccessMaxImpulse() + { + Assert.IsTrue((Flags & JacobianFlags.EnableMaxImpulse) != 0); + byte* ptr = (byte*)UnsafeUtility.AddressOf(ref this); + ptr += UnsafeUtility.SizeOf() + SizeOfBaseJacobian(Type) + + SizeOfColliderKeys(Type, Flags) + SizeOfSurfaceVelocity(Type, Flags); + return ref UnsafeUtilityEx.AsRef(ptr); + } + + public unsafe ref MassFactors AccessMassFactors() + { + Assert.IsTrue((Flags & JacobianFlags.EnableMassFactors) != 0); + byte* ptr = (byte*)UnsafeUtility.AddressOf(ref this); + ptr += UnsafeUtility.SizeOf() + SizeOfBaseJacobian(Type) + + SizeOfColliderKeys(Type, Flags) + SizeOfSurfaceVelocity(Type, Flags) + SizeOfMaxImpulse(Type, Flags); + return ref UnsafeUtilityEx.AsRef(ptr); + } + + public unsafe ref ContactJacAngAndVelToReachCp AccessAngularJacobian(int pointIndex) + { + Assert.IsTrue(Type == JacobianType.Contact || Type == JacobianType.Trigger); + byte* ptr = (byte*)UnsafeUtility.AddressOf(ref this); + ptr += UnsafeUtility.SizeOf() + SizeOfBaseJacobian(Type) + SizeOfModifierData(Type, Flags) + + pointIndex * UnsafeUtility.SizeOf(); + return ref UnsafeUtilityEx.AsRef(ptr); + } + + #endregion + } + + // Helper functions for working with Jacobians + public static class JacobianUtilities + { + public static void CalculateTauAndDamping(float springFrequency, float springDampingRatio, float timestep, int iterations, out float tau, out float damping) + { + // TODO + // - it's a significant amount of work to calculate tau and damping. They depend on step length, so they have to be calculated each step. + // probably worth caching tau and damping for the default spring constants on the world and branching. + // - you always get a higher effective damping ratio than you ask for because of the error from to discrete integration. The error varies + // with step length. Can we estimate or bound that error and compensate for it? + + /* + + How to derive these formulas for tau and damping: + + 1) implicit euler integration of a damped spring + + damped spring equation: x'' = -kx - cx' + h = step length + + x2 = x1 + hv2 + v2 = v1 + h(-kx2 - cv2)/m + = v1 + h(-kx1 - hkv2 - cv2)/m + = v1 / (1 + h^2k/m + hc/m) - hkx1 / (m + h^2k + hc) + + 2) gauss-seidel iterations of a stiff constraint. Example for four iterations: + + t = tau, d = damping, a = 1 - d + v2 = av1 - (t / h)x1 + v3 = av2 - (t / h)x1 + v4 = av3 - (t / h)x1 + v5 = av4 - (t / h)x1 + = a^4v1 - (a^3 + a^2 + a + 1)(t / h)x1 + + 3) by matching coefficients of v1 and x1 in the formulas for v2 in step (1) and v5 in step (2), we see that if: + + (1 - damping)^4 = 1 / (1 + h^2k / m + hc / m) + ((1 - damping)^3 + (1 - damping)^2 + (1 - damping) + 1)(tau / h) = hk / (m + h^2k + hc) + + then our constraint is equivalent to the implicit euler integration of a spring. + solve the first equation for damping, then solve the second equation for tau. + then substitute in k = mw^2, c = 2mzw. + + */ + + float h = timestep; + float w = springFrequency * 2.0f * (float)math.PI; // convert oscillations/sec to radians/sec + float z = springDampingRatio; + float hw = h * w; + float hhww = hw * hw; + + // a = 1-d, aExp = a^iterations, aSum = aExp / sum(i in [0, iterations), a^i) + float aExp = 1.0f / (1.0f + hhww + 2.0f * hw * z); + float a, aSum; + if (iterations == 4) + { + // special case expected iterations = 4 + float invA2 = math.rsqrt(aExp); + float a2 = invA2 * aExp; + a = math.rsqrt(invA2); + aSum = (1.0f + a2 + a * (1.0f + a2)); + } + else + { + a = math.pow(aExp, 1.0f / iterations); + aSum = 1.0f; + for (int i = 1; i < iterations; i++) + { + aSum = a * aSum + 1.0f; + } + } + + damping = 1 - a; + tau = hhww * aExp / aSum; + } + + public static void CalculateTauAndDamping(Constraint constraint, float timestep, int iterations, out float tau, out float damping) + { + CalculateTauAndDamping(constraint.SpringFrequency, constraint.SpringDamping, timestep, iterations, out tau, out damping); + } + + // Returns x - clamp(x, min, max) + public static float CalculateError(float x, float min, float max) + { + float error = math.max(x - max, 0.0f); + error = math.min(x - min, error); + return error; + } + + // Returns the amount of error for the solver to correct, where initialError is the pre-integration error and predictedError is the expected post-integration error + public static float CalculateCorrection(float predictedError, float initialError, float tau, float damping) + { + return (predictedError - initialError) * damping + initialError * tau; + } + + // Integrate the relative orientation of a pair of bodies, faster and less memory than storing both bodies' orientations and integrating them separately + public static quaternion IntegrateOrientationBFromA(quaternion bFromA, float3 angularVelocityA, float3 angularVelocityB, float timestep) + { + quaternion dqA = Integrator.IntegrateAngularVelocity(angularVelocityA, timestep); + quaternion dqB = Integrator.IntegrateAngularVelocity(angularVelocityB, timestep); + return math.normalize(math.mul(math.mul(math.inverse(dqB), bFromA), dqA)); + } + + // Calculate the inverse effective mass of a linear jacobian + public static float CalculateInvEffectiveMassDiag( + float3 angA, float4 invInertiaAndMassA, + float3 angB, float4 invInertiaAndMassB) + { + float3 angularPart = angA * angA * invInertiaAndMassA.xyz + angB * angB * invInertiaAndMassB.xyz; + float linearPart = invInertiaAndMassA.w + invInertiaAndMassB.w; + return (angularPart.x + angularPart.y) + (angularPart.z + linearPart); + } + + // Calculate the inverse effective mass for a pair of jacobians with perpendicular linear parts + public static float CalculateInvEffectiveMassOffDiag( + float3 angA0, float3 angA1, float3 invInertiaA, + float3 angB0, float3 angB1, float3 invInertiaB) + { + return math.csum(angA0 * angA1 * invInertiaA + angB0 * angB1 * invInertiaB); + } + + // Inverts a symmetrix 3x3 matrix with diag = (0, 0), (1, 1), (2, 2), offDiag = (0, 1), (0, 2), (1, 2) = (1, 0), (2, 0), (2, 1) + public static bool InvertSymmetricMatrix(float3 diag, float3 offDiag, out float3 invDiag, out float3 invOffDiag) + { + float3 offDiagSq = offDiag.zyx * offDiag.zyx; + float determinant = (Math.HorizontalMul(diag) + 2.0f * Math.HorizontalMul(offDiag) - math.csum(offDiagSq * diag)); + bool determinantOk = (determinant != 0); + float invDeterminant = math.select(0.0f, 1.0f / determinant, determinantOk); + invDiag = (diag.yxx * diag.zzy - offDiagSq) * invDeterminant; + invOffDiag = (offDiag.yxx * offDiag.zzy - diag.zyx * offDiag) * invDeterminant; + return determinantOk; + } + + // Builds a symmetric 3x3 matrix from diag = (0, 0), (1, 1), (2, 2), offDiag = (0, 1), (0, 2), (1, 2) = (1, 0), (2, 0), (2, 1) + public static float3x3 BuildSymmetricMatrix(float3 diag, float3 offDiag) + { + return new float3x3( + new float3(diag.x, offDiag.x, offDiag.y), + new float3(offDiag.x, diag.y, offDiag.z), + new float3(offDiag.y, offDiag.z, diag.z) + ); + } + } + + // Iterator (and modifier) for jacobians + public unsafe struct JacobianIterator + { + BlockStream.Reader m_Reader; + int m_CurrentWorkItem; + readonly bool m_IterateAll; + readonly int m_MaxWorkItemIndex; + + public JacobianIterator(BlockStream.Reader jacobianStreamReader, int workItemIndex, bool iterateAll = false) + { + m_Reader = jacobianStreamReader; + m_IterateAll = iterateAll; + m_MaxWorkItemIndex = workItemIndex; + + if (iterateAll) + { + m_CurrentWorkItem = 0; + MoveReaderToNextForEachIndex(); + } + else + { + m_CurrentWorkItem = workItemIndex; + m_Reader.BeginForEachIndex(workItemIndex); + } + } + + public bool HasJacobiansLeft() + { + return m_Reader.RemainingItemCount > 0; + } + + public ref JacobianHeader ReadJacobianHeader(out short readSize) + { + readSize = Read(); + return ref UnsafeUtilityEx.AsRef(Read(readSize)); + } + + public ref JacobianHeader ReadJacobianHeader() + { + short readSize = Read(); + return ref UnsafeUtilityEx.AsRef(Read(readSize)); + } + + private void MoveReaderToNextForEachIndex() + { + while (m_Reader.RemainingItemCount == 0 && m_CurrentWorkItem < m_MaxWorkItemIndex) + { + m_Reader.BeginForEachIndex(m_CurrentWorkItem); + m_CurrentWorkItem++; + } + } + + private byte* Read(int size) + { + byte* dataPtr = m_Reader.Read(size); + + if (m_IterateAll) + { + MoveReaderToNextForEachIndex(); + } + + return dataPtr; + } + + private ref T Read() where T : struct + { + int size = UnsafeUtility.SizeOf(); + return ref UnsafeUtilityEx.AsRef(Read(size)); + } + } +} diff --git a/package/Unity.Physics/Dynamics/Jacobians/Jacobian.cs.meta b/package/Unity.Physics/Dynamics/Jacobians/Jacobian.cs.meta new file mode 100755 index 000000000..23841c72e --- /dev/null +++ b/package/Unity.Physics/Dynamics/Jacobians/Jacobian.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 892ccbfa9987bf146824f83a1a61c99e +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Unity.Physics/Dynamics/Jacobians/LinearLimitJacobian.cs b/package/Unity.Physics/Dynamics/Jacobians/LinearLimitJacobian.cs new file mode 100755 index 000000000..7908fd050 --- /dev/null +++ b/package/Unity.Physics/Dynamics/Jacobians/LinearLimitJacobian.cs @@ -0,0 +1,206 @@ +using Unity.Mathematics; +using static Unity.Physics.Math; + +namespace Unity.Physics +{ + // Solve data for a constraint that limits the linear distance between a pair of pivots in 1, 2, or 3 degrees of freedom + public struct LinearLimitJacobian + { + // Pivot positions in motion space + public float3 PivotAinA; + public float3 PivotBinB; + + // Pivot distance limits + public float MinDistance; + public float MaxDistance; + + // Motion transforms before solving + public RigidTransform WorldFromA; + public RigidTransform WorldFromB; + + // If the constraint limits 1 DOF, this is the constrained axis. + // If the constraint limits 2 DOF, this is the free axis. + // If the constraint limits 3 DOF, this is unused and set to float3.zero + public float3 AxisInB; + + // True if the jacobian limits one degree of freedom + public bool Is1D; + + // Position error at the beginning of the step + public float InitialError; + + // Fraction of the position error to correct per step + public float Tau; + + // Fraction of the velocity error to correct per step + public float Damping; + + // Build the Jacobian + public void Build( + MTransform aFromConstraint, MTransform bFromConstraint, + MotionVelocity velocityA, MotionVelocity velocityB, + MotionData motionA, MotionData motionB, + Constraint constraint, float tau, float damping) + { + this = default(LinearLimitJacobian); + + WorldFromA = motionA.WorldFromMotion; + WorldFromB = motionB.WorldFromMotion; + + PivotAinA = aFromConstraint.Translation; + PivotBinB = bFromConstraint.Translation; + + AxisInB = float3.zero; + Is1D = false; + + MinDistance = constraint.Min; + MaxDistance = constraint.Max; + + Tau = tau; + Damping = damping; + + // TODO.ma - this code is not always correct in its choice of pivotB. + // The constraint model is asymmetrical. B is the master, and the constraint feature is defined in B-space as a region affixed to body B. + // For example, we can conceive of a 1D constraint as a plane attached to body B through constraint.PivotB, and constraint.PivotA is constrained to that plane. + // A 2D constraint is a line attached to body B. A 3D constraint is a point. + // So, while we always apply an impulse to body A at pivotA, we apply the impulse to body B somewhere on the constraint region. + // This code chooses that point by projecting pivotA onto the point, line or plane, which seems pretty reasonable and also analogous to how contact constraints work. + // However, if the limits are nonzero, then the region is not a point, line or plane. It is a spherical shell, cylindrical shell, or the space between two parallel planes. + // In that case, it is not projecting A to a point on the constraint region. This will not prevent solving the constraint, but the solution may not look correct. + // For now I am leaving it because it is not important to get the most common constraint situations working. If you use a ball and socket, or a prismatic constraint with a + // static master body, or a stiff spring, then there's no problem. However, I think it should eventually be fixed. The min and max limits have different projections, so + // probably the best solution is to make two jacobians whenever min != max. My assumption is that 99% of these are ball and sockets with min = max = 0, so I would rather have + // some waste in the min != max case than generalize this code to deal with different pivots and effective masses depending on which limit is hit. + + if (!math.all(constraint.ConstrainedAxes)) + { + Is1D = constraint.ConstrainedAxes.x ^ constraint.ConstrainedAxes.y ^ constraint.ConstrainedAxes.z; + + // Project pivot A onto the line or plane in B that it is attached to + RigidTransform bFromA = math.mul(math.inverse(WorldFromB), WorldFromA); + float3 pivotAinB = math.transform(bFromA, PivotAinA); + float3 diff = pivotAinB - PivotBinB; + for (int i = 0; i < 3; i++) + { + float3 column = bFromConstraint.Rotation[i]; + AxisInB = math.select(column, AxisInB, Is1D ^ constraint.ConstrainedAxes[i]); + + float3 dot = math.select(math.dot(column, diff), 0.0f, constraint.ConstrainedAxes[i]); + PivotBinB += column * dot; + } + } + + // Calculate the current error + InitialError = CalculateError( + new MTransform(WorldFromA.rot, WorldFromA.pos), + new MTransform(WorldFromB.rot, WorldFromB.pos), + out float3 directionUnused); + } + + private static void ApplyImpulse(float3 impulse, float3 ang0, float3 ang1, float3 ang2, ref MotionVelocity velocity) + { + velocity.ApplyLinearImpulse(impulse); + velocity.ApplyAngularImpulse(impulse.x * ang0 + impulse.y * ang1 + impulse.z * ang2); + } + + // Solve the Jacobian + public void Solve(ref MotionVelocity velocityA, ref MotionVelocity velocityB, float timestep) + { + // Predict the motions' transforms at the end of the step + MTransform futureWorldFromA; + MTransform futureWorldFromB; + { + quaternion dqA = Integrator.IntegrateAngularVelocity(velocityA.AngularVelocity, timestep); + quaternion dqB = Integrator.IntegrateAngularVelocity(velocityB.AngularVelocity, timestep); + quaternion futureOrientationA = math.normalize(math.mul(WorldFromA.rot, dqA)); + quaternion futureOrientationB = math.normalize(math.mul(WorldFromB.rot, dqB)); + futureWorldFromA = new MTransform(futureOrientationA, WorldFromA.pos + velocityA.LinearVelocity * timestep); + futureWorldFromB = new MTransform(futureOrientationB, WorldFromB.pos + velocityB.LinearVelocity * timestep); + } + + // Calculate the angulars + CalculateAngulars(PivotAinA, futureWorldFromA.Rotation, out float3 angA0, out float3 angA1, out float3 angA2); + CalculateAngulars(PivotBinB, futureWorldFromB.Rotation, out float3 angB0, out float3 angB1, out float3 angB2); + + // Calculate effective mass + float3 EffectiveMassDiag, EffectiveMassOffDiag; + { + // Calculate the inverse effective mass matrix + float3 invEffectiveMassDiag = new float3( + JacobianUtilities.CalculateInvEffectiveMassDiag(angA0, velocityA.InverseInertiaAndMass, angB0, velocityB.InverseInertiaAndMass), + JacobianUtilities.CalculateInvEffectiveMassDiag(angA1, velocityA.InverseInertiaAndMass, angB1, velocityB.InverseInertiaAndMass), + JacobianUtilities.CalculateInvEffectiveMassDiag(angA2, velocityA.InverseInertiaAndMass, angB2, velocityB.InverseInertiaAndMass)); + + float3 invEffectiveMassOffDiag = new float3( + JacobianUtilities.CalculateInvEffectiveMassOffDiag(angA0, angA1, velocityA.InverseInertiaAndMass.xyz, angB0, angB1, velocityB.InverseInertiaAndMass.xyz), + JacobianUtilities.CalculateInvEffectiveMassOffDiag(angA0, angA2, velocityA.InverseInertiaAndMass.xyz, angB0, angB2, velocityB.InverseInertiaAndMass.xyz), + JacobianUtilities.CalculateInvEffectiveMassOffDiag(angA1, angA2, velocityA.InverseInertiaAndMass.xyz, angB1, angB2, velocityB.InverseInertiaAndMass.xyz)); + + // Invert to get the effective mass matrix + JacobianUtilities.InvertSymmetricMatrix(invEffectiveMassDiag, invEffectiveMassOffDiag, out EffectiveMassDiag, out EffectiveMassOffDiag); + } + + // Predict error at the end of the step and calculate the impulse to correct it + float3 impulse; + { + // Find the difference between the future distance and the limit range, then apply tau and damping + float futureDistanceError = CalculateError(futureWorldFromA, futureWorldFromB, out float3 futureDirection); + float solveDistanceError = JacobianUtilities.CalculateCorrection(futureDistanceError, InitialError, Tau, Damping); + + // Calculate the impulse to correct the error + float3 solveError = solveDistanceError * futureDirection; + float3x3 effectiveMass = JacobianUtilities.BuildSymmetricMatrix(EffectiveMassDiag, EffectiveMassOffDiag); + impulse = math.mul(effectiveMass, solveError) * (1.0f / timestep); + } + + // Apply the impulse + ApplyImpulse(impulse, angA0, angA1, angA2, ref velocityA); + ApplyImpulse(-impulse, angB0, angB1, angB2, ref velocityB); + } + + #region Helpers + + private static void CalculateAngulars(float3 pivotInMotion, float3x3 worldFromMotionRotation, out float3 ang0, out float3 ang1, out float3 ang2) + { + // Jacobian directions are i, j, k + // Angulars are pivotInMotion x (motionFromWorld * direction) + float3x3 motionFromWorldRotation = math.transpose(worldFromMotionRotation); + ang0 = math.cross(pivotInMotion, motionFromWorldRotation.c0); + ang1 = math.cross(pivotInMotion, motionFromWorldRotation.c1); + ang2 = math.cross(pivotInMotion, motionFromWorldRotation.c2); + } + + private float CalculateError(MTransform worldFromA, MTransform worldFromB, out float3 direction) + { + // Find the direction from pivot A to B and the distance between them + float3 pivotA = Mul(worldFromA, PivotAinA); + float3 pivotB = Mul(worldFromB, PivotBinB); + float3 axis = math.mul(worldFromB.Rotation, AxisInB); + direction = pivotB - pivotA; + float dot = math.dot(direction, axis); + + // Project for lower-dimension joints + float distance; + if (Is1D) + { + // In 1D, distance is signed and measured along the axis + distance = -dot; + direction = -axis; + } + else + { + // In 2D / 3D, distance is nonnegative. In 2D it is measured perpendicular to the axis. + direction -= axis * dot; + float futureDistanceSq = math.lengthsq(direction); + float invFutureDistance = math.select(math.rsqrt(futureDistanceSq), 0.0f, futureDistanceSq == 0.0f); + distance = futureDistanceSq * invFutureDistance; + direction *= invFutureDistance; + } + + // Find the difference between the future distance and the limit range + return JacobianUtilities.CalculateError(distance, MinDistance, MaxDistance); + } + + #endregion + } +} diff --git a/package/Unity.Physics/Dynamics/Jacobians/LinearLimitJacobian.cs.meta b/package/Unity.Physics/Dynamics/Jacobians/LinearLimitJacobian.cs.meta new file mode 100755 index 000000000..dcbca4a55 --- /dev/null +++ b/package/Unity.Physics/Dynamics/Jacobians/LinearLimitJacobian.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 77f86213ff6ec294c81256924cb80962 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Unity.Physics/Dynamics/Joint.meta b/package/Unity.Physics/Dynamics/Joint.meta new file mode 100755 index 000000000..bcaa91578 --- /dev/null +++ b/package/Unity.Physics/Dynamics/Joint.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: 817adfd99fec7e24da9c0b5d202f9ce5 +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Unity.Physics/Dynamics/Joint/Physics_Joint.cs b/package/Unity.Physics/Dynamics/Joint/Physics_Joint.cs new file mode 100755 index 000000000..7740821d3 --- /dev/null +++ b/package/Unity.Physics/Dynamics/Joint/Physics_Joint.cs @@ -0,0 +1,371 @@ +using System; +using System.Runtime.InteropServices; +using Unity.Collections; +using Unity.Collections.LowLevel.Unsafe; +using Unity.Entities; +using Unity.Mathematics; +using UnityEngine.Assertions; +using static Unity.Physics.Math; + +namespace Unity.Physics +{ + public enum ConstraintType : byte + { + Linear, + Angular + } + + // A linear or angular constraint in 1, 2, or 3 dimensions. + public struct Constraint + { + // TODO think more about these + // Current values give tau = 0.6 damping = 0.99 at 50hz + // The values are huge and we can't get damping = 1 -- a stiff constraint is the limit of a damped spring as spring params go to infinity. + public const float DefaultSpringFrequency = 61950.977267809007887192914302327f; + public const float DefaultSpringDamping = 2530.12155587434178122630287018f; + + public bool3 ConstrainedAxes; + public ConstraintType Type; + + public float Min; + public float Max; + public float SpringFrequency; + public float SpringDamping; + + // Number of affected degrees of freedom. 1, 2, or 3. + public int Dimension => math.select(math.select(2, 1, ConstrainedAxes.x ^ ConstrainedAxes.y ^ ConstrainedAxes.z), 3, math.all(ConstrainedAxes)); + + // Selects the free axis from a constraint with Dimension == 1 + public int FreeAxis2D + { + get + { + Assert.IsTrue(Dimension == 2); + return math.select(2, math.select(0, 1, ConstrainedAxes[0]), ConstrainedAxes[2]); + } + } + + // Selects the constrained axis from a constraint with Dimension == 2 + public int ConstrainedAxis1D + { + get + { + Assert.IsTrue(Dimension == 1); + return math.select(math.select(1, 0, ConstrainedAxes[0]), 2, ConstrainedAxes[2]); + } + } + + #region Common linear constraints + + // 3 DOF linear constraint with limits at zero + public static Constraint BallAndSocket(float springFrequency = DefaultSpringFrequency, float springDamping = DefaultSpringDamping) + { + return new Constraint + { + ConstrainedAxes = new bool3(true), + Type = ConstraintType.Linear, + Min = 0.0f, + Max = 0.0f, + SpringFrequency = springFrequency, + SpringDamping = springDamping + }; + } + + // 3 DOF linear constraint + public static Constraint StiffSpring(float minDistance, float maxDistance, float springFrequency = DefaultSpringFrequency, float springDamping = DefaultSpringDamping) + { + return new Constraint + { + ConstrainedAxes = new bool3(true), + Type = ConstraintType.Linear, + Min = minDistance, + Max = maxDistance, + SpringFrequency = springFrequency, + SpringDamping = springDamping + }; + } + + // 2 DOF linear constraint + public static Constraint Cylindrical(int freeAxis, float minDistance, float maxDistance, float springFrequency = DefaultSpringFrequency, float springDamping = DefaultSpringDamping) + { + Assert.IsTrue(freeAxis >= 0 && freeAxis <= 2); + return new Constraint + { + ConstrainedAxes = new bool3(freeAxis != 0, freeAxis != 1, freeAxis != 2), + Type = ConstraintType.Linear, + Min = minDistance, + Max = maxDistance, + SpringFrequency = springFrequency, + SpringDamping = springDamping + }; + } + + // 1 DOF linear constraint + public static Constraint Planar(int limitedAxis, float minDistance, float maxDistance, float springFrequency = DefaultSpringFrequency, float springDamping = DefaultSpringDamping) + { + Assert.IsTrue(limitedAxis >= 0 && limitedAxis <= 2); + return new Constraint + { + ConstrainedAxes = new bool3(limitedAxis == 0, limitedAxis == 1, limitedAxis == 2), + Type = ConstraintType.Linear, + Min = minDistance, + Max = maxDistance, + SpringFrequency = springFrequency, + SpringDamping = springDamping + }; + } + + #endregion + + #region Common angular constraints + + // 3DOF angular constraint with limits at zero + public static Constraint FixedAngle(float springFrequency = DefaultSpringFrequency, float springDamping = DefaultSpringDamping) + { + return new Constraint + { + ConstrainedAxes = new bool3(true), + Type = ConstraintType.Angular, + Min = 0.0f, + Max = 0.0f, + SpringFrequency = springFrequency, + SpringDamping = springDamping + }; + } + + // 2 DOF angular constraint with limits at zero + public static Constraint Hinge(int freeAxis, float springFrequency = DefaultSpringFrequency, float springDamping = DefaultSpringDamping) + { + Assert.IsTrue(freeAxis >= 0 && freeAxis <= 2); + return new Constraint + { + ConstrainedAxes = new bool3(freeAxis != 0, freeAxis != 1, freeAxis != 2), + Type = ConstraintType.Angular, + Min = 0.0f, + Max = 0.0f, + SpringFrequency = springFrequency, + SpringDamping = springDamping + }; + } + + // 2 DOF angular constraint + public static Constraint Cone(int freeAxis, float minAngle, float maxAngle, float springFrequency = DefaultSpringFrequency, float springDamping = DefaultSpringDamping) + { + Assert.IsTrue(freeAxis >= 0 && freeAxis <= 2); + return new Constraint + { + ConstrainedAxes = new bool3(freeAxis != 0, freeAxis != 1, freeAxis != 2), + Type = ConstraintType.Angular, + Min = minAngle, + Max = maxAngle, + SpringFrequency = springFrequency, + SpringDamping = springDamping + }; + } + + // 1 DOF angular constraint + public static Constraint Twist(int limitedAxis, float minAngle, float maxAngle, float springFrequency = DefaultSpringFrequency, float springDamping = DefaultSpringDamping) + { + Assert.IsTrue(limitedAxis >= 0 && limitedAxis <= 2); + return new Constraint + { + ConstrainedAxes = new bool3(limitedAxis == 0, limitedAxis == 1, limitedAxis == 2), + Type = ConstraintType.Angular, + Min = minAngle, + Max = maxAngle, + SpringFrequency = springFrequency, + SpringDamping = springDamping + }; + } + + #endregion + } + + // A set of constraints on the relative motion between a pair of rigid bodies. + // Warning: This is just the header, the joint's variable sized data follows it in memory. + // Therefore this struct must always be passed by reference, never by value. + public struct JointData + { + // Transform from joint definition space to body space + public MTransform AFromJoint { get; private set; } + public MTransform BFromJoint { get; private set; } + + // Array of constraints + private BlobArray m_ConstraintsBlob; + public byte Version { get; private set; } + + // Accessor for the constraints + public BlobArray.Accessor Constraints => new BlobArray.Accessor(ref m_ConstraintsBlob); + public int NumConstraints => m_ConstraintsBlob.Length; + + // Create a joint asset with the given constraints + public static unsafe BlobAssetReference Create(MTransform aFromJoint, MTransform bFromJoint, Constraint[] constraints) + { + // Allocate + int totalSize = sizeof(JointData) + sizeof(Constraint) * constraints.Length; + JointData* jointData = (JointData*)UnsafeUtility.Malloc(totalSize, 16, Allocator.Temp); + UnsafeUtility.MemClear(jointData, totalSize); + + // Initialize + { + jointData->AFromJoint = aFromJoint; + jointData->BFromJoint = bFromJoint; + jointData->Version = 1; + + byte* end = (byte*)jointData + sizeof(JointData); + jointData->m_ConstraintsBlob.Offset = (int)(end - (byte*)UnsafeUtility.AddressOf(ref jointData->m_ConstraintsBlob.Offset)); + jointData->m_ConstraintsBlob.Length = constraints.Length; + + for (int i = 0; i < constraints.Length; i++) + { + jointData->Constraints[i] = constraints[i]; + } + } + + // Copy it into blob asset + byte[] bytes = new byte[totalSize]; + Marshal.Copy((IntPtr)jointData, bytes, 0, totalSize); + UnsafeUtility.Free(jointData, Allocator.Temp); + return BlobAssetReference.Create(bytes); + } + + #region Common joint descriptions + + public static BlobAssetReference CreateBallAndSocket(float3 positionAinA, float3 positionBinB) + { + return Create( + new MTransform(float3x3.identity, positionAinA), + new MTransform(float3x3.identity, positionBinB), + new[] + { + Constraint.BallAndSocket() + } + ); + } + + public static BlobAssetReference CreateStiffSpring(float3 positionAinA, float3 positionBinB, float minDistance, float maxDistance) + { + return Create( + new MTransform(float3x3.identity, positionAinA), + new MTransform(float3x3.identity, positionBinB), + new[] + { + Constraint.StiffSpring(minDistance, maxDistance) + } + ); + } + + public static BlobAssetReference CreatePrismatic(float3 positionAinA, float3 positionBinB, float3 axisInB, + float minDistanceOnAxis, float maxDistanceOnAxis, float minDistanceFromAxis, float maxDistanceFromAxis) + { + CalculatePerpendicularNormalized(axisInB, out float3 perpendicular1, out float3 perpendicular2); + return Create( + new MTransform(float3x3.identity, positionAinA), + new MTransform(new float3x3(axisInB, perpendicular1, perpendicular2), positionBinB), + new[] + { + Constraint.Planar(0, minDistanceOnAxis, maxDistanceOnAxis), + Constraint.Cylindrical(0, minDistanceFromAxis, maxDistanceFromAxis) + } + ); + } + + public static BlobAssetReference CreateHinge(float3 positionAinA, float3 positionBinB, float3 axisInA, float3 axisInB) + { + CalculatePerpendicularNormalized(axisInA, out float3 perpendicularA1, out float3 perpendicularA2); + CalculatePerpendicularNormalized(axisInB, out float3 perpendicularB1, out float3 perpendicularB2); + return Create( + new MTransform(new float3x3(axisInA, perpendicularA1, perpendicularA2), positionAinA), + new MTransform(new float3x3(axisInB, perpendicularB1, perpendicularB2), positionBinB), + new[] + { + Constraint.Hinge(0), + Constraint.BallAndSocket() + } + ); + } + + public static BlobAssetReference CreateLimitedHinge(float3 positionAinA, float3 positionBinB, + float3 axisInA, float3 axisInB, float3 perpendicularInA, float3 perpendicularInB, float minAngle, float maxAngle) + { + return Create( + new MTransform(new float3x3(axisInA, perpendicularInA, math.cross(axisInA, perpendicularInA)), positionAinA), + new MTransform(new float3x3(axisInB, perpendicularInB, math.cross(axisInB, perpendicularInB)), positionBinB), + new[] + { + Constraint.Twist(0, minAngle, maxAngle), + Constraint.Hinge(0), + Constraint.BallAndSocket() + }); + } + + public static void CreateRagdoll(float3 positionAinA, float3 positionBinB, + float3 twistAxisInA, float3 twistAxisInB, float3 perpendicularAxisInA, float3 perpendicularAxisInB, + float maxConeAngle, float minPerpendicularAngle, float maxPerpendicularAngle, float minTwistAngle, float maxTwistAngle, + out BlobAssetReference jointData0, out BlobAssetReference jointData1) + { + // TODO - the hkpRagdollConstraint can't be represented with a single joint. The reason is that the hkpRagdollConstraint + // cone limit and plane limit apply to different axes in A and B. For example, if the cone limit constrains the angle between + // axis 0 in each body, then the plane limit must constraint axis 0 in one body to a non-0 axis in the other. + // Currently I use two joints to solve this problem. Another solution would be to extend Constraint so that it can swizzle the axes. + + // Check that the perpendicular axes are perpendicular + Assert.IsTrue(math.abs(math.dot(twistAxisInA, perpendicularAxisInA)) < 1e-5f); + Assert.IsTrue(math.abs(math.dot(twistAxisInB, perpendicularAxisInB)) < 1e-5f); + + float3 crossA = math.cross(twistAxisInA, perpendicularAxisInA); + float3 crossB = math.cross(twistAxisInB, perpendicularAxisInB); + var transformA = new MTransform(new float3x3(twistAxisInA, perpendicularAxisInA, crossA), positionAinA); + + // First joint data: twist and primary cone + jointData0 = Create( + transformA, + new MTransform(new float3x3(twistAxisInB, perpendicularAxisInB, crossB), positionBinB), + new[] { + Constraint.Twist(0, minTwistAngle, maxTwistAngle), // Twist limit + Constraint.Cone(0, 0.0f, maxConeAngle) // Cone about the twist axis + } + ); + + // Second joint data: perpendicular cone and ball socket + jointData1 = Create( + transformA, + new MTransform(new float3x3(perpendicularAxisInB, twistAxisInB, -crossB), positionBinB), + new[] + { + Constraint.Cone(0, minPerpendicularAngle, maxPerpendicularAngle), // Cone about the reference axis + Constraint.BallAndSocket() + } + ); + } + + public static BlobAssetReference CreateFixed(float3 positionAinA, float3 positionBinB, quaternion orientationAinA, quaternion orientationBinB) + { + return Create( + new MTransform(orientationAinA, positionAinA), + new MTransform(orientationBinB, positionBinB), + new[] + { + Constraint.BallAndSocket(), + Constraint.FixedAngle() + } + ); + } + + #endregion + } + + // A runtime joint instance, attached to specific rigid bodies + public unsafe struct Joint + { + public JointData* JointData; + public BodyIndexPair BodyPair; + public int EnableCollision; // If non-zero, allows these bodies to collide + + // The entity that contained the component which created this joint + // Note, this isn't necessarily an entity with a rigid body, as a pair + // of bodies can have an arbitrary number of constraints, but only one + // instance of a particular component type per entity + public Entity Entity; + } +} diff --git a/package/Unity.Physics/Dynamics/Joint/Physics_Joint.cs.meta b/package/Unity.Physics/Dynamics/Joint/Physics_Joint.cs.meta new file mode 100755 index 000000000..b44049dba --- /dev/null +++ b/package/Unity.Physics/Dynamics/Joint/Physics_Joint.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: a82ae0efcfd30bb448a0cb00b56c6d5c +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Unity.Physics/Dynamics/Material.meta b/package/Unity.Physics/Dynamics/Material.meta new file mode 100755 index 000000000..d67a9a157 --- /dev/null +++ b/package/Unity.Physics/Dynamics/Material.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: 6c211415bce8b5c4c85ddd4105d3e108 +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Unity.Physics/Dynamics/Material/Material.cs b/package/Unity.Physics/Dynamics/Material/Material.cs new file mode 100755 index 000000000..9bfe906df --- /dev/null +++ b/package/Unity.Physics/Dynamics/Material/Material.cs @@ -0,0 +1,98 @@ +using System; +using Unity.Mathematics; + +namespace Unity.Physics +{ + // Describes how an object should respond to collisions with other objects. + public struct Material + { + public MaterialFlags Flags; + public CombinePolicy FrictionCombinePolicy; + public CombinePolicy RestitutionCombinePolicy; + public float Friction; + public float Restitution; + + // If true, the object does not collide but raises trigger events instead + public bool IsTrigger => (Flags & MaterialFlags.IsTrigger) != 0; + + // If true, the object raises collision events if an impulse is applied during solving + public bool EnableCollisionEvents => (Flags & MaterialFlags.EnableCollisionEvents) != 0; + + // If true, the object can have its inertia and mass overridden during solving + public bool EnableMassFactors => (Flags & MaterialFlags.EnableMassFactors) != 0; + + // If true, the object can apply a surface velocity to its contact points + public bool EnableSurfaceVelocity => (Flags & MaterialFlags.EnableSurfaceVelocity) != 0; + + // If true, the object can limit the impulses applied to its contact points + public bool EnableMaxImpulse => (Flags & MaterialFlags.EnableMaxImpulse) != 0; + + [Flags] + public enum MaterialFlags : byte + { + IsTrigger = 1 << 0, + EnableCollisionEvents = 1 << 1, + EnableMassFactors = 1 << 2, + EnableSurfaceVelocity = 1 << 3, + EnableMaxImpulse = 1 << 4 + } + + // Defines how a value from a pair of materials should be combined. + public enum CombinePolicy : byte + { + GeometricMean, // sqrt(a * b) + Minimum, // min(a, b) + Maximum, // max(a, b) + ArithmeticMean // (a + b) / 2 + } + + // A default material. + public static readonly Material Default = new Material + { + FrictionCombinePolicy = CombinePolicy.GeometricMean, + RestitutionCombinePolicy = CombinePolicy.GeometricMean, + Friction = 0.5f, + Restitution = 0.0f + }; + + // Get a combined friction value for a pair of materials. + // The combine policy with the highest value takes priority. + public static float GetCombinedFriction(Material materialA, Material materialB) + { + var policy = (CombinePolicy)math.max((int)materialA.FrictionCombinePolicy, (int)materialB.FrictionCombinePolicy); + switch (policy) + { + case CombinePolicy.GeometricMean: + return math.sqrt(materialA.Friction * materialB.Friction); + case CombinePolicy.Minimum: + return math.min(materialA.Friction, materialB.Friction); + case CombinePolicy.Maximum: + return math.max(materialA.Friction, materialB.Friction); + case CombinePolicy.ArithmeticMean: + return (materialA.Friction + materialB.Friction) * 0.5f; + default: + return 0; + } + } + + // Get a combined restitution value for a pair of materials. + // The combine policy with the highest value takes priority. + public static float GetCombinedRestitution(Material materialA, Material materialB) + { + var policy = (CombinePolicy)math.max((int)materialA.RestitutionCombinePolicy, (int)materialB.RestitutionCombinePolicy); + switch (policy) + { + case CombinePolicy.GeometricMean: + return math.sqrt(materialA.Restitution * materialB.Restitution); + case CombinePolicy.Minimum: + return math.min(materialA.Restitution, materialB.Restitution); + case CombinePolicy.Maximum: + return math.max(materialA.Restitution, materialB.Restitution); + case CombinePolicy.ArithmeticMean: + return (materialA.Restitution + materialB.Restitution) * 0.5f; + default: + return 0; + } + } + } +} diff --git a/package/Unity.Physics/Dynamics/Material/Material.cs.meta b/package/Unity.Physics/Dynamics/Material/Material.cs.meta new file mode 100755 index 000000000..d81efa82d --- /dev/null +++ b/package/Unity.Physics/Dynamics/Material/Material.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 7582e9a4ecc6ccf47aa57dea13c0a77b +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Unity.Physics/Dynamics/Motion.meta b/package/Unity.Physics/Dynamics/Motion.meta new file mode 100755 index 000000000..aa5b61693 --- /dev/null +++ b/package/Unity.Physics/Dynamics/Motion.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: 519d32bb505b2404d88c9b179ece1867 +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Unity.Physics/Dynamics/Motion/Motion.cs b/package/Unity.Physics/Dynamics/Motion/Motion.cs new file mode 100755 index 000000000..7af3eed33 --- /dev/null +++ b/package/Unity.Physics/Dynamics/Motion/Motion.cs @@ -0,0 +1,143 @@ +using System.Runtime.CompilerServices; +using Unity.Mathematics; + +namespace Unity.Physics +{ + // Describes how mass is distributed within an object. + // Represented by a transformed box inertia of unit mass. + public struct MassDistribution + { + // The center of mass and the orientation to principal axis space + public RigidTransform Transform; + + // Diagonalized inertia tensor for a unit mass + public float3 InertiaTensor; + + // Get the inertia as a 3x3 matrix + public float3x3 InertiaMatrix + { + get + { + var r = new float3x3(Transform.rot); + var r2 = new float3x3(InertiaTensor.x * r.c0, InertiaTensor.y * r.c1, InertiaTensor.z * r.c2); + return math.mul(r2, math.inverse(r)); + } + } + } + + // The mass properties of an object. + public struct MassProperties + { + // The distribution of a unit mass throughout the object. + public MassDistribution MassDistribution; + + // The volume of the object. + public float Volume; + + // Upper bound on the rate of change of the object's extent in any direction, + // with respect to angular speed around its center of mass. + // Used to determine how much to expand a rigid body's AABB to enclose its swept volume. + public float AngularExpansionFactor; + + // The mass properties of a unit sphere + public static readonly MassProperties UnitSphere = new MassProperties + { + MassDistribution = new MassDistribution + { + Transform = RigidTransform.identity, + InertiaTensor = new float3(2.0f / 5.0f) + }, + Volume = (4.0f / 3.0f) * (float)math.PI, + AngularExpansionFactor = 0.0f + }; + } + + // A dynamic rigid body's "cold" motion data, used during Jacobian building and integration. + public struct MotionData + { + // Center of mass and inertia orientation in world space + public RigidTransform WorldFromMotion; + + // Center of mass and inertia orientation in rigid body space + public RigidTransform BodyFromMotion; + + // Damping applied to the motion during each simulation step + public float LinearDamping; + public float AngularDamping; + + // A multiplier applied to the simulation step's gravity vector + public float GravityFactor; + + public static readonly MotionData Zero = new MotionData + { + WorldFromMotion = RigidTransform.identity, + BodyFromMotion = RigidTransform.identity, + LinearDamping = 0.0f, + AngularDamping = 0.0f, + GravityFactor = 0.0f + }; + } + + // A dynamic rigid body's "hot" motion data, used during solving. + public struct MotionVelocity + { + public float3 LinearVelocity; // world space + public float3 AngularVelocity; // motion space + public float4 InverseInertiaAndMass; + public float AngularExpansionFactor; + + public static readonly MotionVelocity Zero = new MotionVelocity + { + LinearVelocity = new float3(0), + AngularVelocity = new float3(0), + InverseInertiaAndMass = new float4(0), + AngularExpansionFactor = 0.0f + }; + + // Apply a linear impulse (in world space) + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public void ApplyLinearImpulse(float3 impulse) + { + LinearVelocity += impulse * InverseInertiaAndMass.w; + } + + // Apply an angular impulse (in motion space) + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public void ApplyAngularImpulse(float3 impulse) + { + AngularVelocity += impulse * InverseInertiaAndMass.xyz; + } + + // Calculate the distances by which to expand collision tolerances based on the speed of the object. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public MotionExpansion CalculateExpansion(float timeStep) => new MotionExpansion + { + Linear = LinearVelocity * timeStep, + Uniform = math.min(math.length(AngularVelocity) * timeStep, (float)math.PI / 2.0f) * AngularExpansionFactor + }; + } + + // Provides an upper bound on change in a body's extents in any direction during a step. + // Used to determine how far away from the body to look for collisions. + public struct MotionExpansion + { + public float3 Linear; // how far to look ahead of the object + public float Uniform; // how far to look around the object + + public float MaxDistance => math.length(Linear) + Uniform; + + public static readonly MotionExpansion Zero = new MotionExpansion + { + Linear = new float3(0), + Uniform = 0.0f + }; + + // Expand an AABB + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public Aabb ExpandAabb(Aabb aabb) => new Aabb + { + Max = math.max(aabb.Max, aabb.Max + Linear) + Uniform, + Min = math.min(aabb.Min, aabb.Min + Linear) - Uniform + }; + } +} diff --git a/package/Unity.Physics/Dynamics/Motion/Motion.cs.meta b/package/Unity.Physics/Dynamics/Motion/Motion.cs.meta new file mode 100755 index 000000000..ccca5d051 --- /dev/null +++ b/package/Unity.Physics/Dynamics/Motion/Motion.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: c7b267e845e91994baf4187f9b164356 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Unity.Physics/Dynamics/Simulation.meta b/package/Unity.Physics/Dynamics/Simulation.meta new file mode 100755 index 000000000..f73ddf99d --- /dev/null +++ b/package/Unity.Physics/Dynamics/Simulation.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: 767d058def2cc8342a09fcbd047953b3 +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Unity.Physics/Dynamics/Simulation/Callbacks.cs b/package/Unity.Physics/Dynamics/Simulation/Callbacks.cs new file mode 100755 index 000000000..e92ebfdab --- /dev/null +++ b/package/Unity.Physics/Dynamics/Simulation/Callbacks.cs @@ -0,0 +1,66 @@ +using System; +using System.Collections.Generic; +using Unity.Collections; +using Unity.Jobs; + +namespace Unity.Physics +{ + // A container of user callbacks, to run during scheduling of the simulation jobs + public class SimulationCallbacks + { + public enum Phase + { + PostCreateDispatchPairs, + PostCreateContacts, + PostCreateContactJacobians, + PostSolveJacobians, + PostIntegrateMotions + } + + public delegate JobHandle Callback(ref ISimulation simulation, JobHandle inputDeps); + + private static readonly int k_NumPhases = Enum.GetValues(typeof(Phase)).Length; + + private readonly List[] m_Callbacks = new List[k_NumPhases]; + + public SimulationCallbacks() + { + for (int i = 0; i < k_NumPhases; ++i) + { + m_Callbacks[i] = new List(8); + } + } + + public void Enqueue(Phase phase, Callback cb) + { + m_Callbacks[(int)phase].Add(cb); + } + + public JobHandle Execute(Phase phase, ISimulation simulation, JobHandle inputDeps) + { + ref List cbs = ref m_Callbacks[(int)phase]; + if (m_Callbacks[(int)phase].Count > 0) + { + NativeList handles = new NativeList(cbs.Count, Allocator.Temp); + foreach (Callback callback in cbs) + { + JobHandle newTask = callback(ref simulation, inputDeps); + handles.Add(newTask); + inputDeps = newTask; // Have to assume each callback will modify the same data + } + JobHandle handle = JobHandle.CombineDependencies(handles); + handles.Dispose(); + return handle; + } + return inputDeps; + } + + public void Clear() + { + for (int i = 0; i < k_NumPhases; i++) + { + m_Callbacks[i].Clear(); + } + } + } +} diff --git a/package/Unity.Physics/Dynamics/Simulation/Callbacks.cs.meta b/package/Unity.Physics/Dynamics/Simulation/Callbacks.cs.meta new file mode 100755 index 000000000..f93cb5dd5 --- /dev/null +++ b/package/Unity.Physics/Dynamics/Simulation/Callbacks.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 51189c121a7b38c41b73d4af759b6112 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Unity.Physics/Dynamics/Simulation/Events.cs b/package/Unity.Physics/Dynamics/Simulation/Events.cs new file mode 100755 index 000000000..b7a6fee56 --- /dev/null +++ b/package/Unity.Physics/Dynamics/Simulation/Events.cs @@ -0,0 +1,153 @@ +using System; +using Unity.Collections; +using Unity.Collections.LowLevel.Unsafe; +using Unity.Mathematics; + +namespace Unity.Physics +{ + // An event raised when a pair of bodies have collided during solving. + public struct CollisionEvent + { + public BodyIndexPair BodyIndices; + public ColliderKeyPair ColliderKeys; + public float3 Normal; + public float4 AccumulatedImpulses; + } + + // An event raised when a pair of bodies involving a trigger material have overlapped during solving. + public struct TriggerEvent + { + public BodyIndexPair BodyIndices; + public ColliderKeyPair ColliderKeys; + } + + // Collision event reader, for both Unity.Physics and Havok.Physics. + // This is a value type, which means it can be used in Burst jobs (unlike IEnumerable). + public unsafe struct CollisionEvents /* : IEnumerable */ + { + //@TODO: Unity should have a Allow null safety restriction + [NativeDisableContainerSafetyRestriction] + private readonly BlockStream m_EventStream; + + public CollisionEvents(BlockStream eventStream) + { + m_EventStream = eventStream; + } + + public Enumerator GetEnumerator() + { + return new Enumerator(m_EventStream); + } + + public struct Enumerator /* : IEnumerator */ + { + private BlockStream.Reader m_Reader; + private int m_CurrentWorkItem; + private readonly int m_NumWorkItems; + + private CollisionEvent m_Current; + public CollisionEvent Current => m_Current; + + public Enumerator(BlockStream stream) + { + m_Reader = stream.IsCreated ? stream : new BlockStream.Reader(); + m_CurrentWorkItem = 0; + m_NumWorkItems = stream.IsCreated ? stream.ForEachCount : 0; + + m_Current = default(CollisionEvent); + + AdvanceReader(); + } + + public bool MoveNext() + { + if (m_Reader.RemainingItemCount > 0) + { + m_Current = m_Reader.Read(); + AdvanceReader(); + return true; + } + return false; + } + + public void Reset() + { + throw new NotImplementedException(); + } + + private void AdvanceReader() + { + if (m_Reader.RemainingItemCount == 0 && m_CurrentWorkItem < m_NumWorkItems) + { + m_Reader.BeginForEachIndex(m_CurrentWorkItem); + m_CurrentWorkItem++; + } + } + } + } + + // Trigger event reader, for both Unity.Physics and Havok.Physics. + // This is a value type, which means it can be used in Burst jobs (unlike IEnumerable). + public unsafe struct TriggerEvents /* : IEnumerable */ + { + + //@TODO: Unity should have a Allow null safety restriction + [NativeDisableContainerSafetyRestriction] + private readonly BlockStream m_EventStream; + + public TriggerEvents(BlockStream eventStream) + { + m_EventStream = eventStream; + } + + public Enumerator GetEnumerator() + { + return new Enumerator(m_EventStream); + } + + public struct Enumerator /* : IEnumerator */ + { + private BlockStream.Reader m_Reader; + private int m_CurrentWorkItem; + private readonly int m_NumWorkItems; + + private TriggerEvent m_Current; + public TriggerEvent Current => m_Current; + + public Enumerator(BlockStream stream) + { + m_Reader = stream.IsCreated ? stream : new BlockStream.Reader(); + m_CurrentWorkItem = 0; + m_NumWorkItems = stream.IsCreated ? stream.ForEachCount : 0; + m_Current = default(TriggerEvent); + + AdvanceReader(); + } + + public bool MoveNext() + { + if (m_Reader.RemainingItemCount > 0) + { + m_Current = m_Reader.Read(); + AdvanceReader(); + return true; + } + return false; + } + + public void Reset() + { + throw new NotImplementedException(); + } + + private void AdvanceReader() + { + if (m_Reader.RemainingItemCount == 0 && m_CurrentWorkItem < m_NumWorkItems) + { + m_Reader.BeginForEachIndex(m_CurrentWorkItem); + m_CurrentWorkItem++; + } + } + } + } +} diff --git a/package/Unity.Physics/Dynamics/Simulation/Events.cs.meta b/package/Unity.Physics/Dynamics/Simulation/Events.cs.meta new file mode 100755 index 000000000..321f799e1 --- /dev/null +++ b/package/Unity.Physics/Dynamics/Simulation/Events.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 8c6565bea8ded3549a8db838305e46f9 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Unity.Physics/Dynamics/Simulation/Narrowphase.cs b/package/Unity.Physics/Dynamics/Simulation/Narrowphase.cs new file mode 100755 index 000000000..da8480520 --- /dev/null +++ b/package/Unity.Physics/Dynamics/Simulation/Narrowphase.cs @@ -0,0 +1,125 @@ +using Unity.Burst; +using Unity.Collections; +using Unity.Jobs; +using Unity.Mathematics; + +namespace Unity.Physics +{ + // Body pair processor / dispatcher + public static class NarrowPhase // TODO: rename + { + public static JobHandle ScheduleProcessBodyPairsJobs(ref PhysicsWorld world, float timeStep, int numIterations, ref Simulation.Context context, JobHandle inputDeps) + { + // PhasedDispatchPairs; +#pragma warning restore CS0649 + + public void Execute() + { + // Nothing to do, this jobs only disposes PhasedDispatchPairs. + } + } + + [BurstCompile] + private struct ProcessBodyPairsJob : IJobParallelFor + { + [ReadOnly] public PhysicsWorld World; + [ReadOnly] public float TimeStep; + [ReadOnly] public int NumIterations; + [ReadOnly] [DeallocateOnJobCompletion] public NativeArray PhasedDispatchPairs; + [ReadOnly] public Scheduler.SolverSchedulerInfo SolverSchedulerInfo; + public BlockStream.Writer ContactWriter; + public BlockStream.Writer JointJacobianWriter; + + public unsafe void Execute(int workItemIndex) + { + int dispatchPairReadOffset = SolverSchedulerInfo.GetWorkItemReadOffset(workItemIndex, out int numPairsToRead); + + ContactWriter.BeginForEachIndex(workItemIndex); + JointJacobianWriter.BeginForEachIndex(workItemIndex); + + for (int i = 0; i < numPairsToRead; i++) + { + Scheduler.DispatchPair dispatchPair = PhasedDispatchPairs[dispatchPairReadOffset + i]; + + // Invalid pairs can exist by being disabled by users + if (dispatchPair.IsValid) + { + if (dispatchPair.IsContact) + { + // Create contact manifolds for this pair of bodies + var pair = new BodyIndexPair + { + BodyAIndex = dispatchPair.BodyAIndex, + BodyBIndex = dispatchPair.BodyBIndex + }; + + ManifoldQueries.BodyBody(ref World, pair, TimeStep, ref ContactWriter); + } + else + { + Joint joint = World.Joints[dispatchPair.JointIndex]; + // Need to fetch the real body indices from the joint, as the scheduler may have reordered them + int bodyAIndex = joint.BodyPair.BodyAIndex; + int bodyBIndex = joint.BodyPair.BodyBIndex; + + GetMotion(ref World, bodyAIndex, out MotionVelocity velocityA, out MotionData motionA); + GetMotion(ref World, bodyBIndex, out MotionVelocity velocityB, out MotionData motionB); + Solver.BuildJointJacobian(joint.JointData, joint.BodyPair, velocityA, velocityB, motionA, motionB, TimeStep, NumIterations, ref JointJacobianWriter); + } + } + } + + JointJacobianWriter.EndForEachIndex(); + ContactWriter.EndForEachIndex(); + } + + // Gets a body's motion, even if the body is static + // TODO - share code with Solver.GetMotions()? + private static void GetMotion(ref PhysicsWorld world, int bodyIndex, out MotionVelocity velocity, out MotionData motion) + { + if (bodyIndex >= world.MotionVelocities.Length) + { + // Body is static + RigidBody body = world.Bodies[bodyIndex]; + velocity = MotionVelocity.Zero; + motion = new MotionData + { + WorldFromMotion = body.WorldFromBody, + BodyFromMotion = RigidTransform.identity + // remaining fields all zero + }; + } + else + { + // Body is dynamic + velocity = world.MotionVelocities[bodyIndex]; + motion = world.MotionDatas[bodyIndex]; + } + } + } + } +} diff --git a/package/Unity.Physics/Dynamics/Simulation/Narrowphase.cs.meta b/package/Unity.Physics/Dynamics/Simulation/Narrowphase.cs.meta new file mode 100755 index 000000000..7bc2d9a1c --- /dev/null +++ b/package/Unity.Physics/Dynamics/Simulation/Narrowphase.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 7b554e745bb005b4a9207ac29e7ca457 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Unity.Physics/Dynamics/Simulation/Scheduler.cs b/package/Unity.Physics/Dynamics/Simulation/Scheduler.cs new file mode 100755 index 000000000..5381e1ed2 --- /dev/null +++ b/package/Unity.Physics/Dynamics/Simulation/Scheduler.cs @@ -0,0 +1,751 @@ +using System; +using System.Diagnostics; +using Unity.Burst; +using Unity.Collections; +using Unity.Collections.LowLevel.Unsafe; +using Unity.Jobs; +using Unity.Mathematics; +using UnityEngine.Assertions; + +namespace Unity.Physics +{ + // Builds phased pairs of interacting bodies, used to parallelize work items during the simulation step. + public class Scheduler : IDisposable + { + private readonly BitLookupTable m_BitLookupTable; + + // A pair of interacting bodies (either potentially colliding, or constrained together using a Joint). + // The indices are compressed into a single 64 bit value, for deterministic sorting, as follows: + // [BodyAIndex|BodyBIndex|JointIndex] + // We additionally choose indices so that BodyAIndex < BodyBIndex. This has subtle side-effects: + // * If one body in the pair is static, it will be body B. + // * Indices used for jointed pairs are not necessarily the same as selected in the joint + // * For some body A, all it's static collisions will be contiguous. + [DebuggerDisplay("{IsJoint ? \"Joint\" : \"Contact\"}, [{BodyAIndex}, {BodyBIndex}]")] + public struct DispatchPair + { + private ulong m_Data; + + private const int k_InvalidBodyIndex = 0xffffff; + private const int k_InvalidJointIndex = 0x7fff; + private const ulong k_EnableJointCollisionBit = 0x8000; + + public bool IsValid => m_Data != 0xffffffffffffffff; + public bool IsContact => JointIndex == k_InvalidJointIndex; + public bool IsJoint => JointIndex != k_InvalidJointIndex; + + public static DispatchPair Invalid = new DispatchPair { m_Data = 0xffffffffffffffff }; + + public int BodyAIndex + { + get => (int)(m_Data >> 40); + set + { + Assert.IsTrue(value < k_InvalidBodyIndex); + m_Data = (m_Data & 0x000000ffffffffff) | ((ulong)value << 40); + } + } + + public int BodyBIndex + { + get => (int)((m_Data >> 16) & k_InvalidBodyIndex); + set + { + Assert.IsTrue(value < k_InvalidBodyIndex); + m_Data = (m_Data & 0xffffff000000ffff) | ((ulong)value << 16); + } + } + + public int JointIndex + { + get => (int)(m_Data & k_InvalidJointIndex); + set + { + Assert.IsTrue(value < k_InvalidJointIndex); + m_Data = (m_Data & 0xffffffffffff0000) | (uint)(value); + } + } + + public bool JointAllowsCollision + { + get => (m_Data & k_EnableJointCollisionBit) != 0; + } + + public static DispatchPair CreateContact(BodyIndexPair pair) + { + return Create(pair, k_InvalidJointIndex, 0); + } + + public static DispatchPair CreateJoint(BodyIndexPair pair, int jointIndex, int allowCollision) + { + Assert.IsTrue(jointIndex < k_InvalidJointIndex); + return Create(pair, jointIndex, allowCollision); + } + + private static DispatchPair Create(BodyIndexPair pair, int jointIndex, int allowCollision) + { + Assert.IsTrue(pair.BodyAIndex < 0xffffff && pair.BodyBIndex < 0xffffff); + int selectedA = math.min(pair.BodyAIndex, pair.BodyBIndex); + int selectedB = math.max(pair.BodyAIndex, pair.BodyBIndex); + return new DispatchPair + { + m_Data = ((ulong)selectedA << 40) | ((ulong)selectedB << 16) | ((ulong)math.min(1, allowCollision) << 15) | (uint)jointIndex + }; + } + } + + // A phased set of dispatch pairs. + // TODO: Better name for this? + public struct SolverSchedulerInfo : IDisposable + { + // A structure which describes the number of items in a single phase + internal struct SolvePhaseInfo + { + internal int DispatchPairCount; // The total number of pairs in this phase + internal int BatchSize; // The number of items per thread work item; at most, the number of pairs + internal int NumWorkItems; // The amount of "subtasks" of size BatchSize in this phase + internal int FirstWorkItemIndex; // The sum of NumWorkItems in all previous phases. Used for output blockstream. + internal int FirstDispatchPairIndex; // Index into the array of DispatchPairs for this phase. + } + + internal NativeArray PhaseInfo; + internal NativeArray NumActivePhases; + + public int NumPhases => PhaseInfo.Length; + public int NumWorkItemsPerPhase(int phaseId) => PhaseInfo[phaseId].NumWorkItems; + public int FirstWorkItemsPerPhase(int phaseId) => PhaseInfo[phaseId].FirstWorkItemIndex; + + public int NumWorkItems + { + get + { + int numWorkItems = 0; + for (int i = 0; i < PhaseInfo.Length; i++) + { + numWorkItems += PhaseInfo[i].NumWorkItems; + } + return numWorkItems; + } + } + + // For a given work item returns phase id. + internal int FindPhaseId(int workItemIndex) + { + int phaseId = 0; + for (int i = NumActivePhases[0] - 1; i >= 0; i--) + { + if (workItemIndex >= PhaseInfo[i].FirstWorkItemIndex) + { + phaseId = i; + break; + } + } + + return phaseId; + } + + // For a given work item returns index into PhasedDispatchPairs and number of pairs to read. + internal int GetWorkItemReadOffset(int workItemIndex, out int pairReadCount) + { + var phaseInfo = PhaseInfo[FindPhaseId(workItemIndex)]; + + int numItemsToRead = phaseInfo.BatchSize; + int readStartOffset = phaseInfo.FirstDispatchPairIndex + (workItemIndex - phaseInfo.FirstWorkItemIndex) * phaseInfo.BatchSize; + + int lastWorkItemIndex = phaseInfo.FirstWorkItemIndex + phaseInfo.NumWorkItems - 1; + bool isLastWorkItemInPhase = workItemIndex == lastWorkItemIndex; + if (isLastWorkItemInPhase) + { + int numPairsBeforeLastWorkItem = (phaseInfo.NumWorkItems - 1) * phaseInfo.BatchSize; + numItemsToRead = phaseInfo.DispatchPairCount - numPairsBeforeLastWorkItem; + readStartOffset = phaseInfo.FirstDispatchPairIndex + numPairsBeforeLastWorkItem; + } + + pairReadCount = numItemsToRead; + + return readStartOffset; + } + + public SolverSchedulerInfo(int numPhases) + { + PhaseInfo = new NativeArray(numPhases, Allocator.TempJob); + NumActivePhases = new NativeArray(1, Allocator.TempJob); + } + + public void Dispose() + { + if (PhaseInfo.IsCreated) + { + PhaseInfo.Dispose(); + } + + if (NumActivePhases.IsCreated) + { + NumActivePhases.Dispose(); + } + } + + public JobHandle ScheduleDisposeJob(JobHandle inputDeps) + { + return new DisposeJob { PhaseInfo = PhaseInfo, NumActivePhases = NumActivePhases }.Schedule(inputDeps); + } + + // A job to dispose the phase information + [BurstCompile] + private struct DisposeJob : IJob + { + [DeallocateOnJobCompletion] + public NativeArray PhaseInfo; + + [DeallocateOnJobCompletion] + public NativeArray NumActivePhases; + + public void Execute() { } + } + } + + public Scheduler() + { + int numPhases = 17; + m_BitLookupTable = new BitLookupTable(numPhases); + } + + public void Dispose() + { + m_BitLookupTable.Dispose(); + } + + // Sort interacting pairs of bodies into phases for multi-threaded simulation + public unsafe JobHandle ScheduleCreatePhasedDispatchPairsJob( + ref PhysicsWorld world, ref BlockStream dynamicVsDynamicBroadphasePairsStream, ref BlockStream staticVsDynamicBroadphasePairStream, + ref Simulation.Context context, JobHandle inputDeps) + { + JobHandle handle = inputDeps; + + int numDispatchPairs = dynamicVsDynamicBroadphasePairsStream.ComputeItemCount() + staticVsDynamicBroadphasePairStream.ComputeItemCount() + world.Joints.Length; + + // First build a sorted array of dispatch pairs. + NativeArray dispatchPairs; + NativeArray phaseIdPerPair; + NativeArray sortBuffer; + { + phaseIdPerPair = new NativeArray(numDispatchPairs, Allocator.TempJob, NativeArrayOptions.UninitializedMemory); + + // Initialize a pair of buffers + NativeArray unsortedPairs; + { + int dispatchPairBufferSize = Math.NextMultipleOf16(sizeof(DispatchPair) * numDispatchPairs); + sortBuffer = new NativeArray(dispatchPairBufferSize * 2, Allocator.TempJob, NativeArrayOptions.UninitializedMemory); + dispatchPairs = NativeArrayUnsafeUtility.ConvertExistingDataToNativeArray((byte*)sortBuffer.GetUnsafePtr(), numDispatchPairs, Allocator.None); + unsortedPairs = NativeArrayUnsafeUtility.ConvertExistingDataToNativeArray((byte*)sortBuffer.GetUnsafePtr() + dispatchPairBufferSize, numDispatchPairs, Allocator.None); +#if ENABLE_UNITY_COLLECTIONS_CHECKS + NativeArrayUnsafeUtility.SetAtomicSafetyHandle(ref dispatchPairs, NativeArrayUnsafeUtility.GetAtomicSafetyHandle(sortBuffer)); + NativeArrayUnsafeUtility.SetAtomicSafetyHandle(ref unsortedPairs, NativeArrayUnsafeUtility.GetAtomicSafetyHandle(sortBuffer)); +#endif + } + + // Merge broadphase pairs and joint pairs into the unsorted array + handle = new CreateDispatchPairsJob + { + DynamicVsDynamicPairReader = dynamicVsDynamicBroadphasePairsStream, + StaticVsDynamicPairReader = staticVsDynamicBroadphasePairStream, + Joints = world.Joints, + DispatchPairs = unsortedPairs + }.Schedule(handle); + + // Dispose our broad phase pairs + context.DisposeBroadphasePairs = JobHandle.CombineDependencies( + dynamicVsDynamicBroadphasePairsStream.ScheduleDispose(handle), + staticVsDynamicBroadphasePairStream.ScheduleDispose(handle)); + + // Sort into the target array + NativeArray sortedPairsULong = NativeArrayUnsafeUtility.ConvertExistingDataToNativeArray(dispatchPairs.GetUnsafePtr(), dispatchPairs.Length, Allocator.None); +#if ENABLE_UNITY_COLLECTIONS_CHECKS + NativeArrayUnsafeUtility.SetAtomicSafetyHandle(ref sortedPairsULong, NativeArrayUnsafeUtility.GetAtomicSafetyHandle(dispatchPairs)); +#endif + handle = ScheduleSortJob(world.NumBodies, unsortedPairs, sortedPairsULong, handle); + } + + // Create phases for multi-threading + context.SolverSchedulerInfo = new SolverSchedulerInfo(m_BitLookupTable.NumPhases); + context.PhasedDispatchPairsArray = new NativeArray(numDispatchPairs, Allocator.TempJob, NativeArrayOptions.UninitializedMemory); + return new CreateDispatchPairPhasesJob // also deallocates sortBuffer & rigidBodyMask + { + DispatchPairs = dispatchPairs, + RigidBodyMask = new NativeArray(world.NumBodies, Allocator.TempJob, NativeArrayOptions.ClearMemory), + SolverSchedulerInfo = context.SolverSchedulerInfo, + NumDynamicBodies = world.NumDynamicBodies, + BitLookupTable = m_BitLookupTable.Table, + NumPhases = m_BitLookupTable.NumPhases, + FullBuffer = sortBuffer, + PhaseIdPerPair = phaseIdPerPair, + PhasedDispatchPairsArray = context.PhasedDispatchPairsArray + }.Schedule(handle); + } + + #region Helpers + + // A lookup table used by CreateDispatchPairPhasesJob + private struct BitLookupTable : IDisposable + { + public readonly int NumPhases; + public readonly NativeArray Table; + + public BitLookupTable(int numPhases) + { + NumPhases = numPhases; + + Table = new NativeArray(UInt16.MaxValue + 1, Allocator.Persistent, NativeArrayOptions.UninitializedMemory); + const ushort end = UInt16.MaxValue; + ushort numBits = (ushort)(numPhases - 1); + for (ushort value = 0; value < end; value++) + { + ushort valueCopy = value; + for (ushort i = 0; i < numBits; i++) + { + if ((valueCopy & 1) == 0) + { + Table[value] = i; + + break; + } + + valueCopy >>= 1; + } + } + Table[end] = numBits; + } + + public void Dispose() + { + Table.Dispose(); + } + } + + // Helper function to schedule jobs to sort an array of dispatch pairs. + // The first single threaded job is a single pass Radix sort on bits 16th to 40th (bodyA index), + // resulting in sub arrays with the same bodyA index. + // The second parallel job dispatches default sorts on each sub array. + private static unsafe JobHandle ScheduleSortJob( + int numBodies, + NativeArray unsortedPairsIn, + NativeArray sortedPairsOut, + JobHandle handle) + { + NativeArray totalCountUpToDigit = new NativeArray(numBodies + 1, Allocator.TempJob); + + // Calculate number digits needed to encode all body indices + int numDigits = 0; + int maxBodyIndex = numBodies - 1; + { + int val = maxBodyIndex; + while (val > 0) + { + val >>= 1; + numDigits++; + } + } + + // Perform single pass of single threaded radix sort. + NativeArray inArray = NativeArrayUnsafeUtility.ConvertExistingDataToNativeArray(unsortedPairsIn.GetUnsafePtr(), unsortedPairsIn.Length, Allocator.None); +#if ENABLE_UNITY_COLLECTIONS_CHECKS + NativeArrayUnsafeUtility.SetAtomicSafetyHandle(ref inArray, NativeArrayUnsafeUtility.GetAtomicSafetyHandle(unsortedPairsIn)); +#endif + + handle = new RadixSortPerBodyAJob + { + InputArray = inArray, + OutputArray = sortedPairsOut, + MaxDigits = numDigits, + MaxIndex = maxBodyIndex, + DigitCount = totalCountUpToDigit + }.Schedule(handle); + + // Sort sub arrays with default sort. + int numPerBatch = math.max(1, maxBodyIndex / 32); + + handle = new SortSubArraysJob + { + InOutArray = sortedPairsOut, + NextElementIndex = totalCountUpToDigit + }.Schedule(totalCountUpToDigit.Length, numPerBatch, handle); + + return handle; + } + + #endregion + + #region Jobs + + // Combines body pairs and joint pairs into an array of dispatch pairs + [BurstCompile] + private struct CreateDispatchPairsJob : IJob + { + // Body pairs from broadphase overlap jobs + public BlockStream.Reader DynamicVsDynamicPairReader; + public BlockStream.Reader StaticVsDynamicPairReader; + + // Joints from dynamics world + [ReadOnly] + public NativeSlice Joints; + + [NativeDisableContainerSafetyRestriction] + public NativeArray DispatchPairs; + + public void Execute() + { + int counter = 0; + for (int i = 0; i < DynamicVsDynamicPairReader.ForEachCount; i++) + { + DynamicVsDynamicPairReader.BeginForEachIndex(i); + int rangeItemCount = DynamicVsDynamicPairReader.RemainingItemCount; + for (int j = 0; j < rangeItemCount; j++) + { + var pair = DynamicVsDynamicPairReader.Read(); + DispatchPairs[counter++] = DispatchPair.CreateContact(pair); + } + } + + for (int i = 0; i < StaticVsDynamicPairReader.ForEachCount; i++) + { + StaticVsDynamicPairReader.BeginForEachIndex(i); + int rangeItemCount = StaticVsDynamicPairReader.RemainingItemCount; + for (int j = 0; j < rangeItemCount; j++) + { + var pair = StaticVsDynamicPairReader.Read(); + DispatchPairs[counter++] = DispatchPair.CreateContact(pair); + } + } + + for (int i = 0; i < Joints.Length; i++) + { + DispatchPairs[counter++] = DispatchPair.CreateJoint(Joints[i].BodyPair, i, Joints[i].EnableCollision); + } + + Assert.AreEqual(counter, DispatchPairs.Length); + } + } + + // Sorts an array of dispatch pairs by Body A index + [BurstCompile] + public struct RadixSortPerBodyAJob : IJob + { + [ReadOnly] + public NativeArray InputArray; + [NativeDisableContainerSafetyRestriction] + public NativeArray OutputArray; + [NativeDisableContainerSafetyRestriction] + public NativeArray DigitCount; + + public int MaxDigits; + public int MaxIndex; + + public void Execute() + { + const int shift = 40; + + RadixSortPerBodyA(InputArray, OutputArray, DigitCount, MaxDigits, MaxIndex, shift); + } + + // Performs single pass of Radix sort on NativeArray based on 16th to 40th bit. Those bits contain bodyA index in DispatchPair. + public static void RadixSortPerBodyA(NativeArray inputArray, NativeArray outputArray, NativeArray digitCount, int maxDigits, int maxIndex, int shift) + { + ulong mask = ((ulong)(1 << maxDigits) - 1) << shift; + + // Count digits + for (int i = 0; i < inputArray.Length; i++) + { + ulong usIndex = inputArray[i] & mask; + int sIndex = (int)(usIndex >> shift); + digitCount[sIndex]++; + } + + // Calculate start index for each digit + int prev = digitCount[0]; + digitCount[0] = 0; + for (int i = 1; i <= maxIndex; i++) + { + int current = digitCount[i]; + digitCount[i] = digitCount[i - 1] + prev; + prev = current; + } + + // Copy elements into buckets based on bodyA index + for (int i = 0; i < inputArray.Length; i++) + { + ulong value = inputArray[i]; + ulong usindex = value & mask; + int sindex = (int)(usindex >> shift); + int index = digitCount[sindex]++; + if (index == 1 && inputArray.Length == 1) + { + outputArray[0] = 0; + } + outputArray[index] = value; + } + } + } + + // Sorts slices of an array in parallel + [BurstCompile] + public struct SortSubArraysJob : IJobParallelFor + { + [NativeDisableContainerSafetyRestriction] + public NativeArray InOutArray; + + // Typically lastDigitIndex is resulting RadixSortPerBodyAJob.digitCount. nextElementIndex[i] = index of first element with bodyA index == i + 1 + [NativeDisableContainerSafetyRestriction] + [DeallocateOnJobCompletion] public NativeArray NextElementIndex; + + public void Execute(int workItemIndex) + { + int startIndex = 0; + if (workItemIndex > 0) + { + startIndex = NextElementIndex[workItemIndex - 1]; + } + + if (startIndex < InOutArray.Length) + { + int length = NextElementIndex[workItemIndex] - startIndex; + DefaultSortOfSubArrays(InOutArray, startIndex, length); + } + } + + // Sorts sub array using default sort + public static unsafe void DefaultSortOfSubArrays(NativeArray inOutArray, int startIndex, int length) + { + // inOutArray[startIndex] to inOutArray[startIndex + length - 1] have the same bodyA index (16th to 40th big) so we can do a simple sorting. + if (length > 2) + { + // Alias sub array as a new array + NativeArray subArray = NativeArrayUnsafeUtility.ConvertExistingDataToNativeArray((byte*)inOutArray.GetUnsafePtr() + startIndex * sizeof(ulong), length, Allocator.None); +#if ENABLE_UNITY_COLLECTIONS_CHECKS + NativeArrayUnsafeUtility.SetAtomicSafetyHandle(ref subArray, NativeArrayUnsafeUtility.GetAtomicSafetyHandle(inOutArray)); +#endif + subArray.Sort(); + } + else if (length == 2) + { + if (inOutArray[startIndex] > inOutArray[startIndex + 1]) + { + ulong temp = inOutArray[startIndex + 1]; + inOutArray[startIndex + 1] = inOutArray[startIndex]; + inOutArray[startIndex] = temp; + } + } + } + } + + // Creates phases based on sorted list of dispatch pairs + [BurstCompile] + private struct CreateDispatchPairPhasesJob : IJob + { + [ReadOnly] + public NativeArray DispatchPairs; + + [ReadOnly] + public NativeArray BitLookupTable; + + [DeallocateOnJobCompletion] + public NativeArray RigidBodyMask; + + [NativeDisableContainerSafetyRestriction] + [DeallocateOnJobCompletion] + public NativeArray FullBuffer; + + [NativeDisableContainerSafetyRestriction] + public SolverSchedulerInfo SolverSchedulerInfo; + + [DeallocateOnJobCompletion] + public NativeArray PhaseIdPerPair; + + public NativeArray PhasedDispatchPairsArray; + + public int NumDynamicBodies; + public int NumPhases; + + const int k_MinBatchSize = 8; + private int m_LastPhaseIndex; + + public unsafe void Execute() + { + m_LastPhaseIndex = NumPhases - 1; + int* numPairsPerPhase = stackalloc int[NumPhases]; + for (int i = 0; i < NumPhases; i++) + { + numPairsPerPhase[i] = 0; + } + + const byte invalidPhaseId = 0xff; + DispatchPair lastPair = new DispatchPair(); // Guaranteed not to be a real pair, as bodyA==bodyB==0 + bool contactsPermitted = true; // Will avoid creating a contact (=non-joint) pair if this is false + + BatchInfo* batchInfos = stackalloc BatchInfo[NumPhases]; + for (int i = 0; i < NumPhases; i++) + { + batchInfos[i].m_NumDynamicBodies = NumDynamicBodies; + batchInfos[i].m_IsLastPhase = i == m_LastPhaseIndex; + batchInfos[i].m_NumElements = 0; + batchInfos[i].m_PhaseIndex = i; + batchInfos[i].m_PhaseMask = (ushort)(1 << i); + } + + // Find phase for each pair + for (int i = 0; i < DispatchPairs.Length; i++) + { + DispatchPair pair = DispatchPairs[i]; + int bodyAIndex = pair.BodyAIndex; + int bodyBIndex = pair.BodyBIndex; + + bool indicesChanged = lastPair.BodyAIndex != bodyAIndex || lastPair.BodyBIndex != bodyBIndex; + + if (indicesChanged || contactsPermitted || pair.IsJoint) + { + byte phaseIndex = (byte)FindFreePhase(bodyAIndex, bodyBIndex); + numPairsPerPhase[phaseIndex]++; + PhaseIdPerPair[i] = phaseIndex; + + batchInfos[phaseIndex].Add(ref RigidBodyMask, pair.BodyAIndex, pair.BodyBIndex); + + // If _any_ Joint between each Pair has Enable Collision set to true, then contacts will be permitted + bool thisPermitsContacts = pair.IsJoint && pair.JointAllowsCollision; + contactsPermitted = (contactsPermitted && !indicesChanged) || thisPermitsContacts; + } + else + { + PhaseIdPerPair[i] = invalidPhaseId; + } + + lastPair = pair; + } + + // Calculate phase start offset + int* offsetInPhase = stackalloc int[NumPhases]; + offsetInPhase[0] = 0; + for (int i = 1; i < NumPhases; i++) + { + offsetInPhase[i] = offsetInPhase[i - 1] + numPairsPerPhase[i - 1]; + } + + // Populate PhasedDispatchPairsArray + for (int i = 0; i < DispatchPairs.Length; i++) + { + if (PhaseIdPerPair[i] != invalidPhaseId) + { + int phaseForPair = PhaseIdPerPair[i]; + int indexInArray = offsetInPhase[phaseForPair]++; + PhasedDispatchPairsArray[indexInArray] = DispatchPairs[i]; + } + } + + // Populate SolvePhaseInfo for each solve phase + int firstWorkItemIndex = 0; + int numPairs = 0; + int numActivePhases = 0; + for (int i = 0; i < NumPhases; i++) + { + SolverSchedulerInfo.SolvePhaseInfo info; + info.DispatchPairCount = numPairsPerPhase[i]; + + if (info.DispatchPairCount == 0) + { + break; + } + + info.BatchSize = math.min(k_MinBatchSize, info.DispatchPairCount); + info.NumWorkItems = (info.DispatchPairCount + info.BatchSize - 1) / info.BatchSize; + info.FirstWorkItemIndex = firstWorkItemIndex; + info.FirstDispatchPairIndex = numPairs; + + firstWorkItemIndex += info.NumWorkItems; + numPairs += info.DispatchPairCount; + + SolverSchedulerInfo.PhaseInfo[i] = info; + numActivePhases++; + } + + SolverSchedulerInfo.NumActivePhases[0] = numActivePhases; + } + + private unsafe struct BatchInfo + { + internal void Add(ref NativeArray rigidBodyMasks, int bodyAIndex, int bodyBIndex) + { + fixed (int* bodyAIndices = m_BodyAIndices) + { + bodyAIndices[m_NumElements] = bodyAIndex; + } + + fixed (int* bodyBIndices = m_BodyBIndices) + { + bodyBIndices[m_NumElements] = bodyBIndex; + } + + if (++m_NumElements == k_MinBatchSize) + { + Flush(ref rigidBodyMasks); + } + } + + private void Flush(ref NativeArray rigidBodyMasks) + { + for (int i = 0; i < m_NumElements; i++) + { + int bodyA; + int bodyB; + + fixed (int* bodyAs = m_BodyAIndices) + { + bodyA = bodyAs[i]; + } + + fixed (int* bodyBs = m_BodyBIndices) + { + bodyB = bodyBs[i]; + } + + if (!m_IsLastPhase) + { + rigidBodyMasks[bodyA] |= m_PhaseMask; + + if (bodyB < m_NumDynamicBodies) + { + rigidBodyMasks[bodyB] |= m_PhaseMask; + } + } + } + + m_NumElements = 0; + } + + private fixed int m_BodyAIndices[k_MinBatchSize]; + private fixed int m_BodyBIndices[k_MinBatchSize]; + + internal int m_NumDynamicBodies; + internal bool m_IsLastPhase; + internal int m_PhaseIndex; + internal ushort m_PhaseMask; + internal int m_NumElements; + } + + private int FindFreePhase(int bodyAIndex, int bodyBIndex) + { + int mask = RigidBodyMask[bodyAIndex]; + + if (bodyBIndex < NumDynamicBodies) + { + // Don't need this check for bodyA, as we can guarantee it is dynamic. + mask |= RigidBodyMask[bodyBIndex]; + } + + int phaseIndex = BitLookupTable[mask]; + + Assert.IsTrue(phaseIndex >= 0 && phaseIndex <= m_LastPhaseIndex); + return phaseIndex; + } + } + + #endregion + } +} diff --git a/package/Unity.Physics/Dynamics/Simulation/Scheduler.cs.meta b/package/Unity.Physics/Dynamics/Simulation/Scheduler.cs.meta new file mode 100755 index 000000000..4a82946c5 --- /dev/null +++ b/package/Unity.Physics/Dynamics/Simulation/Scheduler.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 7d662241278573340a75d854ddbb6a34 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Unity.Physics/Dynamics/Simulation/Simulation.cs b/package/Unity.Physics/Dynamics/Simulation/Simulation.cs new file mode 100755 index 000000000..eb5492f35 --- /dev/null +++ b/package/Unity.Physics/Dynamics/Simulation/Simulation.cs @@ -0,0 +1,204 @@ +using System; +using Unity.Collections; +using Unity.Jobs; +using Unity.Mathematics; + +namespace Unity.Physics +{ + // Implementations of ISimulation + public enum SimulationType + { + NoPhysics, // A dummy implementation which does nothing + UnityPhysics, // Default C# implementation + HavokPhysics // Havok implementation (using C++ plugin) + } + + // Parameters for a simulation step + public struct SimulationStepInput + { + public PhysicsWorld World; + public float TimeStep; + public float3 Gravity; + public int ThreadCountHint; + public int NumSolverIterations; + public bool SynchronizeCollisionWorld; // whether to update the collision world after the step + public SimulationCallbacks Callbacks; + } + + // Interface for simulations + public interface ISimulation : IDisposable + { + // The implementation type + SimulationType Type { get; } + + // Step the simulation (single threaded) + void Step(SimulationStepInput input); + + // Schedule a set of jobs to step the simulation. Returns two job handles: + // - Jobs which use the simulation results should depend on "finalSimulationJobHandle" + // - The end of each step should depend on "finalHandle" (makes sure all simulation and cleanup is finished) + void ScheduleStepJobs(SimulationStepInput input, JobHandle inputDeps, out JobHandle finalSimulationJobHandle, out JobHandle finalJobHandle); + + // Read-write access to simulation data flowing through the step. + // Warning: Only valid at specific sync points during the step! + SimulationData.BodyPairs BodyPairs { get; } + SimulationData.Contacts Contacts { get; } + SimulationData.Jacobians Jacobians { get; } + + // Events produced by the simulation step. + // Valid until the start of the next step. + CollisionEvents CollisionEvents { get; } + TriggerEvents TriggerEvents { get; } + } + + // Default simulation implementation + public class Simulation : ISimulation + { + public SimulationType Type => SimulationType.UnityPhysics; + + public SimulationData.BodyPairs BodyPairs => new SimulationData.BodyPairs(m_Context.PhasedDispatchPairsArray); + public SimulationData.Contacts Contacts => new SimulationData.Contacts(m_Context.Contacts, m_Context.SolverSchedulerInfo); + public SimulationData.Jacobians Jacobians => new SimulationData.Jacobians(m_Context.Jacobians, m_Context.SolverSchedulerInfo.NumWorkItems); + public CollisionEvents CollisionEvents => new CollisionEvents(m_Context.CollisionEventStream); + public TriggerEvents TriggerEvents => new TriggerEvents(m_Context.TriggerEventStream); + + private readonly Scheduler m_Scheduler; + private Context m_Context; + + public Simulation() + { + m_Scheduler = new Scheduler(); + m_Context = new Context(); + } + + public void Dispose() + { + m_Scheduler.Dispose(); + DisposeEventStreams(); + } + + public void Step(SimulationStepInput input) + { + // TODO : Using the multithreaded version for now, but should do a proper single threaded version + ScheduleStepJobs(input, new JobHandle(), out JobHandle handle1, out JobHandle handle2); + JobHandle.CombineDependencies(handle1, handle2).Complete(); + } + + // Schedule all the jobs for the simulation step. + // Enqueued callbacks can choose to inject additional jobs at defined sync points. + public void ScheduleStepJobs(SimulationStepInput input, JobHandle inputDeps, out JobHandle finalSimulationJobHandle, out JobHandle finalJobHandle) + { + // Dispose event streams from previous frame + DisposeEventStreams(); + + m_Context = new Context(); + + if (input.World.NumDynamicBodies == 0) + { + // No need to do anything, since nothing can move + finalSimulationJobHandle = new JobHandle(); + finalJobHandle = new JobHandle(); + return; + } + + SimulationCallbacks callbacks = input.Callbacks ?? new SimulationCallbacks(); + JobHandle handle = inputDeps; + + // We need to make sure that broadphase tree building is done before we schedule FindOverlapsJobs. + handle.Complete(); + + // Find all body pairs that overlap in the broadphase + handle = input.World.CollisionWorld.Broadphase.ScheduleFindOverlapsJobs( + out BlockStream dynamicVsDynamicBroadphasePairsStream, out BlockStream staticVsDynamicBroadphasePairsStream, handle); + handle.Complete(); // Need to know the total number of pairs before continuing + + // Create phased dispatch pairs for all interacting body pairs + handle = m_Scheduler.ScheduleCreatePhasedDispatchPairsJob( + ref input.World, ref dynamicVsDynamicBroadphasePairsStream, ref staticVsDynamicBroadphasePairsStream, ref m_Context, handle); + handle.Complete(); // Need to know the total number of work items before continuing + handle = callbacks.Execute(SimulationCallbacks.Phase.PostCreateDispatchPairs, this, handle); + m_Context.CreateBodyPairsHandle = handle; + + // Create contact points & joint Jacobians + handle = NarrowPhase.ScheduleProcessBodyPairsJobs(ref input.World, input.TimeStep, input.NumSolverIterations, ref m_Context, handle); + handle = callbacks.Execute(SimulationCallbacks.Phase.PostCreateContacts, this, handle); + m_Context.CreateContactsHandle = handle; + + // Create contact Jacobians + handle = Solver.ScheduleBuildContactJacobiansJobs(ref input.World.DynamicsWorld, input.TimeStep, ref m_Context, handle); + handle = callbacks.Execute(SimulationCallbacks.Phase.PostCreateContactJacobians, this, handle); + m_Context.CreateContactJacobiansHandle = handle; + + // Solve all Jacobians + int numIterations = input.NumSolverIterations > 0 ? input.NumSolverIterations : 4; + handle = Solver.ScheduleSolveJacobiansJobs(ref input.World.DynamicsWorld, input.TimeStep, input.Gravity, numIterations, ref m_Context, handle); + handle = callbacks.Execute(SimulationCallbacks.Phase.PostSolveJacobians, this, handle); + m_Context.SolveContactJacobiansHandle = handle; + + // Integration motions + handle = Integrator.ScheduleIntegrateJobs(ref input.World.DynamicsWorld, input.TimeStep, input.Gravity, handle); + handle = callbacks.Execute(SimulationCallbacks.Phase.PostIntegrateMotions, this, handle); + m_Context.IntegrateMotionsHandle = handle; + + // Synchronize the collision world + if (input.SynchronizeCollisionWorld) + { + handle = input.World.CollisionWorld.ScheduleUpdateDynamicLayer(ref input.World, input.TimeStep, input.ThreadCountHint, handle); // TODO: timeStep = 0? + } + + // Return the final simulation handle + finalSimulationJobHandle = handle; + + // Return the final handle, which includes disposing temporary arrays + finalJobHandle = JobHandle.CombineDependencies(finalSimulationJobHandle, m_Context.DisposeBroadphasePairs, m_Context.DisposeContacts); + finalJobHandle = JobHandle.CombineDependencies(finalJobHandle, m_Context.DisposeJacobians, m_Context.DisposeJointJacobians); + finalJobHandle = JobHandle.CombineDependencies(finalJobHandle, m_Context.DisposeSolverSchedulerData); + } + + // Event streams live until the next step, so they should be disposed at the beginning of it + private void DisposeEventStreams() + { + if (m_Context.CollisionEventStream.IsCreated) + { + m_Context.CollisionEventStream.Dispose(); + } + if (m_Context.TriggerEventStream.IsCreated) + { + m_Context.TriggerEventStream.Dispose(); + } + } + + // Temporary data created and destroyed during the step + public struct Context + { + // Built by the scheduler. Groups body pairs into phases in which each + // body appears at most once, so that the interactions within each phase can be solved + // in parallel with each other but not with other phases. This is consumed by the + // ProcessBodyPairsJob, which outputs Contacts and Joint jacobians. + public NativeArray PhasedDispatchPairsArray; + + // Built by the scheduler. Describes the grouping of PhasedBodyPairs + // which informs how we can schedule the solver jobs and where they read info from. + // Freed by the PhysicsEnd system. + public Scheduler.SolverSchedulerInfo SolverSchedulerInfo; + + public BlockStream Contacts; + public BlockStream Jacobians; + public BlockStream JointJacobians; + public BlockStream CollisionEventStream; + public BlockStream TriggerEventStream; + + public JobHandle CreateBodyPairsHandle; + public JobHandle CreateContactsHandle; + public JobHandle CreateContactJacobiansHandle; + public JobHandle SolveContactJacobiansHandle; + public JobHandle IntegrateMotionsHandle; + + public JobHandle DisposeBroadphasePairs; + public JobHandle DisposeContacts; + public JobHandle DisposeJacobians; + public JobHandle DisposeJointJacobians; + public JobHandle DisposeSolverSchedulerData; + } + } +} diff --git a/package/Unity.Physics/Dynamics/Simulation/Simulation.cs.meta b/package/Unity.Physics/Dynamics/Simulation/Simulation.cs.meta new file mode 100755 index 000000000..975067b85 --- /dev/null +++ b/package/Unity.Physics/Dynamics/Simulation/Simulation.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 7c8ffabd78945a642a104fc69e47632a +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Unity.Physics/Dynamics/Simulation/SimulationData.cs b/package/Unity.Physics/Dynamics/Simulation/SimulationData.cs new file mode 100755 index 000000000..eb3ceeab1 --- /dev/null +++ b/package/Unity.Physics/Dynamics/Simulation/SimulationData.cs @@ -0,0 +1,251 @@ +using System; +using Unity.Collections; +using Unity.Collections.LowLevel.Unsafe; +using Unity.Mathematics; + +namespace Unity.Physics +{ + // Utilities for accessing the data flowing through a simulation step in a consistent way, + // regardless of which simulation implementation is being used. + // WORK IN PROGRESS. + public static class SimulationData + { + // Access to the body pairs produced by the broad phase + public struct BodyPairs + { + readonly SimulationType m_SimulationType; + + // Members for Unity simulation + [NativeDisableContainerSafetyRestriction] + NativeArray m_PhasedDispatchPairs; + + // Constructor for Unity simulation + public unsafe BodyPairs(NativeArray phasedCollisionPairs) + { + m_SimulationType = SimulationType.UnityPhysics; + m_PhasedDispatchPairs = phasedCollisionPairs; + } + + // Create an iterator + public Iterator GetIterator() => new Iterator(this); + + // An iterator with read-write access to the body pairs + public struct Iterator + { + BodyPairs m_Pairs; + int m_CurrentPairIndex; + + public unsafe Iterator(BodyPairs pairs) + { + m_Pairs = pairs; + m_CurrentPairIndex = 0; + } + + // Returns true is there are more pairs to iterate through + public unsafe bool HasPairsLeft() + { + if (m_Pairs.m_SimulationType == SimulationType.UnityPhysics) + { + return m_CurrentPairIndex <= m_Pairs.m_PhasedDispatchPairs.Length - 1; + } + return false; + } + + // Move to the next body pair + public unsafe BodyIndexPair NextPair() + { + if (m_Pairs.m_SimulationType == SimulationType.UnityPhysics) + { + Scheduler.DispatchPair pair = m_Pairs.m_PhasedDispatchPairs[m_CurrentPairIndex++]; + return new BodyIndexPair { BodyAIndex = pair.BodyAIndex, BodyBIndex = pair.BodyBIndex }; + } + return BodyIndexPair.Invalid; + } + + // Disable the previous body pair + public void DisableLastPair() + { + if (m_Pairs.m_SimulationType == SimulationType.UnityPhysics) + { + if (m_CurrentPairIndex != 0) + { + m_Pairs.m_PhasedDispatchPairs[m_CurrentPairIndex - 1] = Scheduler.DispatchPair.Invalid; + } + } + } + } + } + + // Access to the contacts produced by the narrow phase + public struct Contacts + { + readonly SimulationType m_SimulationType; + [NativeDisableContainerSafetyRestriction] + BlockStream.Writer m_ContactWriter; + int m_NumContactsAdded; + + // Members for Unity simulation + //@TODO: Unity should have a Allow null safety restriction + [NativeDisableContainerSafetyRestriction] + BlockStream m_ContactStream; + [NativeDisableContainerSafetyRestriction] + NativeArray m_PhaseInfo; + readonly int m_MaxNumWorkItems; + + public int NumWorkItems => m_MaxNumWorkItems; + + // Constructor for Unity simulation + public unsafe Contacts(BlockStream contactStream, Scheduler.SolverSchedulerInfo ssi) + { + m_SimulationType = SimulationType.UnityPhysics; + m_ContactWriter = new BlockStream.Writer(contactStream); + m_NumContactsAdded = 0; + + m_ContactStream = contactStream; + m_PhaseInfo = ssi.PhaseInfo; + m_MaxNumWorkItems = ssi.NumWorkItems; + } + + // Create an iterator over the contact manifolds + public Iterator GetIterator() => new Iterator(this); + + // Add a contact point + // 0; + } + + public ContactHeader GetNextContactHeader() + { + while (m_NumPointsLeft > 0) + { + m_ContactReader.Read(); + } + + m_LastHeader = m_ContactReader.Read(); + m_NumPointsLeft = m_LastHeader.NumContacts; + return m_LastHeader; + } + + public ContactPoint GetNextContact() + { + //(); + + while (m_ContactReader.RemainingItemCount == 0 && m_CurrentWorkItem < m_MaxNumWorkItems) + { + m_ContactReader.BeginForEachIndex(m_CurrentWorkItem); + m_CurrentWorkItem++; + } + + return cp; + } + + public void SetManifoldNormal(float3 newNormal) + { + m_LastHeader.Normal = newNormal; + m_ContactReader.Write(m_LastHeader); + } + + public void UpdatePreviousContactHeader(ContactHeader newHeader) + { + // m_IsCreated + ? new JacobianIterator(m_JacobianStreamReader, m_WorkItemIndex, iterateAll : true) + : new JacobianIterator(); + } + } +} diff --git a/package/Unity.Physics/Dynamics/Simulation/SimulationData.cs.meta b/package/Unity.Physics/Dynamics/Simulation/SimulationData.cs.meta new file mode 100755 index 000000000..8991e4e2e --- /dev/null +++ b/package/Unity.Physics/Dynamics/Simulation/SimulationData.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: a073d4fc854a6fc4fba3df264bf988db +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Unity.Physics/Dynamics/Solver.meta b/package/Unity.Physics/Dynamics/Solver.meta new file mode 100755 index 000000000..e20aec752 --- /dev/null +++ b/package/Unity.Physics/Dynamics/Solver.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: 69562b715cf0a5d429c6ac27988868a2 +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Unity.Physics/Dynamics/Solver/Solver.cs b/package/Unity.Physics/Dynamics/Solver/Solver.cs new file mode 100755 index 000000000..038c4d4f0 --- /dev/null +++ b/package/Unity.Physics/Dynamics/Solver/Solver.cs @@ -0,0 +1,648 @@ +using System; +using Unity.Burst; +using Unity.Collections; +using Unity.Collections.LowLevel.Unsafe; +using Unity.Jobs; +using Unity.Mathematics; +using UnityEngine.Assertions; +using static Unity.Physics.Math; + +namespace Unity.Physics +{ + public static class Solver + { + public struct StepInput + { + public bool IsLastIteration; + public float InvNumSolverIterations; + public float Timestep; + public float GravityLength; + } + + // Schedule some jobs to build Jacobians from the contacts stored in the simulation context + public static JobHandle ScheduleBuildContactJacobiansJobs(ref DynamicsWorld world, float timeStep, ref Simulation.Context context, JobHandle inputDeps) + { + var buildJob = new BuildContactJacobiansJob + { + ContactReader = context.Contacts, + JointJacobianReader = context.JointJacobians, + JacobianWriter = context.Jacobians, + TimeStep = timeStep, + MotionDatas = world.MotionDatas, + MotionVelocities = world.MotionVelocities + }; + + int numWorkItems = context.SolverSchedulerInfo.NumWorkItems; + JobHandle handle = buildJob.Schedule(numWorkItems, 1, inputDeps); + + context.DisposeContacts = context.Contacts.ScheduleDispose(handle); + + return handle; + } + + // Schedule some jobs to solve the Jacobians stored in the simulation context + public static unsafe JobHandle ScheduleSolveJacobiansJobs(ref DynamicsWorld dynamicsWorld, float timestep, float3 gravity, int numIterations, ref Simulation.Context context, JobHandle inputDeps) + { + JobHandle handle = inputDeps; + + int numPhases = context.SolverSchedulerInfo.NumPhases; + + // Use persistent allocator to allow these to live until the start of next step + int numWorkItems = math.max(context.SolverSchedulerInfo.NumWorkItems, 1); // Need at least one work item if user is going to add contacts + { + context.CollisionEventStream = new BlockStream(numWorkItems, 0xb17b474f, Allocator.Persistent); + context.TriggerEventStream = new BlockStream(numWorkItems, 0x43875d8f, Allocator.Persistent); + + float invNumIterations = math.rcp(numIterations); + float gravityLength = math.length(gravity); + for (int solverIterationId = 0; solverIterationId < numIterations; solverIterationId++) + { + bool lastIteration = solverIterationId == numIterations - 1; + for (int phaseId = 0; phaseId < numPhases; phaseId++) + { + int numWorkItemsPerPhase = context.SolverSchedulerInfo.NumWorkItemsPerPhase(phaseId); + if (numWorkItemsPerPhase == 0) + { + continue; + } + + var job = new SolverJob + { + JacobianReader = context.Jacobians, + WorkItemStartIndexOffset = context.SolverSchedulerInfo.FirstWorkItemsPerPhase(phaseId), + MotionVelocities = dynamicsWorld.MotionVelocities, + StepInput = new StepInput + { + InvNumSolverIterations = invNumIterations, + IsLastIteration = lastIteration, + Timestep = timestep, + GravityLength = gravityLength + } + }; + + // Only initialize event writers for last solver iteration jobs + if (lastIteration) + { + job.CollisionEventsWriter = context.CollisionEventStream; + job.TriggerEventsWriter = context.TriggerEventStream; + } + + bool isLastPhase = phaseId == numPhases - 1; + int batchSize = isLastPhase ? numWorkItemsPerPhase : 1; + handle = job.Schedule(numWorkItemsPerPhase, batchSize, handle); + } + } + } + + // Dispose processed data + context.DisposeJacobians = context.Jacobians.ScheduleDispose(handle); + context.DisposeJointJacobians = context.JointJacobians.ScheduleDispose(handle); + context.DisposeSolverSchedulerData = context.SolverSchedulerInfo.ScheduleDisposeJob(handle); + + return handle; + } + + #region Jobs + + [BurstCompile] + private struct BuildContactJacobiansJob : IJobParallelFor + { + [ReadOnly] public NativeSlice MotionDatas; + [ReadOnly] public NativeSlice MotionVelocities; + + public BlockStream.Reader ContactReader; + public BlockStream.Reader JointJacobianReader; + public BlockStream.Writer JacobianWriter; + public float TimeStep; + + public void Execute(int workItemIndex) + { + BuildContactJacobians(ref MotionDatas, ref MotionVelocities, ref ContactReader, ref JointJacobianReader, ref JacobianWriter, + TimeStep, workItemIndex); + } + } + + [BurstCompile] + private struct SolverJob : IJobParallelFor + { + [NativeDisableContainerSafetyRestriction] + public NativeSlice MotionVelocities; + + public BlockStream.Reader JacobianReader; + + //@TODO: Unity should have a Allow null safety restriction + [NativeDisableContainerSafetyRestriction] + public BlockStream.Writer CollisionEventsWriter; + //@TODO: Unity should have a Allow null safety restriction + [NativeDisableContainerSafetyRestriction] + public BlockStream.Writer TriggerEventsWriter; + + public int WorkItemStartIndexOffset; + public StepInput StepInput; + + public void Execute(int workItemIndex) + { + Solve(MotionVelocities, ref JacobianReader, ref CollisionEventsWriter, ref TriggerEventsWriter, workItemIndex + WorkItemStartIndexOffset, StepInput); + } + } + + #endregion + + #region Implementation + + private static void BuildJacobian(MTransform worldFromA, MTransform worldFromB, float3 normal, float3 armA, float3 armB, + float3 invInertiaA, float3 invInertiaB, float sumInvMass, out float3 angularA, out float3 angularB, out float invEffectiveMass) + { + float3 crossA = math.cross(armA, normal); + angularA = math.mul(worldFromA.InverseRotation, crossA).xyz; + + float3 crossB = math.cross(normal, armB); + angularB = math.mul(worldFromB.InverseRotation, crossB).xyz; + + float3 temp = angularA * angularA * invInertiaA + angularB * angularB * invInertiaB; + invEffectiveMass = temp.x + temp.y + temp.z + sumInvMass; + } + + private static void BuildContactJacobian( + int contactPointIndex, + float3 normal, + MTransform worldFromA, + MTransform worldFromB, + float timestep, + float invDt, + MotionVelocity velocityA, + MotionVelocity velocityB, + float sumInvMass, + ref JacobianHeader jacobianHeader, + ref float3 centerA, + ref float3 centerB, + ref BlockStream.Reader contactReader) + { + ref ContactJacAngAndVelToReachCp jacAngular = ref jacobianHeader.AccessAngularJacobian(contactPointIndex); + ContactPoint contact = contactReader.Read(); + float3 pointOnB = contact.Position; + float3 pointOnA = contact.Position + normal * contact.Distance; + float3 armA = pointOnA - worldFromA.Translation; + float3 armB = pointOnB - worldFromB.Translation; + float invEffectiveMass; + BuildJacobian(worldFromA, worldFromB, normal, armA, armB, velocityA.InverseInertiaAndMass.xyz, velocityB.InverseInertiaAndMass.xyz, sumInvMass, + out jacAngular.Jac.AngularA, out jacAngular.Jac.AngularB, out invEffectiveMass); + jacAngular.Jac.EffectiveMass = 1.0f / invEffectiveMass; + jacAngular.Jac.Impulse = 0.0f; + + float solveDistance = contact.Distance; + float solveVelocity = solveDistance * invDt; + + // If contact distance is negative, use an artificially reduced penetration depth to prevent the contact from depenetrating too quickly + const float maxDepenetrationVelocity = 3.0f; // meter/seconds time step independent + solveVelocity = math.max(-maxDepenetrationVelocity, solveVelocity); + + jacAngular.VelToReachCp = -solveVelocity; + + // Calculate average position for friction + centerA += armA; + centerB += armB; + } + + private static void InitModifierData(ref JacobianHeader jacobianHeader, ColliderKeyPair colliderKeys) + { + if (jacobianHeader.HasColliderKeys) + { + jacobianHeader.AccessColliderKeys() = colliderKeys; + } + if (jacobianHeader.HasSurfaceVelocity) + { + jacobianHeader.AccessSurfaceVelocity() = new SurfaceVelocity(); + } + if (jacobianHeader.HasMaxImpulse) + { + jacobianHeader.AccessMaxImpulse() = float.MaxValue; + } + if (jacobianHeader.HasMassFactors) + { + jacobianHeader.AccessMassFactors() = MassFactors.Default; + } + } + + private static void GetMotions( + BodyIndexPair pair, + ref NativeSlice motionDatas, + ref NativeSlice motionVelocities, + out MotionVelocity velocityA, + out MotionVelocity velocityB, + out MTransform worldFromA, + out MTransform worldFromB) + { + bool bodyAIsStatic = pair.BodyAIndex >= motionVelocities.Length; + bool bodyBIsStatic = pair.BodyBIndex >= motionVelocities.Length; + + if (bodyAIsStatic) + { + if (bodyBIsStatic) + { + Assert.IsTrue(false); // static-static pairs should have been filtered during broadphase overlap test + velocityA = MotionVelocity.Zero; + velocityB = MotionVelocity.Zero; + worldFromA = MTransform.Identity; + worldFromB = MTransform.Identity; + return; + } + + velocityA = MotionVelocity.Zero; + velocityB = motionVelocities[pair.BodyBIndex]; + + worldFromA = MTransform.Identity; + worldFromB = new MTransform(motionDatas[pair.BodyBIndex].WorldFromMotion); + } + else if (bodyBIsStatic) + { + velocityA = motionVelocities[pair.BodyAIndex]; + velocityB = MotionVelocity.Zero; + + worldFromA = new MTransform(motionDatas[pair.BodyAIndex].WorldFromMotion); + worldFromB = MTransform.Identity; + } + else + { + velocityA = motionVelocities[pair.BodyAIndex]; + velocityB = motionVelocities[pair.BodyBIndex]; + + worldFromA = new MTransform(motionDatas[pair.BodyAIndex].WorldFromMotion); + worldFromB = new MTransform(motionDatas[pair.BodyBIndex].WorldFromMotion); + } + } + + private static void GetMotionVelocities( + BodyIndexPair pair, + ref NativeSlice motionVelocities, + out MotionVelocity velocityA, + out MotionVelocity velocityB) + { + bool bodyAIsStatic = pair.BodyAIndex >= motionVelocities.Length; + bool bodyBIsStatic = pair.BodyBIndex >= motionVelocities.Length; + + if (bodyAIsStatic) + { + if (bodyBIsStatic) + { + Assert.IsTrue(false); // static-static pairs should have been filtered during broadphase overlap test + velocityA = MotionVelocity.Zero; + velocityB = MotionVelocity.Zero; + return; + } + + velocityA = MotionVelocity.Zero; + velocityB = motionVelocities[pair.BodyBIndex]; + } + else if (bodyBIsStatic) + { + velocityA = motionVelocities[pair.BodyAIndex]; + velocityB = MotionVelocity.Zero; + } + else + { + velocityA = motionVelocities[pair.BodyAIndex]; + velocityB = motionVelocities[pair.BodyBIndex]; + } + } + + private static unsafe void AppendJointJacobiansToContactStream( + int workItemIndex, ref BlockStream.Reader jointJacobianReader, ref BlockStream.Writer jacobianWriter) + { + JacobianIterator jacIterator = new JacobianIterator(jointJacobianReader, workItemIndex); + while (jacIterator.HasJacobiansLeft()) + { + short jacobianSize; + ref JacobianHeader jacHeader = ref jacIterator.ReadJacobianHeader(out jacobianSize); + if ((jacHeader.Flags & JacobianFlags.Disabled) == 0) + { + // Allocate enough memory and copy the data + jacobianWriter.Write(jacobianSize); + byte* jacDataPtr = jacobianWriter.Allocate(jacobianSize); + ref JacobianHeader copiedJacobianHeader = ref UnsafeUtilityEx.AsRef(jacDataPtr); + copiedJacobianHeader = jacHeader; + + switch (jacHeader.Type) + { + case JacobianType.LinearLimit: + copiedJacobianHeader.AccessBaseJacobian() = jacHeader.AccessBaseJacobian(); + break; + case JacobianType.AngularLimit1D: + copiedJacobianHeader.AccessBaseJacobian() = jacHeader.AccessBaseJacobian(); + break; + case JacobianType.AngularLimit2D: + copiedJacobianHeader.AccessBaseJacobian() = jacHeader.AccessBaseJacobian(); + break; + case JacobianType.AngularLimit3D: + copiedJacobianHeader.AccessBaseJacobian() = jacHeader.AccessBaseJacobian(); + break; + default: + throw new NotImplementedException(); + } + } + } + } + + private static unsafe void BuildContactJacobians( + ref NativeSlice motionDatas, + ref NativeSlice motionVelocities, + ref BlockStream.Reader contactReader, + ref BlockStream.Reader jointJacobianReader, + ref BlockStream.Writer jacobianWriter, + float timestep, + int workItemIndex) + { + float invDt = 1.0f / timestep; + + contactReader.BeginForEachIndex(workItemIndex); + jacobianWriter.BeginForEachIndex(workItemIndex); + while (contactReader.RemainingItemCount > 0) + { + // Get the motion pair + ref ContactHeader contactHeader = ref contactReader.Read(); + MotionVelocity velocityA, velocityB; + MTransform worldFromA, worldFromB; + GetMotions(contactHeader.BodyPair, ref motionDatas, ref motionVelocities, out velocityA, out velocityB, out worldFromA, out worldFromB); + + float sumInvMass = velocityA.InverseInertiaAndMass.w + velocityB.InverseInertiaAndMass.w; + + if (sumInvMass == 0) + { + // Skip contacts between two infinite-mass objects + for (int j = 0; j < contactHeader.NumContacts; j++) + { + contactReader.Read(); + } + continue; + } + + JacobianType jacType = ((int)(contactHeader.JacobianFlags) & (int)(JacobianFlags.IsTrigger)) != 0 ? + JacobianType.Trigger : JacobianType.Contact; + JacobianFlags jacFlags = contactHeader.JacobianFlags; + + // Write size before every jacobian + short jacobianSize = (short)JacobianHeader.CalculateSize(jacType, jacFlags, contactHeader.NumContacts); + jacobianWriter.Write(jacobianSize); + + // Allocate all necessary data for this jacobian + byte* jacDataPtr = jacobianWriter.Allocate(jacobianSize); + ref JacobianHeader jacobianHeader = ref UnsafeUtilityEx.AsRef(jacDataPtr); + jacobianHeader.BodyPair = contactHeader.BodyPair; + jacobianHeader.Type = jacType; + jacobianHeader.Flags = jacFlags; + + BaseContactJacobian baseJac = new BaseContactJacobian(); + baseJac.NumContacts = contactHeader.NumContacts; + baseJac.Normal = contactHeader.Normal; + + if (jacobianHeader.Type == JacobianType.Contact) + { + ref ContactJacobian contactJacobian = ref jacobianHeader.AccessBaseJacobian(); + contactJacobian.BaseJacobian = baseJac; + contactJacobian.CoefficientOfFriction = contactHeader.CoefficientOfFriction; + contactJacobian.CoefficientOfRestitution = contactHeader.CoefficientOfRestitution; + + // Initialize modifier data (in order from JacobianModifierFlags) before angular jacobians + InitModifierData(ref jacobianHeader, contactHeader.ColliderKeys); + + // Build normal jacobians + float3 centerA = new float3(0.0f); + float3 centerB = new float3(0.0f); + for (int j = 0; j < contactHeader.NumContacts; j++) + { + // Build the jacobian + BuildContactJacobian( + j, contactJacobian.BaseJacobian.Normal, worldFromA, worldFromB, timestep, invDt, velocityA, velocityB, sumInvMass, + ref jacobianHeader, ref centerA, ref centerB, ref contactReader); + } + + // Build friction jacobians + { + // Clear accumulated impulse + contactJacobian.Friction0.Impulse = 0.0f; + contactJacobian.Friction1.Impulse = 0.0f; + contactJacobian.AngularFriction.Impulse = 0.0f; + + // Calculate average position + float invNumContacts = math.rcp(contactJacobian.BaseJacobian.NumContacts); + centerA *= invNumContacts; + centerB *= invNumContacts; + + // Choose friction axes + CalculatePerpendicularNormalized(contactJacobian.BaseJacobian.Normal, out float3 frictionDir0, out float3 frictionDir1); + + // Build linear jacobian + float invEffectiveMass0, invEffectiveMass1; + { + float3 armA = centerA; + float3 armB = centerB; + BuildJacobian(worldFromA, worldFromB, frictionDir0, armA, armB, velocityA.InverseInertiaAndMass.xyz, velocityB.InverseInertiaAndMass.xyz, sumInvMass, + out contactJacobian.Friction0.AngularA, out contactJacobian.Friction0.AngularB, out invEffectiveMass0); + BuildJacobian(worldFromA, worldFromB, frictionDir1, armA, armB, velocityA.InverseInertiaAndMass.xyz, velocityB.InverseInertiaAndMass.xyz, sumInvMass, + out contactJacobian.Friction1.AngularA, out contactJacobian.Friction1.AngularB, out invEffectiveMass1); + } + + // Build angular jacobian + float invEffectiveMassAngular; + { + contactJacobian.AngularFriction.AngularA = math.mul(worldFromA.InverseRotation, contactJacobian.BaseJacobian.Normal); + contactJacobian.AngularFriction.AngularB = math.mul(worldFromB.InverseRotation, -contactJacobian.BaseJacobian.Normal); + float3 temp = contactJacobian.AngularFriction.AngularA * contactJacobian.AngularFriction.AngularA * velocityA.InverseInertiaAndMass.xyz; + temp += contactJacobian.AngularFriction.AngularB * contactJacobian.AngularFriction.AngularB * velocityB.InverseInertiaAndMass.xyz; + invEffectiveMassAngular = math.csum(temp); + } + + // Build effective mass + { + // Build the inverse effective mass matrix + float3 invEffectiveMassDiag = new float3(invEffectiveMass0, invEffectiveMass1, invEffectiveMassAngular); + float3 invEffectiveMassOffDiag = new float3( // (0, 1), (0, 2), (1, 2) + JacobianUtilities.CalculateInvEffectiveMassOffDiag(contactJacobian.Friction0.AngularA, contactJacobian.Friction1.AngularA, velocityA.InverseInertiaAndMass.xyz, + contactJacobian.Friction0.AngularB, contactJacobian.Friction1.AngularB, velocityB.InverseInertiaAndMass.xyz), + JacobianUtilities.CalculateInvEffectiveMassOffDiag(contactJacobian.Friction0.AngularA, contactJacobian.AngularFriction.AngularA, velocityA.InverseInertiaAndMass.xyz, + contactJacobian.Friction0.AngularB, contactJacobian.AngularFriction.AngularB, velocityB.InverseInertiaAndMass.xyz), + JacobianUtilities.CalculateInvEffectiveMassOffDiag(contactJacobian.Friction1.AngularA, contactJacobian.AngularFriction.AngularA, velocityA.InverseInertiaAndMass.xyz, + contactJacobian.Friction1.AngularB, contactJacobian.AngularFriction.AngularB, velocityB.InverseInertiaAndMass.xyz)); + + // Invert the matrix and store it to the jacobians + float3 effectiveMassDiag, effectiveMassOffDiag; + if (!JacobianUtilities.InvertSymmetricMatrix(invEffectiveMassDiag, invEffectiveMassOffDiag, out effectiveMassDiag, out effectiveMassOffDiag)) + { + // invEffectiveMass can be singular if the bodies have infinite inertia about the normal. + // In that case angular friction does nothing so we can regularize the matrix, set col2 = row2 = (0, 0, 1) + invEffectiveMassOffDiag.y = 0.0f; + invEffectiveMassOffDiag.z = 0.0f; + invEffectiveMassDiag.z = 1.0f; + bool success = JacobianUtilities.InvertSymmetricMatrix(invEffectiveMassDiag, invEffectiveMassOffDiag, out effectiveMassDiag, out effectiveMassOffDiag); + Assert.IsTrue(success); // it should never fail, if it does then friction will be disabled + } + contactJacobian.Friction0.EffectiveMass = effectiveMassDiag.x; + contactJacobian.Friction1.EffectiveMass = effectiveMassDiag.y; + contactJacobian.AngularFriction.EffectiveMass = effectiveMassDiag.z; + contactJacobian.FrictionEffectiveMassOffDiag = effectiveMassOffDiag; + } + } + } + // Much less data needed for triggers + else + { + ref TriggerJacobian triggerJacobian = ref jacobianHeader.AccessBaseJacobian(); + + triggerJacobian.BaseJacobian = baseJac; + triggerJacobian.ColliderKeys = contactHeader.ColliderKeys; + + // Build normal jacobians + float3 centerA = new float3(0.0f); + float3 centerB = new float3(0.0f); + for (int j = 0; j < contactHeader.NumContacts; j++) + { + // Build the jacobian + BuildContactJacobian( + j, triggerJacobian.BaseJacobian.Normal, worldFromA, worldFromB, timestep, invDt, velocityA, velocityB, sumInvMass, + ref jacobianHeader, ref centerA, ref centerB, ref contactReader); + } + } + } + + // Copy joint jacobians into the main jacobian stream + AppendJointJacobiansToContactStream(workItemIndex, ref jointJacobianReader, ref jacobianWriter); + + jacobianWriter.EndForEachIndex(); + } + + public static unsafe void BuildJointJacobian(JointData* jointData, BodyIndexPair pair, + MotionVelocity velocityA, MotionVelocity velocityB, MotionData motionA, MotionData motionB, + float timestep, int numIterations, ref BlockStream.Writer jacobianWriter) + { + MTransform bodyAFromMotionA = new MTransform(motionA.BodyFromMotion); + MTransform motionAFromJoint = Mul(Inverse(bodyAFromMotionA), jointData->AFromJoint); + + MTransform bodyBFromMotionB = new MTransform(motionB.BodyFromMotion); + MTransform motionBFromJoint = Mul(Inverse(bodyBFromMotionB), jointData->BFromJoint); + + for (int i = 0; i < jointData->NumConstraints; i++) + { + Constraint constraint = jointData->Constraints[i]; + + JacobianType jacType; + switch (constraint.Type) + { + case ConstraintType.Linear: + jacType = JacobianType.LinearLimit; + break; + case ConstraintType.Angular: + switch (constraint.Dimension) + { + case 1: + jacType = JacobianType.AngularLimit1D; + break; + case 2: + jacType = JacobianType.AngularLimit2D; + break; + case 3: + jacType = JacobianType.AngularLimit3D; + break; + default: + throw new NotImplementedException(); + } + break; + default: + throw new NotImplementedException(); + } + + // Write size before every jacobian + JacobianFlags jacFlags = 0; + short jacobianSize = (short)JacobianHeader.CalculateSize(jacType, jacFlags); + jacobianWriter.Write(jacobianSize); + + // Allocate all necessary data for this jacobian + byte* jacDataPtr = jacobianWriter.Allocate(jacobianSize); + ref JacobianHeader header = ref UnsafeUtilityEx.AsRef(jacDataPtr); + header.BodyPair = pair; + header.Type = jacType; + header.Flags = jacFlags; + + JacobianUtilities.CalculateTauAndDamping(constraint, timestep, numIterations, out float tau, out float damping); + + // Build the Jacobian + switch (constraint.Type) + { + case ConstraintType.Linear: + header.AccessBaseJacobian().Build( + motionAFromJoint, motionBFromJoint, + velocityA, velocityB, motionA, motionB, constraint, tau, damping); + break; + case ConstraintType.Angular: + switch (constraint.Dimension) + { + case 1: + header.AccessBaseJacobian().Build( + motionAFromJoint, motionBFromJoint, + velocityA, velocityB, motionA, motionB, constraint, tau, damping); + break; + case 2: + header.AccessBaseJacobian().Build( + motionAFromJoint, motionBFromJoint, + velocityA, velocityB, motionA, motionB, constraint, tau, damping); + break; + case 3: + header.AccessBaseJacobian().Build( + motionAFromJoint, motionBFromJoint, + velocityA, velocityB, motionA, motionB, constraint, tau, damping); + break; + default: + throw new NotImplementedException(); + } + break; + default: + throw new NotImplementedException(); + } + } + } + + private static void Solve( + NativeSlice motionVelocities, + ref BlockStream.Reader jacobianReader, + ref BlockStream.Writer collisionEventsWriter, + ref BlockStream.Writer triggerEventsWriter, + int workItemIndex, + StepInput stepInput) + { + if (stepInput.IsLastIteration) + { + collisionEventsWriter.BeginForEachIndex(workItemIndex); + triggerEventsWriter.BeginForEachIndex(workItemIndex); + } + + JacobianIterator jacIterator = new JacobianIterator(jacobianReader, workItemIndex); + while (jacIterator.HasJacobiansLeft()) + { + ref JacobianHeader header = ref jacIterator.ReadJacobianHeader(); + + // Get the motion pair + GetMotionVelocities(header.BodyPair, ref motionVelocities, out MotionVelocity velocityA, out MotionVelocity velocityB); + + // Solve the jacobian + header.Solve(ref velocityA, ref velocityB, stepInput, ref collisionEventsWriter, ref triggerEventsWriter); + + // Write back velocity for dynamic bodies + if (header.BodyPair.BodyAIndex < motionVelocities.Length) + { + motionVelocities[header.BodyPair.BodyAIndex] = velocityA; + } + if (header.BodyPair.BodyBIndex < motionVelocities.Length) + { + motionVelocities[header.BodyPair.BodyBIndex] = velocityB; + } + } + + if (stepInput.IsLastIteration) + { + collisionEventsWriter.EndForEachIndex(); + triggerEventsWriter.EndForEachIndex(); + } + } + + #endregion + } +} diff --git a/package/Unity.Physics/Dynamics/Solver/Solver.cs.meta b/package/Unity.Physics/Dynamics/Solver/Solver.cs.meta new file mode 100755 index 000000000..94ccb4af0 --- /dev/null +++ b/package/Unity.Physics/Dynamics/Solver/Solver.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: a9a01ed1e821673408422b7dbafe07db +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Unity.Physics/Dynamics/World.meta b/package/Unity.Physics/Dynamics/World.meta new file mode 100755 index 000000000..5b53f7c05 --- /dev/null +++ b/package/Unity.Physics/Dynamics/World.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: b0addde1f67b55f4bbf082edb61471b8 +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Unity.Physics/Dynamics/World/DynamicsWorld.cs b/package/Unity.Physics/Dynamics/World/DynamicsWorld.cs new file mode 100755 index 000000000..24cfe1dd0 --- /dev/null +++ b/package/Unity.Physics/Dynamics/World/DynamicsWorld.cs @@ -0,0 +1,90 @@ +using System; +using Unity.Collections; + +namespace Unity.Physics +{ + // A collection of motion information used during physics simulation. + public struct DynamicsWorld : IDisposable, ICloneable + { + private NativeArray m_MotionDatas; + private NativeArray m_MotionVelocities; + private int m_NumMotions; // number of motionDatas and motionVelocities currently in use + + private NativeArray m_Joints; + private int m_NumJoints; // number of joints currently in use + + public NativeSlice MotionDatas => new NativeSlice(m_MotionDatas, 0, m_NumMotions); + public NativeSlice MotionVelocities => new NativeSlice(m_MotionVelocities, 0, m_NumMotions); + public NativeSlice Joints => new NativeSlice(m_Joints, 0, m_NumJoints); + + public int NumMotions + { + get => m_NumMotions; + set + { + m_NumMotions = value; + if (m_MotionDatas.Length < m_NumMotions) + { + m_MotionDatas.Dispose(); + m_MotionDatas = new NativeArray(m_NumMotions, Allocator.Persistent, NativeArrayOptions.UninitializedMemory); + } + if (m_MotionVelocities.Length < m_NumMotions) + { + m_MotionVelocities.Dispose(); + m_MotionVelocities = new NativeArray(m_NumMotions, Allocator.Persistent, NativeArrayOptions.UninitializedMemory); + } + } + } + + public int NumJoints + { + get => m_NumJoints; + set + { + m_NumJoints = value; + if (m_Joints.Length < m_NumJoints) + { + m_Joints.Dispose(); + m_Joints = new NativeArray(m_NumJoints, Allocator.Persistent, NativeArrayOptions.UninitializedMemory); + } + } + } + + + // Construct a dynamics world with the given number of uninitialized motions + public DynamicsWorld(int numMotions, int numJoints) + { + m_MotionDatas = new NativeArray(numMotions, Allocator.Persistent, NativeArrayOptions.UninitializedMemory); + m_MotionVelocities = new NativeArray(numMotions, Allocator.Persistent, NativeArrayOptions.UninitializedMemory); + m_NumMotions = numMotions; + + m_Joints = new NativeArray(numMotions, Allocator.Persistent, NativeArrayOptions.UninitializedMemory); + m_NumJoints = numJoints; + } + + // Free internal memory + public void Dispose() + { + m_MotionDatas.Dispose(); + m_MotionVelocities.Dispose(); + m_Joints.Dispose(); + } + + // Clone the world + public object Clone() + { + DynamicsWorld clone = new DynamicsWorld + { + m_MotionDatas = new NativeArray(m_MotionDatas.Length, Allocator.Persistent, NativeArrayOptions.UninitializedMemory), + m_MotionVelocities = new NativeArray(m_MotionVelocities.Length, Allocator.Persistent, NativeArrayOptions.UninitializedMemory), + m_NumMotions = m_NumMotions, + m_Joints = new NativeArray(m_Joints.Length, Allocator.Persistent, NativeArrayOptions.UninitializedMemory), + m_NumJoints = m_NumJoints + }; + clone.m_MotionDatas.CopyFrom(m_MotionDatas); + clone.m_MotionVelocities.CopyFrom(m_MotionVelocities); + clone.m_Joints.CopyFrom(m_Joints); + return clone; + } + } +} diff --git a/package/Unity.Physics/Dynamics/World/DynamicsWorld.cs.meta b/package/Unity.Physics/Dynamics/World/DynamicsWorld.cs.meta new file mode 100755 index 000000000..abbd21f41 --- /dev/null +++ b/package/Unity.Physics/Dynamics/World/DynamicsWorld.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 32d62e05bbda8c349a4ce193cf204bf9 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Unity.Physics/Dynamics/World/PhysicsWorld.cs b/package/Unity.Physics/Dynamics/World/PhysicsWorld.cs new file mode 100755 index 000000000..49054805e --- /dev/null +++ b/package/Unity.Physics/Dynamics/World/PhysicsWorld.cs @@ -0,0 +1,106 @@ +using System; +using Unity.Collections; +using Unity.Mathematics; + +namespace Unity.Physics +{ + // A collection of rigid bodies and joints. + public struct PhysicsWorld : ICollidable, IDisposable, ICloneable + { + public CollisionWorld CollisionWorld; // stores rigid bodies and broadphase + public DynamicsWorld DynamicsWorld; // stores motions and joints + + public int NumBodies => CollisionWorld.NumBodies; + public int NumDynamicBodies => DynamicsWorld.NumMotions; + public int NumStaticBodies => CollisionWorld.NumBodies - DynamicsWorld.NumMotions; + public int NumJoints => DynamicsWorld.NumJoints; + + public NativeSlice Bodies => CollisionWorld.Bodies; + public NativeSlice StaticBodies => new NativeSlice(CollisionWorld.Bodies, DynamicsWorld.NumMotions, CollisionWorld.NumBodies - DynamicsWorld.NumMotions); + public NativeSlice DynamicBodies => new NativeSlice(CollisionWorld.Bodies, 0, DynamicsWorld.NumMotions); + public NativeSlice MotionDatas => DynamicsWorld.MotionDatas; + public NativeSlice MotionVelocities => DynamicsWorld.MotionVelocities; + public NativeSlice Joints => DynamicsWorld.Joints; + + public float CollisionTolerance => 0.1f; // todo - make this configurable? + + // Construct a world with the given number of uninitialized bodies and joints + public PhysicsWorld(int numStaticBodies, int numDynamicBodies, int numJoints) + { + CollisionWorld = new CollisionWorld(numDynamicBodies + numStaticBodies); + DynamicsWorld = new DynamicsWorld(numDynamicBodies, numJoints); + } + + // Reset the number of bodies and joints in the world + public void Reset(int numStaticBodies, int numDynamicBodies, int numJoints) + { + CollisionWorld.NumBodies = numDynamicBodies + numStaticBodies; + DynamicsWorld.NumMotions = numDynamicBodies; + DynamicsWorld.NumJoints = numJoints; + } + + // Free internal memory + public void Dispose() + { + CollisionWorld.Dispose(); + DynamicsWorld.Dispose(); + } + + // Clone the world + public object Clone() => new PhysicsWorld + { + CollisionWorld = (CollisionWorld)CollisionWorld.Clone(), + DynamicsWorld = (DynamicsWorld)DynamicsWorld.Clone() + }; + + #region ICollidable implementation + + public Aabb CalculateAabb() + { + return CollisionWorld.CalculateAabb(); + } + + public Aabb CalculateAabb(RigidTransform transform) + { + return CollisionWorld.CalculateAabb(transform); + } + + // Cast ray + public bool CastRay(RaycastInput input) => QueryWrappers.RayCast(ref this, input); + public bool CastRay(RaycastInput input, out RaycastHit closestHit) => QueryWrappers.RayCast(ref this, input, out closestHit); + public bool CastRay(RaycastInput input, ref NativeList allHits) => QueryWrappers.RayCast(ref this, input, ref allHits); + public bool CastRay(RaycastInput input, ref T collector) where T : struct, ICollector + { + return CollisionWorld.CastRay(input, ref collector); + } + + // Cast collider + public bool CastCollider(ColliderCastInput input) => QueryWrappers.ColliderCast(ref this, input); + public bool CastCollider(ColliderCastInput input, out ColliderCastHit closestHit) => QueryWrappers.ColliderCast(ref this, input, out closestHit); + public bool CastCollider(ColliderCastInput input, ref NativeList allHits) => QueryWrappers.ColliderCast(ref this, input, ref allHits); + public bool CastCollider(ColliderCastInput input, ref T collector) where T : struct, ICollector + { + return CollisionWorld.CastCollider(input, ref collector); + } + + // Point distance + public bool CalculateDistance(PointDistanceInput input) => QueryWrappers.CalculateDistance(ref this, input); + public bool CalculateDistance(PointDistanceInput input, out DistanceHit closestHit) => QueryWrappers.CalculateDistance(ref this, input, out closestHit); + public bool CalculateDistance(PointDistanceInput input, ref NativeList allHits) => QueryWrappers.CalculateDistance(ref this, input, ref allHits); + public bool CalculateDistance(PointDistanceInput input, ref T collector) where T : struct, ICollector + { + return CollisionWorld.CalculateDistance(input, ref collector); + } + + // Collider distance + public bool CalculateDistance(ColliderDistanceInput input) => QueryWrappers.CalculateDistance(ref this, input); + public bool CalculateDistance(ColliderDistanceInput input, out DistanceHit closestHit) => QueryWrappers.CalculateDistance(ref this, input, out closestHit); + public bool CalculateDistance(ColliderDistanceInput input, ref NativeList allHits) => QueryWrappers.CalculateDistance(ref this, input, ref allHits); + public bool CalculateDistance(ColliderDistanceInput input, ref T collector) where T : struct, ICollector + { + return CollisionWorld.CalculateDistance(input, ref collector); + } + + #endregion + } +} diff --git a/package/Unity.Physics/Dynamics/World/PhysicsWorld.cs.meta b/package/Unity.Physics/Dynamics/World/PhysicsWorld.cs.meta new file mode 100755 index 000000000..d49752799 --- /dev/null +++ b/package/Unity.Physics/Dynamics/World/PhysicsWorld.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 13a1abaf4e91aa9499d8d47c6ef6d036 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Unity.Physics/ECS.meta b/package/Unity.Physics/ECS.meta new file mode 100755 index 000000000..8733df2c3 --- /dev/null +++ b/package/Unity.Physics/ECS.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: e8a3010b2dc8ed1419e46a540a0f5885 +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Unity.Physics/ECS/Components.meta b/package/Unity.Physics/ECS/Components.meta new file mode 100755 index 000000000..7f78a9858 --- /dev/null +++ b/package/Unity.Physics/ECS/Components.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: 2253b9138ed1902489400cde00ac0ddc +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Unity.Physics/ECS/Components/PhysicsComponents.cs b/package/Unity.Physics/ECS/Components/PhysicsComponents.cs new file mode 100755 index 000000000..dabe4cb1d --- /dev/null +++ b/package/Unity.Physics/ECS/Components/PhysicsComponents.cs @@ -0,0 +1,115 @@ +using Unity.Entities; +using Unity.Mathematics; + +namespace Unity.Physics +{ + // The collision geometry of a rigid body. + // This causes the entity to appear in the physics world. + public struct PhysicsCollider : IComponentData + { + public BlobAssetReference Value; // null is allowed + + public unsafe bool IsValid => Value.GetUnsafePtr() != null; + public unsafe Collider* ColliderPtr => (Collider*)Value.GetUnsafePtr(); + public unsafe MassProperties MassProperties => Value.GetUnsafePtr() != null ? Value.Value.MassProperties : MassProperties.UnitSphere; + } + + // The mass properties of a rigid body. + // If not present, the rigid body has infinite mass and inertia. + public struct PhysicsMass : IComponentData + { + public RigidTransform Transform; // center of mass and orientation of principal axes + public float InverseMass; // zero is allowed, for infinite mass + public float3 InverseInertia; // zero is allowed, for infinite inertia + public float AngularExpansionFactor; // see MassProperties.AngularExpansionFactor + + public float3 CenterOfMass => Transform.pos; + public quaternion InertiaOrientation => Transform.rot; + + public static PhysicsMass CreateDynamic(MassProperties massProperties, float mass) + { + if (mass <= 0) + throw new System.ArgumentOutOfRangeException(); + + return new PhysicsMass + { + Transform = massProperties.MassDistribution.Transform, + InverseMass = math.rcp(mass), + InverseInertia = math.rcp(massProperties.MassDistribution.InertiaTensor * mass), + AngularExpansionFactor = massProperties.AngularExpansionFactor + }; + } + + public static PhysicsMass CreateKinematic(MassProperties massProperties) + { + return new PhysicsMass + { + Transform = massProperties.MassDistribution.Transform, + InverseMass = 0f, + InverseInertia = float3.zero, + AngularExpansionFactor = massProperties.AngularExpansionFactor + }; + } + } + + // The velocity of a rigid body. + // If not present, the rigid body is static. + public struct PhysicsVelocity : IComponentData + { + public float3 Linear; // in world space + public float3 Angular; // in inertia space, around the rigid body's center of mass // TODO: make this world space + } + + // Optional damping applied to the rigid body velocities during each simulation step. + // This scales the velocities using: math.clamp(1 - damping * Timestep, 0, 1) + public struct PhysicsDamping : IComponentData + { + public float Linear; // damping applied to the linear velocity + public float Angular; // damping applied to the angular velocity + } + + // Optional gravity factor applied to a rigid body during each simulation step. + // This scales the gravity vector supplied to the simulation step. + public struct PhysicsGravityFactor : IComponentData + { + public float Value; + } + + // Optional custom data attached to a rigid body. + // This will be copied to any contacts and Jacobians involving this rigid body, + // providing additional context to any user logic operating on those structures. + public struct PhysicsCustomData : IComponentData + { + public byte Value; + } + + // A set of constraints on the relative motion of a pair of rigid bodies. + public struct PhysicsJoint : IComponentData + { + public BlobAssetReference JointData; + public Entity EntityA; + public Entity EntityB; + public int EnableCollision; // If non-zero, the constrained entities can collide with each other + } + + // Parameters describing how to step the physics world. + // If none is present in the scene, default values will be used. + public struct PhysicsStep : IComponentData + { + public SimulationType SimulationType; + public float3 Gravity; + public int SolverIterationCount; + public int ThreadCountHint; + + public static readonly PhysicsStep Default = new PhysicsStep + { + SimulationType = SimulationType.UnityPhysics, + Gravity = -9.81f * math.up(), + SolverIterationCount = 4, + // Unity DOTS framework doesn't expose number worker threads in current version, + // For this reason we have to make a guess. + // For optimal physics performance set ThreadCountHint to number of physical CPU cores on your target device. + ThreadCountHint = 4 + }; + } +} diff --git a/package/Unity.Physics/ECS/Components/PhysicsComponents.cs.meta b/package/Unity.Physics/ECS/Components/PhysicsComponents.cs.meta new file mode 100755 index 000000000..acd3928fc --- /dev/null +++ b/package/Unity.Physics/ECS/Components/PhysicsComponents.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 67edda1429582534b86e23ec5d8a45e1 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Unity.Physics/ECS/Systems.meta b/package/Unity.Physics/ECS/Systems.meta new file mode 100755 index 000000000..5e6d0d3e2 --- /dev/null +++ b/package/Unity.Physics/ECS/Systems.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: d7b6e50205875cf4ebcd255989782277 +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Unity.Physics/ECS/Systems/BuildPhysicsWorld.cs b/package/Unity.Physics/ECS/Systems/BuildPhysicsWorld.cs new file mode 100755 index 000000000..bb2345970 --- /dev/null +++ b/package/Unity.Physics/ECS/Systems/BuildPhysicsWorld.cs @@ -0,0 +1,522 @@ +using Unity.Burst; +using Unity.Collections; +using Unity.Collections.LowLevel.Unsafe; +using Unity.Entities; +using Unity.Jobs; +using Unity.Mathematics; +using Unity.Transforms; +using UnityEngine.Assertions; + +namespace Unity.Physics.Systems +{ + // A system which builds the physics world based on the entity world. + // The world will contain a rigid body for every entity which has a rigid body component, + // and a joint for every entity which has a joint component. + public class BuildPhysicsWorld : JobComponentSystem + { + public PhysicsWorld PhysicsWorld = new PhysicsWorld(0, 0, 0); + public JobHandle FinalJobHandle { get; private set; } + + // Entity group queries + public ComponentGroup DynamicEntityGroup { get; private set; } + public ComponentGroup StaticEntityGroup { get; private set; } + public ComponentGroup JointEntityGroup { get; private set; } + + protected override void OnCreateManager() + { + base.OnCreateManager(); + + DynamicEntityGroup = GetComponentGroup(new EntityArchetypeQuery + { + All = new ComponentType[] + { + typeof(PhysicsCollider), + typeof(PhysicsVelocity), + typeof(Translation), + typeof(Rotation) + } + }); + + StaticEntityGroup = GetComponentGroup(new EntityArchetypeQuery + { + All = new ComponentType[] + { + typeof(PhysicsCollider), + typeof(Translation), + typeof(Rotation) + }, + None = new ComponentType[] + { + typeof(PhysicsVelocity) + }, + }); + + JointEntityGroup = GetComponentGroup(new EntityArchetypeQuery + { + All = new ComponentType[] + { + typeof(PhysicsJoint) + } + }); + } + + protected override void OnDestroyManager() + { + PhysicsWorld.Dispose(); + base.OnDestroyManager(); + } + + protected override JobHandle OnUpdate(JobHandle inputDeps) + { + // Extract types used by initialize jobs + var entityType = GetArchetypeChunkEntityType(); + var positionType = GetArchetypeChunkComponentType(true); + var rotationType = GetArchetypeChunkComponentType(true); + var physicsColliderType = GetArchetypeChunkComponentType(true); + var physicsVelocityType = GetArchetypeChunkComponentType(true); + var physicsMassType = GetArchetypeChunkComponentType(true); + var physicsDampingType = GetArchetypeChunkComponentType(true); + var physicsGravityFactorType = GetArchetypeChunkComponentType(true); + var physicsCustomDataType = GetArchetypeChunkComponentType(true); + var physicsJointType = GetArchetypeChunkComponentType(true); + + int numDynamicBodies = DynamicEntityGroup.CalculateLength(); + int numStaticBodies = StaticEntityGroup.CalculateLength(); + int numJoints = JointEntityGroup.CalculateLength(); + + // Check for static body changes before the reset() + bool haveStaticBodiesChanged = false; + { + // For now, do this before the reset() - otherwise, we need the BuildRigidBodies jobs to finish + if (numStaticBodies != (PhysicsWorld.StaticBodies.Length - 1)) //-1 for fake static body we add + { + // Quick test if number of bodies changed + haveStaticBodiesChanged = true; + } + else + { + // Make a job to test for changes + + int numChunks; // There has to be a better way of doing this... + { + var chunks = StaticEntityGroup.CreateArchetypeChunkArray(Allocator.TempJob); + numChunks = chunks.Length; + chunks.Dispose(); + } + + var chunksHaveChanges = new NativeArray(numChunks, Allocator.TempJob); + var checkStaticChanges = new Jobs.CheckStaticBodyChangesJob + { + PositionType = positionType, + RotationType = rotationType, + PhysicsColliderType = physicsColliderType, + StaticRigidBodies = PhysicsWorld.StaticBodies, + ChunkHasChangesOutput = chunksHaveChanges + }; + + checkStaticChanges.Schedule(StaticEntityGroup, inputDeps).Complete(); + for (int i = 0; i < numChunks; i++) + { + haveStaticBodiesChanged |= chunksHaveChanges[i] != 0; + } + chunksHaveChanges.Dispose(); + } + } + + // Resize the world's native arrays + PhysicsWorld.Reset( + numStaticBodies: numStaticBodies + 1, // +1 for the default static body + numDynamicBodies: numDynamicBodies, + numJoints: numJoints); + + var jobHandles = new NativeList(4, Allocator.Temp); + + // Create the default static body at the end of the body list + // TODO: could skip this if no joints present + jobHandles.Add(new Jobs.CreateDefaultStaticRigidBody + { + NativeBodies = PhysicsWorld.Bodies, + BodyIndex = PhysicsWorld.Bodies.Length - 1 + }.Schedule(inputDeps)); + + // Dynamic bodies. Create these separately from static bodies to maintain a 1:1 mapping + // between dynamic bodies and their motions. + if (numDynamicBodies > 0) + { + jobHandles.Add(new Jobs.CreateRigidBodies + { + EntityType = entityType, + PositionType = positionType, + RotationType = rotationType, + PhysicsColliderType = physicsColliderType, + PhysicsCustomDataType = physicsCustomDataType, + + FirstBodyIndex = 0, + RigidBodies = PhysicsWorld.Bodies + }.Schedule(DynamicEntityGroup, inputDeps)); + + jobHandles.Add(new Jobs.CreateMotions + { + PositionType = positionType, + RotationType = rotationType, + PhysicsVelocityType = physicsVelocityType, + PhysicsMassType = physicsMassType, + PhysicsDampingType = physicsDampingType, + PhysicsGravityFactorType = physicsGravityFactorType, + + MotionDatas = PhysicsWorld.MotionDatas, + MotionVelocities = PhysicsWorld.MotionVelocities + }.Schedule(DynamicEntityGroup, inputDeps)); + } + + // Now, schedule creation of static bodies, with FirstBodyIndex pointing after the dynamic bodies + if (numStaticBodies > 0) + { + jobHandles.Add(new Jobs.CreateRigidBodies + { + EntityType = entityType, + PositionType = positionType, + RotationType = rotationType, + PhysicsColliderType = physicsColliderType, + PhysicsCustomDataType = physicsCustomDataType, + + FirstBodyIndex = numDynamicBodies, + RigidBodies = PhysicsWorld.Bodies + }.Schedule(StaticEntityGroup, inputDeps)); + } + + var handle = JobHandle.CombineDependencies(jobHandles); + jobHandles.Clear(); + + // Build joints + if (numJoints > 0) + { + jobHandles.Add(new Jobs.CreateJoints + { + JointComponentType = physicsJointType, + EntityType = entityType, + RigidBodies = PhysicsWorld.Bodies, + Joints = PhysicsWorld.Joints, + DefaultStaticBodyIndex = PhysicsWorld.Bodies.Length - 1 + }.Schedule(JointEntityGroup, handle)); + } + + // Build the broadphase + // TODO: could optimize this by gathering the AABBs and filters at the same time as building the bodies above + + float timeStep = UnityEngine.Time.fixedDeltaTime; + + PhysicsStep stepComponent = PhysicsStep.Default; + if (HasSingleton()) + { + stepComponent = GetSingleton(); + } + + jobHandles.Add(PhysicsWorld.CollisionWorld.Broadphase.ScheduleBuildJobs(ref PhysicsWorld, timeStep, stepComponent.ThreadCountHint, haveStaticBodiesChanged, handle)); + + FinalJobHandle = JobHandle.CombineDependencies(jobHandles); + jobHandles.Dispose(); + + return JobHandle.CombineDependencies(FinalJobHandle, inputDeps); + } + + #region Jobs + + private static class Jobs + { + [BurstCompile] + internal struct CheckStaticBodyChangesJob : IJobChunk + { + [ReadOnly] public ArchetypeChunkComponentType PositionType; + [ReadOnly] public ArchetypeChunkComponentType RotationType; + [ReadOnly] public ArchetypeChunkComponentType PhysicsColliderType; + [ReadOnly] public NativeSlice StaticRigidBodies; + + [NativeDisableContainerSafetyRestriction] public NativeSlice ChunkHasChangesOutput; + + public unsafe void Execute(ArchetypeChunk chunk, int chunkIndex, int firstEntityIndex) + { + var chunkColliders = chunk.GetNativeArray(PhysicsColliderType); + var chunkPositions = chunk.GetNativeArray(PositionType); + var chunkRotations = chunk.GetNativeArray(RotationType); + + ChunkHasChangesOutput[chunkIndex] = 0; + + // Check the contents of collider/positions/rotations and determine if the value changed since we + // last built the rigid bodies array + int count = chunk.Count; + for (int i = 0, rigidBodyIndex = firstEntityIndex; i < count; i++, rigidBodyIndex++) + { + // ToDo: implement a check to verify if collider was modified in place. + bool colliderDifferent = StaticRigidBodies[rigidBodyIndex].Collider != chunkColliders[i].ColliderPtr; + bool positionDifferent = !StaticRigidBodies[rigidBodyIndex].WorldFromBody.pos.Equals(chunkPositions[i].Value); + bool rotationDifferent = !StaticRigidBodies[rigidBodyIndex].WorldFromBody.rot.Equals(chunkRotations[i].Value); + + if (positionDifferent || rotationDifferent || colliderDifferent) + { + ChunkHasChangesOutput[chunkIndex] = 1; + return; + } + } + } + } + + [BurstCompile] + internal struct CreateDefaultStaticRigidBody : IJob + { + [NativeDisableContainerSafetyRestriction] + public NativeSlice NativeBodies; + public int BodyIndex; + + public void Execute() + { + NativeBodies[BodyIndex] = new RigidBody + { + WorldFromBody = new RigidTransform(quaternion.identity, float3.zero), + Collider = null, + Entity = Entity.Null, + CustomData = 0 + }; + } + } + + [BurstCompile] + internal struct CreateRigidBodies : IJobChunk + { + [ReadOnly] public ArchetypeChunkEntityType EntityType; + [ReadOnly] public ArchetypeChunkComponentType PositionType; + [ReadOnly] public ArchetypeChunkComponentType RotationType; + [ReadOnly] public ArchetypeChunkComponentType PhysicsColliderType; + [ReadOnly] public ArchetypeChunkComponentType PhysicsCustomDataType; + [ReadOnly] public int FirstBodyIndex; + + [NativeDisableContainerSafetyRestriction] public NativeSlice RigidBodies; + + public unsafe void Execute(ArchetypeChunk chunk, int chunkIndex, int firstEntityIndex) + { + var chunkColliders = chunk.GetNativeArray(PhysicsColliderType); + var chunkPositions = chunk.GetNativeArray(PositionType); + var chunkRotations = chunk.GetNativeArray(RotationType); + var chunkEntities = chunk.GetNativeArray(EntityType); + + int instanceCount = chunk.Count; + int rbIndex = FirstBodyIndex + firstEntityIndex; + + if (!chunk.Has(PhysicsCustomDataType)) + { + for (int i = 0; i < instanceCount; i++, rbIndex++) + { + RigidBodies[rbIndex] = new RigidBody + { + WorldFromBody = new RigidTransform(chunkRotations[i].Value, chunkPositions[i].Value), + Collider = chunkColliders[i].ColliderPtr, + Entity = chunkEntities[i] + }; + } + } + else + { + var chunkCustomDatas = chunk.GetNativeArray(PhysicsCustomDataType); + for (int i = 0; i < instanceCount; i++, rbIndex++) + { + RigidBodies[rbIndex] = new RigidBody + { + WorldFromBody = new RigidTransform(chunkRotations[i].Value, chunkPositions[i].Value), + Collider = chunkColliders[i].ColliderPtr, + Entity = chunkEntities[i], + CustomData = chunkCustomDatas[i].Value + }; + } + } + } + } + + // Create a native motion data + private static MotionData CreateMotionData( + Translation position, Rotation orientation, PhysicsMass massComponent, + float linearDamping, float angularDamping, float gravityFactor = 1.0f) + { + return new MotionData + { + WorldFromMotion = new RigidTransform( + math.mul(orientation.Value, massComponent.InertiaOrientation), + math.rotate(orientation.Value, massComponent.CenterOfMass) + position.Value + ), + BodyFromMotion = massComponent.Transform, + LinearDamping = linearDamping, + AngularDamping = angularDamping, + GravityFactor = gravityFactor + }; + } + + [BurstCompile] + internal struct CreateMotions : IJobChunk + { + [ReadOnly] public ArchetypeChunkComponentType PositionType; + [ReadOnly] public ArchetypeChunkComponentType RotationType; + [ReadOnly] public ArchetypeChunkComponentType PhysicsVelocityType; + [ReadOnly] public ArchetypeChunkComponentType PhysicsMassType; + [ReadOnly] public ArchetypeChunkComponentType PhysicsDampingType; + [ReadOnly] public ArchetypeChunkComponentType PhysicsGravityFactorType; + + [NativeDisableContainerSafetyRestriction] public NativeSlice MotionDatas; + [NativeDisableContainerSafetyRestriction] public NativeSlice MotionVelocities; + + public unsafe void Execute(ArchetypeChunk chunk, int chunkIndex, int firstEntityIndex) + { + var chunkPositions = chunk.GetNativeArray(PositionType); + var chunkRotations = chunk.GetNativeArray(RotationType); + var chunkVelocities = chunk.GetNativeArray(PhysicsVelocityType); + var chunkMasses = chunk.GetNativeArray(PhysicsMassType); + + int motionStart = firstEntityIndex; + int instanceCount = chunk.Count; + + // Create motion velocities + for (int i = 0, motionIndex = motionStart; i < instanceCount; i++, motionIndex++) + { + MotionVelocities[motionIndex] = new MotionVelocity + { + LinearVelocity = chunkVelocities[i].Linear, // world space + AngularVelocity = chunkVelocities[i].Angular, // inertia space + InverseInertiaAndMass = new float4(chunkMasses[i].InverseInertia, chunkMasses[i].InverseMass), + AngularExpansionFactor = chunkMasses[i].AngularExpansionFactor + }; + } + + // Create motion datas + const float defaultLinearDamping = 0.0f; // TODO: Use non-zero defaults? + const float defaultAngularDamping = 0.0f; + if (chunk.Has(PhysicsGravityFactorType)) + { + // With gravity factor ... + var chunkGravityFactors = chunk.GetNativeArray(PhysicsGravityFactorType); + if (chunk.Has(PhysicsDampingType)) + { + // ... with damping + var chunkDampings = chunk.GetNativeArray(PhysicsDampingType); + for (int i = 0, motionIndex = motionStart; i < instanceCount; i++, motionIndex++) + { + MotionDatas[motionIndex] = CreateMotionData( + chunkPositions[i], chunkRotations[i], chunkMasses[i], + chunkDampings[i].Linear, chunkDampings[i].Angular, chunkGravityFactors[i].Value); + } + } + else + { + // ... without damping + for (int i = 0, motionIndex = motionStart; i < instanceCount; i++, motionIndex++) + { + MotionDatas[motionIndex] = CreateMotionData( + chunkPositions[i], chunkRotations[i], chunkMasses[i], + defaultLinearDamping, defaultAngularDamping, chunkGravityFactors[i].Value); + } + } + } + else + { + // Without gravity factor ... + if (chunk.Has(PhysicsDampingType)) + { + // ... with damping + var chunkDampings = chunk.GetNativeArray(PhysicsDampingType); + for (int i = 0, motionIndex = motionStart; i < instanceCount; i++, motionIndex++) + { + MotionDatas[motionIndex] = CreateMotionData( + chunkPositions[i], chunkRotations[i], chunkMasses[i], + chunkDampings[i].Linear, chunkDampings[i].Angular); + } + } + else + { + // ... without damping + for (int i = 0, motionIndex = motionStart; i < instanceCount; i++, motionIndex++) + { + MotionDatas[motionIndex] = CreateMotionData( + chunkPositions[i], chunkRotations[i], chunkMasses[i], + defaultLinearDamping, defaultAngularDamping); + } + } + } + } + } + + [BurstCompile] + internal struct CreateJoints : IJobChunk + { + [ReadOnly] public ArchetypeChunkComponentType JointComponentType; + [ReadOnly] public ArchetypeChunkEntityType EntityType; + [ReadOnly] public NativeSlice RigidBodies; + + [NativeDisableContainerSafetyRestriction] + public NativeSlice Joints; + + public int DefaultStaticBodyIndex; + + public unsafe void Execute(ArchetypeChunk chunk, int chunkIndex, int firstEntityIndex) + { + var chunkJoint = chunk.GetNativeArray(JointComponentType); + var chunkEntities = chunk.GetNativeArray(EntityType); + + int instanceCount = chunk.Count; + for (int i = 0; i < instanceCount; i++) + { + PhysicsJoint joint = chunkJoint[i]; + Assert.IsTrue(joint.EntityA != joint.EntityB); + + // TODO find a reasonable way to look up the constraint body indices + // - stash body index in a component on the entity? But we don't have random access to Entity data in a job + // - make a map from entity to rigid body index? Sounds bad and I don't think there is any NativeArray-based map data structure yet + + // If one of the entities is null, use the default static entity + var pair = new BodyIndexPair + { + BodyAIndex = joint.EntityA == Entity.Null ? DefaultStaticBodyIndex : -1, + BodyBIndex = joint.EntityB == Entity.Null ? DefaultStaticBodyIndex : -1 + }; + + // Find the body indices + for (int bodyIndex = 0; bodyIndex < RigidBodies.Length; bodyIndex++) + { + if (joint.EntityA != Entity.Null) + { + if (RigidBodies[bodyIndex].Entity == joint.EntityA) + { + pair.BodyAIndex = bodyIndex; + if (pair.BodyBIndex >= 0) + { + break; + } + } + } + + if (joint.EntityB != Entity.Null) + { + if (RigidBodies[bodyIndex].Entity == joint.EntityB) + { + pair.BodyBIndex = bodyIndex; + if (pair.BodyAIndex >= 0) + { + break; + } + } + } + } + + Assert.IsTrue(pair.BodyAIndex != -1 && pair.BodyBIndex != -1); + + Joints[firstEntityIndex + i] = new Joint + { + JointData = (JointData*)joint.JointData.GetUnsafePtr(), + BodyPair = pair, + Entity = chunkEntities[i], + EnableCollision = joint.EnableCollision, + }; + } + } + } + } + + #endregion + } +} diff --git a/package/Unity.Physics/ECS/Systems/BuildPhysicsWorld.cs.meta b/package/Unity.Physics/ECS/Systems/BuildPhysicsWorld.cs.meta new file mode 100755 index 000000000..7abf138fa --- /dev/null +++ b/package/Unity.Physics/ECS/Systems/BuildPhysicsWorld.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: d6b780b01ab1d8e4284cd4a6864aa69d +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Unity.Physics/ECS/Systems/EndFramePhysicsSystem.cs b/package/Unity.Physics/ECS/Systems/EndFramePhysicsSystem.cs new file mode 100755 index 000000000..c56d8fb71 --- /dev/null +++ b/package/Unity.Physics/ECS/Systems/EndFramePhysicsSystem.cs @@ -0,0 +1,43 @@ +using Unity.Collections; +using Unity.Entities; +using Unity.Jobs; + +namespace Unity.Physics.Systems +{ + // A system which waits for any remaining physics jobs to finish + [UpdateAfter(typeof(BuildPhysicsWorld)), UpdateAfter(typeof(StepPhysicsWorld)), + UpdateAfter(typeof(ExportPhysicsWorld))] + public class EndFramePhysicsSystem : ComponentSystem + { + public NativeList HandlesToWaitFor; + + BuildPhysicsWorld m_BuildPhysicsWorld; + StepPhysicsWorld m_StepPhysicsWorld; + ExportPhysicsWorld m_ExportPhysicsWorld; + + protected override void OnCreateManager() + { + HandlesToWaitFor = new NativeList(16, Allocator.Persistent); + + m_BuildPhysicsWorld = World.GetOrCreateManager(); + m_StepPhysicsWorld = World.GetOrCreateManager(); + m_ExportPhysicsWorld = World.GetOrCreateManager(); + } + + protected override void OnDestroyManager() + { + HandlesToWaitFor.Dispose(); + } + + protected override void OnUpdate() + { + HandlesToWaitFor.Add(m_BuildPhysicsWorld.FinalJobHandle); + HandlesToWaitFor.Add(m_StepPhysicsWorld.FinalJobHandle); + HandlesToWaitFor.Add(m_ExportPhysicsWorld.FinalJobHandle); + + JobHandle handle = JobHandle.CombineDependencies(HandlesToWaitFor); + HandlesToWaitFor.Clear(); + handle.Complete(); + } + } +} diff --git a/package/Unity.Physics/ECS/Systems/EndFramePhysicsSystem.cs.meta b/package/Unity.Physics/ECS/Systems/EndFramePhysicsSystem.cs.meta new file mode 100755 index 000000000..9efb80473 --- /dev/null +++ b/package/Unity.Physics/ECS/Systems/EndFramePhysicsSystem.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 369a2c039808333439a2d533242edc61 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Unity.Physics/ECS/Systems/ExportPhysicsWorld.cs b/package/Unity.Physics/ECS/Systems/ExportPhysicsWorld.cs new file mode 100755 index 000000000..70e0eedb9 --- /dev/null +++ b/package/Unity.Physics/ECS/Systems/ExportPhysicsWorld.cs @@ -0,0 +1,84 @@ +using Unity.Burst; +using Unity.Collections; +using Unity.Entities; +using Unity.Jobs; +using Unity.Mathematics; +using Unity.Transforms; + +namespace Unity.Physics.Systems +{ + // A system which copies transforms and velocities from the physics world back to the original entity components. + // CK: We make sure we update before CopyTransformToGameObjectSystem so that hybrid GameObjects can work with this OK, even if that path is slow. + [UpdateAfter(typeof(StepPhysicsWorld)), UpdateBefore(typeof(EndFramePhysicsSystem)), UpdateBefore(typeof(TransformSystemGroup))] + public class ExportPhysicsWorld : JobComponentSystem + { + public JobHandle FinalJobHandle { get; private set; } + + BuildPhysicsWorld m_BuildPhysicsWorldSystem; + StepPhysicsWorld m_StepPhysicsWorldSystem; + + protected override void OnCreateManager() + { + m_BuildPhysicsWorldSystem = World.GetOrCreateManager(); + m_StepPhysicsWorldSystem = World.GetOrCreateManager(); + } + + protected override JobHandle OnUpdate(JobHandle inputDeps) + { + JobHandle handle = JobHandle.CombineDependencies(inputDeps, m_StepPhysicsWorldSystem.FinalSimulationJobHandle); + + ref PhysicsWorld world = ref m_BuildPhysicsWorldSystem.PhysicsWorld; + + var positionType = GetArchetypeChunkComponentType(); + var rotationType = GetArchetypeChunkComponentType(); + var velocityType = GetArchetypeChunkComponentType(); + + handle = new ExportDynamicBodiesJob + { + DynamicBodies = world.DynamicBodies, + MotionVelocities = world.MotionVelocities, + MotionDatas = world.MotionDatas, + + PositionType = positionType, + RotationType = rotationType, + VelocityType = velocityType + }.Schedule(m_BuildPhysicsWorldSystem.DynamicEntityGroup, handle); + + FinalJobHandle = handle; + return handle; + } + + [BurstCompile] + internal struct ExportDynamicBodiesJob : IJobChunk + { + [ReadOnly] public NativeSlice DynamicBodies; + [ReadOnly] public NativeSlice MotionVelocities; + [ReadOnly] public NativeSlice MotionDatas; + + public ArchetypeChunkComponentType PositionType; + public ArchetypeChunkComponentType RotationType; + public ArchetypeChunkComponentType VelocityType; + + public void Execute(ArchetypeChunk chunk, int chunkIndex, int entityStartIndex) + { + var chunkPositions = chunk.GetNativeArray(PositionType); + var chunkRotations = chunk.GetNativeArray(RotationType); + var chunkVelocities = chunk.GetNativeArray(VelocityType); + + int numItems = chunk.Count; + for(int i = 0, motionIndex = entityStartIndex; i < numItems; i++, motionIndex++) + { + MotionData md = MotionDatas[motionIndex]; + RigidTransform worldFromBody = math.mul(md.WorldFromMotion, math.inverse(md.BodyFromMotion)); + chunkPositions[i] = new Translation { Value = worldFromBody.pos }; + chunkRotations[i] = new Rotation { Value = worldFromBody.rot }; + chunkVelocities[i] = new PhysicsVelocity + { + Linear = MotionVelocities[motionIndex].LinearVelocity, + Angular = MotionVelocities[motionIndex].AngularVelocity + }; + } + } + } + } +} diff --git a/package/Unity.Physics/ECS/Systems/ExportPhysicsWorld.cs.meta b/package/Unity.Physics/ECS/Systems/ExportPhysicsWorld.cs.meta new file mode 100755 index 000000000..69cd2c4c7 --- /dev/null +++ b/package/Unity.Physics/ECS/Systems/ExportPhysicsWorld.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: fa4059b3a77bf49439090e1f91c00a4a +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Unity.Physics/ECS/Systems/StepPhysicsWorld.cs b/package/Unity.Physics/ECS/Systems/StepPhysicsWorld.cs new file mode 100755 index 000000000..e7f6102ef --- /dev/null +++ b/package/Unity.Physics/ECS/Systems/StepPhysicsWorld.cs @@ -0,0 +1,139 @@ +using System; +using Unity.Entities; +using Unity.Jobs; + +namespace Unity.Physics.Systems +{ + // Simulates the physics world forwards in time + [UpdateAfter(typeof(BuildPhysicsWorld)), UpdateBefore(typeof(ExportPhysicsWorld))] + public class StepPhysicsWorld : JobComponentSystem + { + // The simulation implementation + public ISimulation Simulation { get; private set; } + + // The final job handle produced by this system. + // This includes all simulation jobs as well as array disposal jobs. + public JobHandle FinalJobHandle { get; private set; } + + // The final simulation job handle produced by this system. + // Systems which read the simulation results should depend on this. + public JobHandle FinalSimulationJobHandle { get; private set; } + + // Optional callbacks to execute while scheduling the next simulation step + private SimulationCallbacks m_Callbacks; + + // Simulation factory + public delegate ISimulation SimulationCreator(); + private readonly SimulationCreator[] m_SimulationCreators = new SimulationCreator[Enum.GetValues(typeof(SimulationType)).Length]; + + BuildPhysicsWorld m_BuildPhysicsWorldSystem; + + // Entity group queries + private ComponentGroup m_PhysicsEntityGroup; + + + protected override void OnCreateManager() + { + m_BuildPhysicsWorldSystem = World.GetOrCreateManager(); + + Simulation = new DummySimulation(); + RegisterSimulation(SimulationType.NoPhysics, () => new DummySimulation()); + RegisterSimulation(SimulationType.UnityPhysics, () => new Simulation()); + RegisterSimulation(SimulationType.HavokPhysics, () => throw new NotSupportedException("Havok Physics package not present. Available Summer 2019.")); + + FinalSimulationJobHandle = new JobHandle(); + FinalJobHandle = new JobHandle(); + + m_Callbacks = new SimulationCallbacks(); + + base.OnCreateManager(); + + // Needed to keep ComponentSystem active when no Entity has PhysicsStep component + m_PhysicsEntityGroup = GetComponentGroup(new EntityArchetypeQuery + { + All = new ComponentType[] + { + typeof(PhysicsVelocity) + } + }); + } + + protected override void OnDestroyManager() + { + Simulation.Dispose(); + base.OnDestroyManager(); + } + + // Register a simulation creator + public void RegisterSimulation(SimulationType type, SimulationCreator creator) + { + m_SimulationCreators[(int)type] = creator; + } + + // Enqueue a callback to run during scheduling of the next simulation step + public void EnqueueCallback(SimulationCallbacks.Phase phase, SimulationCallbacks.Callback callback) + { + m_Callbacks.Enqueue(phase, callback); + } + + protected override JobHandle OnUpdate(JobHandle inputDeps) + { + var handle = JobHandle.CombineDependencies(m_BuildPhysicsWorldSystem.FinalJobHandle, inputDeps); + + PhysicsStep stepComponent = PhysicsStep.Default; + if (HasSingleton()) + { + stepComponent = GetSingleton(); + } + + // Swap the simulation implementation if the requested type changed + if (Simulation.Type != stepComponent.SimulationType) + { + Simulation.Dispose(); + Simulation = m_SimulationCreators[(int)stepComponent.SimulationType](); + } + + // Schedule the simulation jobs + var stepInput = new SimulationStepInput + { + World = m_BuildPhysicsWorldSystem.PhysicsWorld, + TimeStep = UnityEngine.Time.fixedDeltaTime, + ThreadCountHint = stepComponent.ThreadCountHint, + Gravity = stepComponent.Gravity, + SynchronizeCollisionWorld = false, + NumSolverIterations = stepComponent.SolverIterationCount, + Callbacks = m_Callbacks + }; + Simulation.ScheduleStepJobs(stepInput, handle, out JobHandle finalSimulationJobHandle, out JobHandle finalJobHandle); + FinalSimulationJobHandle = finalSimulationJobHandle; + FinalJobHandle = finalJobHandle; + + // Clear the callbacks. User must enqueue them again before the next step. + m_Callbacks.Clear(); + + return handle; + } + + // A simulation which does nothing + private class DummySimulation : ISimulation + { + public SimulationType Type => SimulationType.NoPhysics; + + // TODO: Make sure these return dummies that can have foreach() etc safely called on them + public SimulationData.BodyPairs BodyPairs => new SimulationData.BodyPairs(); + public SimulationData.Contacts Contacts => new SimulationData.Contacts(); + public SimulationData.Jacobians Jacobians => new SimulationData.Jacobians(); + public CollisionEvents CollisionEvents => new CollisionEvents(); + public TriggerEvents TriggerEvents => new TriggerEvents(); + + public void Dispose() { } + public void Step(SimulationStepInput input) { } + public void ScheduleStepJobs(SimulationStepInput input, JobHandle inputDeps, + out JobHandle finalSimulationJobHandle, out JobHandle finalJobHandle) + { + finalSimulationJobHandle = new JobHandle(); + finalJobHandle = new JobHandle(); + } + } + } +} diff --git a/package/Unity.Physics/ECS/Systems/StepPhysicsWorld.cs.meta b/package/Unity.Physics/ECS/Systems/StepPhysicsWorld.cs.meta new file mode 100755 index 000000000..b4e13bae7 --- /dev/null +++ b/package/Unity.Physics/ECS/Systems/StepPhysicsWorld.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 8e4907e965e300a4cb878ad3f28eea25 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Unity.Physics/Extensions.meta b/package/Unity.Physics/Extensions.meta new file mode 100755 index 000000000..2405806a7 --- /dev/null +++ b/package/Unity.Physics/Extensions.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: 63fbef8049effb54782de0fa93a42b2d +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Unity.Physics/Extensions/World.meta b/package/Unity.Physics/Extensions/World.meta new file mode 100755 index 000000000..b7ff0106d --- /dev/null +++ b/package/Unity.Physics/Extensions/World.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: e7e857ea0f222db4fa22ef9b9b0a29c7 +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Unity.Physics/Extensions/World/ComponentExtensions.cs b/package/Unity.Physics/Extensions/World/ComponentExtensions.cs new file mode 100755 index 000000000..118935446 --- /dev/null +++ b/package/Unity.Physics/Extensions/World/ComponentExtensions.cs @@ -0,0 +1,325 @@ +using Unity.Entities; +using Unity.Mathematics; +using Unity.Transforms; + +namespace Unity.Physics.Extensions +{ + // Utility functions acting on physics components + public static class ComponentExtensions + { + public static CollisionFilter GetCollisionFilter(this Entity entity) + { + var em = World.Active.GetOrCreateManager(); + + if (em.HasComponent(entity)) + { + var collider = em.GetComponentData(entity); + if (collider.IsValid) + { + return collider.Value.Value.Filter; + } + } + return CollisionFilter.Zero; + } + + public static float GetMass(this Entity entity) + { + var em = World.Active.GetOrCreateManager(); + + if (em.HasComponent(entity)) + { + var mass = em.GetComponentData(entity); + if (mass.InverseMass != 0) + { + return math.rcp(mass.InverseMass); + } + } + return 0; // a.k.a infinite + } + + public static float GetEffectiveMass(this Entity entity, float3 impulse, float3 point) + { + var em = World.Active.GetOrCreateManager(); + + float effMass = 0; + + // TODO: convert over from WorldExtensions + //MotionVelocity mv = world.MotionVelocities[rigidBodyIndex]; + + //float3 pointDir = math.normalizesafe(point - GetCenterOfMass(world, rigidBodyIndex)); + //float3 impulseDir = math.normalizesafe(impulse); + + //float3 jacobian = math.cross(pointDir, impulseDir); + //float invEffMass = math.csum(math.dot(jacobian, jacobian) * mv.InverseInertiaAndMass.xyz); + //effMass = math.select(1.0f / invEffMass, 0.0f, math.abs(invEffMass) < 1e-5); + + return effMass; + } + + // Get the center of mass in world space + public static float3 GetCenterOfMass(this Entity entity) + { + var em = World.Active.GetOrCreateManager(); + + if (em.HasComponent(entity) && + em.HasComponent(entity) && + em.HasComponent(entity)) + { + var massCData = em.GetComponentData(entity); + var posCData = em.GetComponentData(entity); + var rotCData = em.GetComponentData(entity); + + return GetCenterOfMass(massCData, posCData, rotCData); + } + return float3.zero; + } + + // Get the center of mass in world space + public static float3 GetCenterOfMass(PhysicsMass massData, Translation posData, Rotation rotData) + { + return math.rotate(rotData.Value, massData.CenterOfMass) + posData.Value; + } + + public static float3 GetPosition(this Entity entity) + { + var em = World.Active.GetOrCreateManager(); + + if (em.HasComponent(entity)) + { + return em.GetComponentData(entity).Value; + } + return float3.zero; + } + + public static quaternion GetRotation(this Entity entity) + { + var em = World.Active.GetOrCreateManager(); + + if (em.HasComponent(entity)) + { + return em.GetComponentData(entity).Value; + } + return quaternion.identity; + } + + public static void SetVelocities(this Entity entity, float3 linearVelocity, float3 angularVelocity) + { + var em = World.Active.GetOrCreateManager(); + + if (em.HasComponent(entity)) + { + var velocityData = em.GetComponentData(entity); + velocityData.Linear = linearVelocity; + velocityData.Angular = angularVelocity; + + em.SetComponentData(entity, velocityData); + } + } + + public static void GetVelocities(this Entity entity, out float3 linearVelocity, out float3 angularVelocity) + { + var em = World.Active.GetOrCreateManager(); + + if (em.HasComponent(entity)) + { + var velocityData = em.GetComponentData(entity); + linearVelocity = velocityData.Linear; + angularVelocity = velocityData.Angular; + } + else + { + linearVelocity = float3.zero; + angularVelocity = float3.zero; + } + } + + // Get the linear velocity of a rigid body (in world space) + public static float3 GetLinearVelocity(this Entity entity) + { + var em = World.Active.GetOrCreateManager(); + + if (em.HasComponent(entity)) + { + var velocityData = em.GetComponentData(entity); + return velocityData.Linear; + } + return float3.zero; + } + + // Get the linear velocity of a rigid body at a given point (in world space) + public static float3 GetLinearVelocity(this Entity entity, float3 point) + { + var em = World.Active.GetOrCreateManager(); + + if (em.HasComponent(entity) && + em.HasComponent(entity) && + em.HasComponent(entity) && + em.HasComponent(entity)) + { + var velocityData = em.GetComponentData(entity); + var massData = em.GetComponentData(entity); + var posCData = em.GetComponentData(entity); + var rotCData = em.GetComponentData(entity); + + return velocityData.GetLinearVelocity(massData, posCData, rotCData, point); + } + return float3.zero; + } + + // Get the linear velocity of a rigid body at a given point (in world space) + public static float3 GetLinearVelocity(this PhysicsVelocity velocityData, PhysicsMass massData, Translation posData, Rotation rotData, float3 point) + { + var worldFromEntity = new RigidTransform(rotData.Value, posData.Value); + var worldFromMotion = math.mul(worldFromEntity, massData.Transform); + + float3 angularVelocity = math.rotate(worldFromMotion, velocityData.Angular); + float3 linearVelocity = math.cross(angularVelocity, (point - worldFromMotion.pos)); + return velocityData.Linear + linearVelocity; + } + + // Set the linear velocity of a rigid body (in world space) + public static void SetLinearVelocity(this Entity entity, float3 linearVelocity) + { + var em = World.Active.GetOrCreateManager(); + + if (em.HasComponent(entity)) + { + var velocityData = em.GetComponentData(entity); + velocityData.Linear = linearVelocity; + em.SetComponentData(entity, velocityData); + } + } + + // Get the angular velocity of a rigid body around it's center of mass (in world space) + public static float3 GetAngularVelocity(this Entity entity) + { + var em = World.Active.GetOrCreateManager(); + + if (em.HasComponent(entity) && + em.HasComponent(entity) && + em.HasComponent(entity)) + { + var velocityData = em.GetComponentData(entity); + var massData = em.GetComponentData(entity); + var rotCData = em.GetComponentData(entity); + + return velocityData.GetAngularVelocity(massData, rotCData); + } + return float3.zero; + } + + // Get the angular velocity of a rigid body around it's center of mass (in world space) + public static float3 GetAngularVelocity(this PhysicsVelocity velocityData, PhysicsMass massData, Rotation rotData) + { + quaternion inertiaOrientationInWorldSpace = math.mul(rotData.Value, massData.InertiaOrientation); + return math.rotate(inertiaOrientationInWorldSpace, velocityData.Angular); + } + + // Set the angular velocity of a rigid body (in world space) + public static void SetAngularVelocity(this Entity entity, float3 angularVelocity) + { + var em = World.Active.GetOrCreateManager(); + + if (em.HasComponent(entity) && + em.HasComponent(entity) && + em.HasComponent(entity)) + { + var velocityData = em.GetComponentData(entity); + var massData = em.GetComponentData(entity); + var rotCData = em.GetComponentData(entity); + + velocityData.SetAngularVelocity(massData, rotCData, angularVelocity); + + em.SetComponentData(entity, velocityData); + } + } + + // Set the angular velocity of a rigid body (in world space) + public static void SetAngularVelocity(ref this PhysicsVelocity velocityData, PhysicsMass massData, Rotation rotData, float3 angularVelocity) + { + quaternion inertiaOrientationInWorldSpace = math.mul(rotData.Value, massData.InertiaOrientation); + float3 angularVelocityInertiaSpace = math.rotate(math.inverse(inertiaOrientationInWorldSpace), angularVelocity); + + velocityData.Angular = angularVelocityInertiaSpace; + } + + public static void ApplyImpulse(this Entity entity, float3 impulse, float3 point) + { + var em = World.Active.GetOrCreateManager(); + + if (em.HasComponent(entity) && + em.HasComponent(entity) && + em.HasComponent(entity) && + em.HasComponent(entity)) + { + var velocityData = em.GetComponentData(entity); + var massData = em.GetComponentData(entity); + var posCData = em.GetComponentData(entity); + var rotCData = em.GetComponentData(entity); + + velocityData.ApplyImpulse(massData, posCData, rotCData, impulse, point); + + em.SetComponentData(entity, velocityData); + } + } + + public static void ApplyImpulse(ref this PhysicsVelocity pv, PhysicsMass pm, Translation t, Rotation r, float3 impulse, float3 point) + { + // Linear + pv.Linear += impulse; + + // Angular + { + // Calculate point impulse + var worldFromEntity = new RigidTransform(r.Value, t.Value); + var worldFromMotion = math.mul(worldFromEntity, pm.Transform); + float3 angularImpulseWorldSpace = math.cross(point - worldFromMotion.pos, impulse); + float3 angularImpulseInertiaSpace = math.rotate(math.inverse(worldFromMotion.rot), angularImpulseWorldSpace); + + pv.Angular += angularImpulseInertiaSpace * pm.InverseInertia; + } + } + + public static void ApplyLinearImpulse(this Entity entity, float3 impulse) + { + var em = World.Active.GetOrCreateManager(); + + if (em.HasComponent(entity) && + em.HasComponent(entity)) + { + var velocityData = em.GetComponentData(entity); + var massData = em.GetComponentData(entity); + + velocityData.ApplyLinearImpulse(massData, impulse); + + em.SetComponentData(entity, velocityData); + } + } + + public static void ApplyLinearImpulse(ref this PhysicsVelocity velocityData, PhysicsMass massData, float3 impulse) + { + velocityData.Linear += impulse * massData.InverseMass; + } + + public static void ApplyAngularImpulse(this Entity entity, float3 impulse) + { + var em = World.Active.GetOrCreateManager(); + + if (em.HasComponent(entity) && + em.HasComponent(entity)) + { + var velocityData = em.GetComponentData(entity); + var massData = em.GetComponentData(entity); + + velocityData.ApplyAngularImpulse(massData, impulse); + + em.SetComponentData(entity, velocityData); + } + } + + public static void ApplyAngularImpulse(ref this PhysicsVelocity velocityData, PhysicsMass massData, float3 impulse) + { + velocityData.Angular += impulse * massData.InverseInertia; + } + } +} diff --git a/package/Unity.Physics/Extensions/World/ComponentExtensions.cs.meta b/package/Unity.Physics/Extensions/World/ComponentExtensions.cs.meta new file mode 100755 index 000000000..25335cc27 --- /dev/null +++ b/package/Unity.Physics/Extensions/World/ComponentExtensions.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 97c7f410dbf04ed49874c1a38df8286e +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Unity.Physics/Extensions/World/PhysicsWorldExtensions.cs b/package/Unity.Physics/Extensions/World/PhysicsWorldExtensions.cs new file mode 100755 index 000000000..e3f3c9da6 --- /dev/null +++ b/package/Unity.Physics/Extensions/World/PhysicsWorldExtensions.cs @@ -0,0 +1,201 @@ +using Unity.Entities; +using Unity.Mathematics; + +namespace Unity.Physics.Extensions +{ + // Utility functions acting on a physics world + public static class PhysicsWorldExtensions + { + // Find the index in the Bodies array of a particular Rigid Body from its Entity + public static int GetRigidBodyIndex(this in PhysicsWorld world, Entity entity) + { + int idx = 0; + for( int i = 0; i < world.Bodies.Length; i++) + { + if (world.Bodies[i].Entity == entity) break; + idx++; + } + return (idx < world.NumBodies) ? idx : -1; + } + + public static CollisionFilter GetCollisionFilter(this in PhysicsWorld world, int rigidBodyIndex) + { + CollisionFilter filter = CollisionFilter.Default; + if (!(0 <= rigidBodyIndex && rigidBodyIndex < world.NumBodies)) return filter; + + unsafe { filter = world.Bodies[rigidBodyIndex].Collider->Filter; } + + return filter; + } + + public static float GetMass(this in PhysicsWorld world, int rigidBodyIndex) + { + if (!(0 <= rigidBodyIndex && rigidBodyIndex < world.NumDynamicBodies)) return 0; + + MotionVelocity mv = world.MotionVelocities[rigidBodyIndex]; + + return 0 == mv.InverseInertiaAndMass.w ? 0.0f : 1.0f / mv.InverseInertiaAndMass.w; + } + + // Get the effective mass of a Rigid Body in a given direction and from a particular point (in World Space) + public static float GetEffectiveMass(this in PhysicsWorld world, int rigidBodyIndex, float3 impulse, float3 point) + { + if (!(0 <= rigidBodyIndex && rigidBodyIndex < world.NumDynamicBodies)) return 0; + + float effMass = 0; + + MotionVelocity mv = world.MotionVelocities[rigidBodyIndex]; + + float3 pointDir = math.normalizesafe(point - GetCenterOfMass(world, rigidBodyIndex)); + float3 impulseDir = math.normalizesafe(impulse); + + float3 jacobian = math.cross(pointDir, impulseDir); + float invEffMass = math.csum(math.dot(jacobian, jacobian) * mv.InverseInertiaAndMass.xyz); + effMass = math.select(1.0f / invEffMass, 0.0f, math.abs(invEffMass) < 1e-5); + + return effMass; + } + + // Get the Rigid Bodies Center of Mass (in World Space) + public static float3 GetCenterOfMass(this in PhysicsWorld world, int rigidBodyIndex) + { + if (!(0 <= rigidBodyIndex && rigidBodyIndex < world.NumDynamicBodies)) return float3.zero; + + return world.MotionDatas[rigidBodyIndex].WorldFromMotion.pos; + } + + public static float3 GetPosition(this in PhysicsWorld world, int rigidBodyIndex) + { + if (!(0 <= rigidBodyIndex && rigidBodyIndex < world.NumDynamicBodies)) return float3.zero; + + // Motion to body transform + MotionData md = world.MotionDatas[rigidBodyIndex]; + + RigidTransform worldFromBody = math.mul(md.WorldFromMotion, math.inverse(md.BodyFromMotion)); + return worldFromBody.pos; + } + + public static quaternion GetRotation(this in PhysicsWorld world, int rigidBodyIndex) + { + if (!(0 <= rigidBodyIndex && rigidBodyIndex < world.NumDynamicBodies)) return quaternion.identity; + + // Motion to body transform + MotionData md = world.MotionDatas[rigidBodyIndex]; + + RigidTransform worldFromBody = math.mul(md.WorldFromMotion, math.inverse(md.BodyFromMotion)); + return worldFromBody.rot; + } + + // Get the linear velocity of a rigid body (in world space) + public static float3 GetLinearVelocity(this in PhysicsWorld world, int rigidBodyIndex) + { + if (!(0 <= rigidBodyIndex && rigidBodyIndex < world.NumDynamicBodies)) return float3.zero; + + return world.MotionVelocities[rigidBodyIndex].LinearVelocity; + } + + // Set the linear velocity of a rigid body (in world space) + public static void SetLinearVelocity(this PhysicsWorld world, int rigidBodyIndex, float3 linearVelocity) + { + if (!(0 <= rigidBodyIndex && rigidBodyIndex < world.NumDynamicBodies)) return; + + Unity.Collections.NativeSlice motionVelocities = world.MotionVelocities; + MotionVelocity mv = motionVelocities[rigidBodyIndex]; + mv.LinearVelocity = linearVelocity; + motionVelocities[rigidBodyIndex] = mv; + } + + // Get the linear velocity of a rigid body at a given point (in world space) + public static float3 GetLinearVelocity(this in PhysicsWorld world, int rigidBodyIndex, float3 point) + { + if (!(0 <= rigidBodyIndex && rigidBodyIndex < world.NumDynamicBodies)) return float3.zero; + + MotionVelocity mv = world.MotionVelocities[rigidBodyIndex]; + MotionData md = world.MotionDatas[rigidBodyIndex]; + + float3 av = math.rotate(md.WorldFromMotion, mv.AngularVelocity); + float3 lv = float3.zero; + lv += math.cross(av, (point - md.WorldFromMotion.pos)); + lv += mv.LinearVelocity; + + return lv; + } + + // Get the angular velocity of a rigid body around it's center of mass (in world space) + public static float3 GetAngularVelocity(this in PhysicsWorld world, int rigidBodyIndex) + { + if (!(0 <= rigidBodyIndex && rigidBodyIndex < world.NumDynamicBodies)) return float3.zero; + + MotionVelocity mv = world.MotionVelocities[rigidBodyIndex]; + MotionData md = world.MotionDatas[rigidBodyIndex]; + + return math.rotate(md.WorldFromMotion, mv.AngularVelocity); + } + + // Set the angular velocity of a rigid body (in world space) + public static void SetAngularVelocity(this PhysicsWorld world, int rigidBodyIndex, float3 angularVelocity) + { + if (!(0 <= rigidBodyIndex && rigidBodyIndex < world.NumDynamicBodies)) return; + + MotionData md = world.MotionDatas[rigidBodyIndex]; + float3 angularVelocityMotionSpace = math.rotate(math.inverse(md.WorldFromMotion.rot), angularVelocity); + + Unity.Collections.NativeSlice motionVelocities = world.MotionVelocities; + MotionVelocity mv = motionVelocities[rigidBodyIndex]; + mv.AngularVelocity = angularVelocityMotionSpace; + motionVelocities[rigidBodyIndex] = mv; + } + + // Apply an impulse to a rigid body at a point (in world space) + public static void ApplyImpulse(this PhysicsWorld world, int rigidBodyIndex, float3 linearImpulse, float3 point) + { + if (!(0 <= rigidBodyIndex && rigidBodyIndex < world.NumDynamicBodies)) return; + + MotionData md = world.MotionDatas[rigidBodyIndex]; + float3 angularImpulseWorldSpace = math.cross(point - md.WorldFromMotion.pos, linearImpulse); + float3 angularImpulseMotionSpace = math.rotate(math.inverse(md.WorldFromMotion.rot), angularImpulseWorldSpace); + + Unity.Collections.NativeSlice motionVelocities = world.MotionVelocities; + MotionVelocity mv = motionVelocities[rigidBodyIndex]; + mv.ApplyLinearImpulse(linearImpulse); + mv.ApplyAngularImpulse(angularImpulseMotionSpace); + motionVelocities[rigidBodyIndex] = mv; + } + + // Apply a linear impulse to a rigid body (in world space) + public static void ApplyLinearImpulse(this PhysicsWorld world, int rigidBodyIndex, float3 linearImpulse) + { + if (!(0 <= rigidBodyIndex && rigidBodyIndex < world.NumDynamicBodies)) return; + + Unity.Collections.NativeSlice motionVelocities = world.MotionVelocities; + MotionVelocity mv = motionVelocities[rigidBodyIndex]; + mv.ApplyLinearImpulse(linearImpulse); + motionVelocities[rigidBodyIndex] = mv; + } + + // Apply an angular impulse to a rigidBodyIndex (in world space) + public static void ApplyAngularImpulse(this PhysicsWorld world, int rigidBodyIndex, float3 angularImpulse) + { + if (!(0 <= rigidBodyIndex && rigidBodyIndex < world.NumDynamicBodies)) return; + + MotionData md = world.MotionDatas[rigidBodyIndex]; + float3 angularImpulseInertiaSpace = math.rotate(math.inverse(md.WorldFromMotion.rot), angularImpulse); + + Unity.Collections.NativeSlice motionVelocities = world.MotionVelocities; + MotionVelocity mv = motionVelocities[rigidBodyIndex]; + mv.ApplyAngularImpulse(angularImpulseInertiaSpace); + motionVelocities[rigidBodyIndex] = mv; + } + + // Calculate a linear and angular velocity required to move the given rigid body to the given target transform + // in the given time step. + public static void CalculateVelocityToTarget( + this PhysicsWorld world, int rigidBodyIndex, float3 targetPosition, quaternion targetOrientation, float timeStep, + out float3 requiredLinearVelocity, out float3 requiredAngularVelocity) + { + // TODO + requiredLinearVelocity = float3.zero; + requiredAngularVelocity = float3.zero; + } + } +} diff --git a/package/Unity.Physics/Extensions/World/PhysicsWorldExtensions.cs.meta b/package/Unity.Physics/Extensions/World/PhysicsWorldExtensions.cs.meta new file mode 100755 index 000000000..f5b351c88 --- /dev/null +++ b/package/Unity.Physics/Extensions/World/PhysicsWorldExtensions.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 52fef0fd9effe5e4a8a11931342d2a4b +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/Unity.Physics/Unity.Physics.asmdef b/package/Unity.Physics/Unity.Physics.asmdef new file mode 100755 index 000000000..caaabe4c7 --- /dev/null +++ b/package/Unity.Physics/Unity.Physics.asmdef @@ -0,0 +1,18 @@ +{ + "name": "Unity.Physics", + "references": [ + "Unity.Burst", + "Unity.Collections", + "Unity.Entities", + "Unity.Entities.Hybrid", + "Unity.Mathematics", + "Unity.Transforms", + "Unity.Transforms.Hybrid", + "Unity.Rendering.Hybrid", + "Unity.Blobs" + ], + "optionalUnityReferences": [], + "includePlatforms": [], + "excludePlatforms": [], + "allowUnsafeCode": true +} \ No newline at end of file diff --git a/package/Unity.Physics/Unity.Physics.asmdef.meta b/package/Unity.Physics/Unity.Physics.asmdef.meta new file mode 100755 index 000000000..9e989d081 --- /dev/null +++ b/package/Unity.Physics/Unity.Physics.asmdef.meta @@ -0,0 +1,7 @@ +fileFormatVersion: 2 +guid: 63afb046c8423dd448ae7aba042ea63d +AssemblyDefinitionImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package/package.json b/package/package.json new file mode 100755 index 000000000..1dd17a7f4 --- /dev/null +++ b/package/package.json @@ -0,0 +1,22 @@ +{ + "displayName": "Unity Physics", + "category": "Unity", + "description": "Unity's C# stateless physics library. This package is still in experimental phase.", + "dependencies": { + "com.unity.burst": "1.0.0-preview.6", + "com.unity.entities": "0.0.12-preview.29", + "com.unity.test-framework.performance": "1.0.6-preview" + }, + "keywords": [ + "unity", + "physics" + ], + "name": "com.unity.physics", + "unity": "2019.1", + "version": "0.0.1-preview.1", + "repository": { + "type": "git", + "url": "git@github.com:Unity-Technologies/Unity.Physics.git", + "revision": "a2c3c11a0e2292b084fe133eef90a30c8f41af7d" + } +} diff --git a/package/package.json.meta b/package/package.json.meta new file mode 100755 index 000000000..4f04b33c8 --- /dev/null +++ b/package/package.json.meta @@ -0,0 +1,7 @@ +fileFormatVersion: 2 +guid: 952190cfedfe3ac4abb4da4b2b94aec4 +TextScriptImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/versions.txt b/versions.txt new file mode 100755 index 000000000..99b8be86d --- /dev/null +++ b/versions.txt @@ -0,0 +1 @@ +0.0.1-preview.1