// Retrace portals between corners and register if type of polygon changes
public static int RetracePortals(UnityEngine.Experimental.AI.NavMeshQuery query, int startIndex, int endIndex,
Unity.Collections.NativeSlice <UnityEngine.Experimental.AI.PolygonId> path, int n, Vector3 termPos,
ref Unity.Collections.NativeArray <UnityEngine.Experimental.AI.NavMeshLocation> straightPath,
ref Unity.Collections.NativeArray <StraightPathFlags> straightPathFlags, int maxStraightPath)
{
for (var k = startIndex; k < endIndex - 1; ++k)
{
var type1 = query.GetPolygonType(path[k]);
var type2 = query.GetPolygonType(path[k + 1]);
if (type1 != type2)
{
Vector3 l, r;
var status = query.GetPortalPoints(path[k], path[k + 1], out l, out r);
Unity.Mathematics.float3 cpa1, cpa2;
GeometryUtils.SegmentSegmentCPA(out cpa1, out cpa2, l, r, straightPath[n - 1].position, termPos);
straightPath[n] = query.CreateLocation(cpa1, path[k + 1]);
straightPathFlags[n] = type2 == UnityEngine.Experimental.AI.NavMeshPolyTypes.OffMeshConnection ? StraightPathFlags.OffMeshConnection : 0;
if (++n == maxStraightPath)
{
return(maxStraightPath);
}
}
}
straightPath[n] = query.CreateLocation(termPos, path[endIndex]);
straightPathFlags[n] = query.GetPolygonType(path[endIndex]) == UnityEngine.Experimental.AI.NavMeshPolyTypes.OffMeshConnection
? StraightPathFlags.OffMeshConnection
: 0;
return(++n);
}
//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 <Physics.RigidBody> dynamicBodies = world.DynamicBodies;
Physics.RigidBody rb = dynamicBodies[index];
BlobAssetReference <Physics.Collider> collider = Unity.Physics.BoxCollider.Create(pos, orientation, size, .01f);
rb.Collider = (Collider *)collider.GetUnsafePtr();
dynamicBodies[index] = rb;
}
public static ref T GetRefRead <T>(this Unity.Collections.NativeSlice <T> array, int index) where T : struct
{
CheckArray(index, array.Length);
unsafe {
var ptr = array.GetUnsafeReadOnlyPtr();
#if UNITY_2020_1_OR_NEWER
return(ref UnsafeUtility.ArrayElementAsRef <T>(ptr, index));
#else
throw new Exception("UnsafeUtility.ArrayElementAsRef");
#endif
}
}
/// <summary>
/// Apply a linear constant force to a rigid body considering deltaTime(in world space)
/// </summary>
/// <param name="world"></param>
/// <param name="rigidBodyIndex"></param>
/// <param name="linearImpulse"></param>
public static void ApplyLinearForce(this PhysicsWorld world, int rigidBodyIndex, float3 linearImpulse)
{
if (!(0 <= rigidBodyIndex && rigidBodyIndex < world.NumDynamicBodies))
{
return;
}
Unity.Collections.NativeSlice <MotionVelocity> motionVelocities = world.MotionVelocities;
MotionVelocity mv = motionVelocities[rigidBodyIndex];
mv.LinearVelocity += (linearImpulse) * mv.InverseMass * Time.deltaTime;
motionVelocities[rigidBodyIndex] = mv;
}
/// <summary>
/// Add acceleration to Velocity considering deltaTime (in world space)
/// </summary>
/// <param name="world"></param>
/// <param name="rigidBodyIndex"></param>
/// <param name="acceleration"></param>
public static void ApplyLinearAcceleration(this PhysicsWorld world, int rigidBodyIndex, float3 acceleration)
{
if (!(0 <= rigidBodyIndex && rigidBodyIndex < world.NumDynamicBodies))
{
return;
}
Unity.Collections.NativeSlice <MotionVelocity> motionVelocities = world.MotionVelocities;
MotionVelocity mv = motionVelocities[rigidBodyIndex];
mv.LinearVelocity += acceleration;
motionVelocities[rigidBodyIndex] = mv;
}
public static ref T GetRef <T>(this Unity.Collections.NativeSlice <T> array, int index) where T : struct
{
if (index < 0 || index >= array.Length)
{
throw new ArgumentOutOfRangeException(nameof(index));
}
unsafe {
var ptr = array.GetUnsafePtr();
#if UNITY_2020_1_OR_NEWER
return(ref UnsafeUtility.ArrayElementAsRef <T>(ptr, index));
#else
throw new Exception("UnsafeUtility.ArrayElementAsRef");
#endif
}
}
static StackObject *CombineDependencies_9(ILIntepreter __intp, StackObject *__esp, IList <object> __mStack, CLRMethod __method, bool isNewObj)
{
CSHotFix.Runtime.Enviorment.AppDomain __domain = __intp.AppDomain;
StackObject *ptr_of_this_method;
StackObject *__ret = ILIntepreter.Minus(__esp, 1);
ptr_of_this_method = ILIntepreter.Minus(__esp, 1);
Unity.Collections.NativeSlice <Unity.Jobs.JobHandle> @jobs = (Unity.Collections.NativeSlice <Unity.Jobs.JobHandle>) typeof(Unity.Collections.NativeSlice <Unity.Jobs.JobHandle>).CheckCLRTypes(StackObject.ToObject(ptr_of_this_method, __domain, __mStack));
__intp.Free(ptr_of_this_method);
var result_of_this_method = Unity.Jobs.JobHandle.CombineDependencies(@jobs);
return(ILIntepreter.PushObject(__ret, __mStack, result_of_this_method));
}
/// <summary>
/// Add to the angular velocity of a rigid body considering deltaTime (in world space)
/// </summary>
/// <param name="world"></param>
/// <param name="rigidBodyIndex"></param>
/// <param name="angularVelocity"></param>
public static void ApplyAngularAcceleration(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 <MotionVelocity> motionVelocities = world.MotionVelocities;
MotionVelocity mv = motionVelocities[rigidBodyIndex];
mv.AngularVelocity += angularVelocityMotionSpace * Time.deltaTime;
motionVelocities[rigidBodyIndex] = mv;
}
//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 <Physics.RigidBody> staticBodies = world.StaticBodies;
Physics.RigidBody rb = staticBodies[index];
BlobAssetReference <Physics.Collider> collider = Unity.Physics.BoxCollider.Create(new BoxGeometry
{
Center = pos,
Orientation = orientation,
Size = size,
BevelRadius = 0.01f
});
rb.Collider = (Collider *)collider.GetUnsafePtr();
staticBodies[index] = rb;
}
/// <summary>
/// Apply an Acceleration to a rigid body at a point considering mass (in world space)
/// </summary>
/// <param name="world"></param>
/// <param name="rigidBodyIndex"></param>
/// <param name="linearImpulse"></param>
/// <param name="point"></param>
public static void ApplyAccelerationImpulse(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 <MotionVelocity> motionVelocities = world.MotionVelocities;
MotionVelocity mv = motionVelocities[rigidBodyIndex];
mv.LinearVelocity += (linearImpulse) * Time.deltaTime;
mv.AngularVelocity += (angularImpulseMotionSpace) * Time.deltaTime;
motionVelocities[rigidBodyIndex] = mv;
}
public static void Lines(Unity.Collections.NativeSlice <Vector3> lineSegments, Color color, float duration = 0, bool depthTest = true)
{
if (lineSegments == null || lineSegments.Length == 0)
{
return;
}
LineBatchJob job;
if (!TryGetLineBatch(out job, depthTest, UnityEngine.Rendering.CullMode.Off))
{
return;
}
for (var n = 1; n < lineSegments.Length; ++n)
{
job.AddLine(lineSegments[n - 1], lineSegments[n], color, duration);
}
}
public DataBuffer(World world, ME.ECS.Collections.BufferArray <Entity> arr, int minIdx, int maxIdx)
{
var reg = (StructComponents <T>)world.currentState.structComponents.list.arr[WorldUtilities.GetAllComponentTypeId <T>()];
this.minIdx = minIdx;
if (this.minIdx > maxIdx)
{
this.minIdx = 0;
maxIdx = 0;
}
if (this.minIdx < 0)
{
this.minIdx = 0;
}
if (maxIdx >= reg.components.Length)
{
maxIdx = reg.components.Length - 1;
}
this.Length = maxIdx - this.minIdx;
this.arr = new Unity.Collections.NativeArray <T>(reg.components.arr, Unity.Collections.Allocator.Persistent);
this.data = new Unity.Collections.NativeSlice <T>(this.arr, this.minIdx, maxIdx);
}
public static UnityEngine.Experimental.AI.PathQueryStatus FindStraightPath(UnityEngine.Experimental.AI.NavMeshQuery query, Vector3 startPos, Vector3 endPos,
Unity.Collections.NativeSlice <UnityEngine.Experimental.AI.PolygonId> path, int pathSize,
ref Unity.Collections.NativeArray <UnityEngine.Experimental.AI.NavMeshLocation> straightPath,
ref Unity.Collections.NativeArray <StraightPathFlags> straightPathFlags,
ref Unity.Collections.NativeArray <float> vertexSide, ref int straightPathCount,
int maxStraightPath)
{
if (!query.IsValid(path[0]))
{
straightPath[0] = new UnityEngine.Experimental.AI.NavMeshLocation(); // empty terminator
return(UnityEngine.Experimental.AI.PathQueryStatus.Failure); // | PathQueryStatus.InvalidParam;
}
straightPath[0] = query.CreateLocation(startPos, path[0]);
straightPathFlags[0] = StraightPathFlags.Start;
var apexIndex = 0;
var n = 1;
if (pathSize > 1)
{
var startPolyWorldToLocal = query.PolygonWorldToLocalMatrix(path[0]);
var apex = startPolyWorldToLocal.MultiplyPoint(startPos);
var left = new Vector3(0, 0, 0); // Vector3.zero accesses a static readonly which does not work in burst yet
var right = new Vector3(0, 0, 0);
var leftIndex = -1;
var rightIndex = -1;
for (var i = 1; i <= pathSize; ++i)
{
var polyWorldToLocal = query.PolygonWorldToLocalMatrix(path[apexIndex]);
Vector3 vl, vr;
if (i == pathSize)
{
vl = vr = polyWorldToLocal.MultiplyPoint(endPos);
}
else
{
var success = query.GetPortalPoints(path[i - 1], path[i], out vl, out vr);
if (!success)
{
return(UnityEngine.Experimental.AI.PathQueryStatus.Failure); // | PathQueryStatus.InvalidParam;
}
vl = polyWorldToLocal.MultiplyPoint(vl);
vr = polyWorldToLocal.MultiplyPoint(vr);
}
vl = vl - apex;
vr = vr - apex;
// Ensure left/right ordering
if (PathUtils.Perp2D(vl, vr) < 0)
{
PathUtils.Swap(ref vl, ref vr);
}
// Terminate funnel by turning
if (PathUtils.Perp2D(left, vr) < 0)
{
var polyLocalToWorld = query.PolygonLocalToWorldMatrix(path[apexIndex]);
var termPos = polyLocalToWorld.MultiplyPoint(apex + left);
n = PathUtils.RetracePortals(query, apexIndex, leftIndex, path, n, termPos, ref straightPath, ref straightPathFlags, maxStraightPath);
if (vertexSide.Length > 0)
{
vertexSide[n - 1] = -1;
}
//Debug.Log("LEFT");
if (n == maxStraightPath)
{
straightPathCount = n;
return(UnityEngine.Experimental.AI.PathQueryStatus.Success); // | PathQueryStatus.BufferTooSmall;
}
apex = polyWorldToLocal.MultiplyPoint(termPos);
left.Set(0, 0, 0);
right.Set(0, 0, 0);
i = apexIndex = leftIndex;
continue;
}
if (PathUtils.Perp2D(right, vl) > 0)
{
var polyLocalToWorld = query.PolygonLocalToWorldMatrix(path[apexIndex]);
var termPos = polyLocalToWorld.MultiplyPoint(apex + right);
n = PathUtils.RetracePortals(query, apexIndex, rightIndex, path, n, termPos, ref straightPath, ref straightPathFlags, maxStraightPath);
if (vertexSide.Length > 0)
{
vertexSide[n - 1] = 1;
}
//Debug.Log("RIGHT");
if (n == maxStraightPath)
{
straightPathCount = n;
return(UnityEngine.Experimental.AI.PathQueryStatus.Success); // | PathQueryStatus.BufferTooSmall;
}
apex = polyWorldToLocal.MultiplyPoint(termPos);
left.Set(0, 0, 0);
right.Set(0, 0, 0);
i = apexIndex = rightIndex;
continue;
}
// Narrow funnel
if (PathUtils.Perp2D(left, vl) >= 0)
{
left = vl;
leftIndex = i;
}
if (PathUtils.Perp2D(right, vr) <= 0)
{
right = vr;
rightIndex = i;
}
}
}
// Remove the the next to last if duplicate point - e.g. start and end positions are the same
// (in which case we have get a single point)
if (n > 0 && straightPath[n - 1].position == endPos)
{
n--;
}
n = PathUtils.RetracePortals(query, apexIndex, pathSize - 1, path, n, endPos, ref straightPath, ref straightPathFlags, maxStraightPath);
if (vertexSide.Length > 0)
{
vertexSide[n - 1] = 0;
}
if (n == maxStraightPath)
{
straightPathCount = n;
return(UnityEngine.Experimental.AI.PathQueryStatus.Success); // | PathQueryStatus.BufferTooSmall;
}
// Fix flag for final path point
straightPathFlags[n - 1] = StraightPathFlags.End;
straightPathCount = n;
return(UnityEngine.Experimental.AI.PathQueryStatus.Success);
}
