/** Gradient and value of the cost function of this VO.
* The VO has a cost function which is 0 outside the VO
* and increases inside it as the point moves further into
* the VO.
*
* This is the negative gradient of that function as well as its
* value (the weight). The negative gradient points in the direction
* where the function decreases the fastest.
*
* The value of the function is the distance to the closest edge
* of the VO and the gradient is normalized.
*/
public Vector2 Gradient(Vector2 p, out float weight)
{
if (colliding)
{
// Calculate double signed area of the triangle consisting of the points
// {line1, line1+dir1, p}
float l1 = SignedDistanceFromLine(line1, dir1, p);
// Serves as a check for which side of the line the point p is
if (l1 >= 0)
{
weight = l1;
return(new Vector2(-dir1.y, dir1.x));
}
else
{
weight = 0;
return(new Vector2(0, 0));
}
}
float det3 = SignedDistanceFromLine(cutoffLine, cutoffDir, p);
if (det3 <= 0)
{
weight = 0;
return(Vector2.zero);
}
else
{
// Signed distances to the two edges along the sides of the VO
float det1 = SignedDistanceFromLine(line1, dir1, p);
float det2 = SignedDistanceFromLine(line2, dir2, p);
if (det1 >= 0 && det2 >= 0)
{
// We are inside both of the half planes
// (all three if we count the cutoff line)
// and thus inside the forbidden region in velocity space
// Actually the negative gradient because we want the
// direction where it slopes the most downwards, not upwards
Vector2 gradient;
// Check if we are in the semicircle region near the cap of the VO
if (Vector2.Dot(p - line1, dir1) > 0 && Vector2.Dot(p - line2, dir2) < 0)
{
if (segment)
{
// This part will only be reached for line obstacles (i.e not other agents)
if (det3 < radius)
{
PF.Vector3 closestPointOnLine = VectorMath.ClosestPointOnSegment(segmentStart.ToPFV2(), segmentEnd.ToPFV2(), p.ToPFV2());
var dirFromCenter = p.ToPFV2() - closestPointOnLine.ToV2();
float distToCenter;
gradient = VectorMath.Normalize(dirFromCenter, out distToCenter);
// The weight is the distance to the edge
weight = radius - distToCenter;
return(gradient);
}
}
else
{
var dirFromCenter = p - circleCenter;
float distToCenter;
gradient = VectorMath.Normalize(dirFromCenter, out distToCenter);
// The weight is the distance to the edge
weight = radius - distToCenter;
return(gradient);
}
}
if (segment && det3 < det1 && det3 < det2)
{
weight = det3;
gradient = new Vector2(-cutoffDir.y, cutoffDir.x);
return(gradient);
}
// Just move towards the closest edge
// The weight is the distance to the edge
if (det1 < det2)
{
weight = det1;
gradient = new Vector2(-dir1.y, dir1.x);
}
else
{
weight = det2;
gradient = new Vector2(-dir2.y, dir2.x);
}
return(gradient);
}
weight = 0;
return(Vector2.zero);
}
}
/** Simulates rotating the agent towards the specified direction and returns the new rotation.
* \param direction Direction in the movement plane to rotate towards.
* \param maxDegrees Maximum number of degrees to rotate this frame.
*
* Note that this only calculates a new rotation, it does not change the actual rotation of the agent.
*
* \see #rotationIn2D
* \see #movementPlane
*/
protected Quaternion SimulateRotationTowards(Vector2 direction, float maxDegrees)
{
if (direction != Vector2.zero)
{
Quaternion targetRotation = Quaternion.LookRotation(movementPlane.ToWorld(direction.ToPFV2(), 0).ToUnityV3(), movementPlane.ToWorld(Vector2.zero.ToPFV2(), 1).ToUnityV3());
// This causes the character to only rotate around the Z axis
if (rotationIn2D)
{
targetRotation *= Quaternion.Euler(90, 0, 0);
}
return(Quaternion.RotateTowards(simulatedRotation, targetRotation, maxDegrees));
}
return(simulatedRotation);
}
/** Creates a VO for avoiding another agent.
* Note that the segment is directed, the agent will want to be on the left side of the segment.
*/
public static VO SegmentObstacle(Vector2 segmentStart, Vector2 segmentEnd, Vector2 offset, float radius, float inverseDt, float inverseDeltaTime)
{
var vo = new VO();
// Adjusted so that a parameter weightFactor of 1 will be the default ("natural") weight factor
vo.weightFactor = 1;
// Just higher than anything else
vo.weightBonus = Mathf.Max(radius, 1) * 40;
var closestOnSegment = VectorMath.ClosestPointOnSegment(segmentStart.ToPFV2(), segmentEnd.ToPFV2(), Vector2.zero.ToPFV2());
// Collision?
if (closestOnSegment.magnitude <= radius)
{
vo.colliding = true;
vo.line1 = closestOnSegment.normalized.ToUnityV3() * (closestOnSegment.magnitude - radius) * 0.3f * inverseDeltaTime;
vo.dir1 = new Vector2(vo.line1.y, -vo.line1.x).normalized;
vo.line1 += offset;
vo.cutoffDir = Vector2.zero;
vo.cutoffLine = Vector2.zero;
vo.dir2 = Vector2.zero;
vo.line2 = Vector2.zero;
vo.radius = 0;
vo.segmentStart = Vector2.zero;
vo.segmentEnd = Vector2.zero;
vo.segment = false;
}
else
{
vo.colliding = false;
segmentStart *= inverseDt;
segmentEnd *= inverseDt;
radius *= inverseDt;
var cutoffTangent = (segmentEnd - segmentStart).normalized;
vo.cutoffDir = cutoffTangent;
vo.cutoffLine = segmentStart + new Vector2(-cutoffTangent.y, cutoffTangent.x) * radius;
vo.cutoffLine += offset;
// See documentation for details
// The call to Max is just to prevent floating point errors causing NaNs to appear
var startSqrMagnitude = segmentStart.sqrMagnitude;
var normal1 = -VectorMath.ComplexMultiply(segmentStart, new Vector2(radius, Mathf.Sqrt(Mathf.Max(0, startSqrMagnitude - radius * radius)))) / startSqrMagnitude;
var endSqrMagnitude = segmentEnd.sqrMagnitude;
var normal2 = -VectorMath.ComplexMultiply(segmentEnd, new Vector2(radius, -Mathf.Sqrt(Mathf.Max(0, endSqrMagnitude - radius * radius)))) / endSqrMagnitude;
vo.line1 = segmentStart + normal1.ToUnityV2() * radius + offset;
vo.line2 = segmentEnd + normal2.ToUnityV2() * radius + offset;
// Note that the normals are already normalized
vo.dir1 = new Vector2(normal1.y, -normal1.x);
vo.dir2 = new Vector2(normal2.y, -normal2.x);
vo.segmentStart = segmentStart;
vo.segmentEnd = segmentEnd;
vo.radius = radius;
vo.segment = true;
}
return(vo);
}
/** Calculates how far to move during a single frame */
protected Vector2 CalculateDeltaToMoveThisFrame(Vector2 position, float distanceToEndOfPath, float deltaTime)
{
if (rvoController != null && rvoController.enabled)
{
// Use RVOController to get a processed delta position
// such that collisions will be avoided if possible
return(movementPlane.ToPlane(rvoController.CalculateMovementDelta(movementPlane.ToWorld(position.ToPFV2(), 0).ToUnityV3(), deltaTime).ToPFV3()).ToUnityV2());
}
// Direction and distance to move during this frame
return(Vector2.ClampMagnitude(velocity2D * deltaTime, distanceToEndOfPath));
}