/// <summary>
/// Composes this transform with another.
/// </summary>
/// <param name="transform">The transform to compose with this one.</param>
/// <returns>A single <see cref="TransformUtil"/> representing this transform followed by the other.</returns>
public TransformUtil Compose(TransformUtil transform)
{
// NB: This is just matrix multiplication of transform * this (not forgetting the implicit third row of 0 0 1 in each).
return(new TransformUtil(
transform.r1c1 * this.r1c1 + transform.r1c2 * this.r2c1,
transform.r1c1 * this.r1c2 + transform.r1c2 * this.r2c2,
transform.r1c1 * this.r1c3 + transform.r1c2 * this.r2c3 + transform.r1c3,
transform.r2c1 * this.r1c1 + transform.r2c2 * this.r2c1,
transform.r2c1 * this.r1c2 + transform.r2c2 * this.r2c2,
transform.r2c1 * this.r1c3 + transform.r2c2 * this.r2c3 + transform.r2c3));
}
/// <summary>
/// Given a line segment AB and a circle position and radius, this function
/// determines if there is an intersection and stores the position of the
/// closest intersection in the reference IntersectionPoint. returns false
/// if no intersection point is found.
/// </summary>
/// <param name="a">One end of the line segment.</param>
/// <param name="b">Other end of the line segment.</param>
/// <param name="pos">The centre of the circle.</param>
/// <param name="radius">The radius of the circle.</param>
/// <returns>The intersection point closest to a any exist, otherwise null.</returns>
public static float2?GetLineSegmentCircleClosestIntersectionPoint(float2 a, float2 b, float2 pos, float radius)
{
float2?intersectionPoint = null;
// To make the following calculations easier, move the circle into the local space defined by the vector AB with origin at A
var toBNorm = math.normalize(b - a);
//todo
var tmpX = toBNorm.x;
toBNorm.x = toBNorm.y;
toBNorm.y = -tmpX;
var localPos = TransformUtil.ToLocalSpace(a, toBNorm, toBNorm).Apply(pos);
// if the local position + the radius is negative then the circle lays behind
// point A so there is no intersection possible. If the local x pos minus the
// radius is greater than length A-B then the circle cannot intersect the
// line segment
if ((localPos.x + radius >= 0) && (localPos.x - radius) * (localPos.x - radius) <= math.lengthsq(b - a))
{
// if the distance from the x axis to the object's position is less
// than its radius then there is a potential intersection.
if (Math.Abs(localPos.y) < radius)
{
// now to do a line/circle intersection test. If the center of the
// circle is at (x, y) then the intersection points are at
// (x +/- sqrt(r^2 - y^2), 0). We only need to look at the smallest
// positive value of x.
var x = localPos.x;
var y = localPos.y;
var ip = x - ( float )Math.Sqrt(radius * radius - y * y);
if (ip <= 0)
{
ip = x + ( float )Math.Sqrt(radius * radius - y * y);
}
intersectionPoint = a + toBNorm * ip;
}
}
return(intersectionPoint);
}
protected override void OnUpdate()
{
var targetQuery = this.GetEntityQuery(typeof(WallData));
var targetEntityDataArray = targetQuery.ToComponentDataArray <WallData>(Allocator.TempJob);
var targetInfos = new NativeArray <WallInfo>(targetEntityDataArray.Length, Allocator.TempJob);
for (var i = 0; i < targetInfos.Length; i++)
{
targetInfos[i] = new WallInfo
{
from = targetEntityDataArray[i].from,
to = targetEntityDataArray[i].to,
normal = targetEntityDataArray[i].normal
}
}
;
targetEntityDataArray.Dispose();
var wallDetectionFeelers = new NativeArray <float2>(3, Allocator.TempJob);
this.Entities.ForEach((Entity vehicle, ref VehicleData vehicleData, in EntityData entityData, in MovingData movingData) =>
{
// feeler pointing straight in front
vehicleData.wallDetectionFeeler0 = entityData.position + movingData.forward * vehicleData.wallDetectionFeelerLength;
// feeler to left
var temp = TransformUtil.Rotation(( float )System.Math.PI * 1.75f).Apply(movingData.forward);
vehicleData.wallDetectionFeeler1 = entityData.position + vehicleData.wallDetectionFeelerLength / 2f * temp;
// feeler to right
temp = TransformUtil.Rotation(( float )System.Math.PI * 0.25f).Apply(movingData.forward);
vehicleData.wallDetectionFeeler2 = entityData.position + vehicleData.wallDetectionFeelerLength / 2f * temp;
var distToThisIP = 0.0f;
var distToClosestIP = float.MaxValue;
var steeringForce = float2.zero;
wallDetectionFeelers[0] = vehicleData.wallDetectionFeeler0;
wallDetectionFeelers[1] = vehicleData.wallDetectionFeeler1;
wallDetectionFeelers[2] = vehicleData.wallDetectionFeeler2;
// examine each feeler in turn
var count = wallDetectionFeelers.Length;
var flr = 0;
for ( ; flr < count; ++flr)
{
// run through each wall checking for any intersection points
var c2 = targetInfos.Length;
for (var i = 0; i < c2; i++)
{
var targetInfo = targetInfos[i];
if (GeometryUtil.LineSegmentIntersectionPoint(
entityData.position,
wallDetectionFeelers[flr],
targetInfo.from,
targetInfo.to,
out var point))
{
// is this the closest found so far? If so keep a record
if (math.lengthsq(entityData.position - point) < distToClosestIP)
{
distToClosestIP = distToThisIP;
vehicleData.wallDetectionData = new VehicleData.WallDetectionData
{
wallNormal = targetInfo.normal,
closestPoint = point,
wallDetectionFeelerIndex = flr
};
break;
}
}
}
}
}).ScheduleParallel();
