/// <summary>
/// Performs collision detection and response on two spheres.
/// </summary>
public void SphereOnSphere(SphereCollider lhs, SphereCollider rhs)
{
// Obtain the positions.
var a = lhs.position;
var b = rhs.position;
// We need to test whether the length from A to B is more than the radius.
var difference = b - a;
var lengthSqr = difference.sqrMagnitude;
var radiusSum = lhs.radius + rhs.radius;
var radiusSumSqr = radiusSum * radiusSum;
if (lengthSqr <= radiusSumSqr)
{
// We've collided.
var length = Mathf.Sqrt(lengthSqr);
var normal = length != 0f ? difference / length : lhs.transform.forward;
var intersection = radiusSum - length;
var pointOfContact = a + normal * (lhs.radius - intersection);
response.Respond(lhs, rhs, normal, intersection, pointOfContact);
}
else
{
response.NotColliding(lhs, rhs);
}
}
public void Normalize()
{
var len = (float)Math.Sqrt(x * x + y * y);
x /= len;
y /= len;
}
public static UnityEngine.Vector2Int GetSpiralPointByIndex(UnityEngine.Vector2Int center, int index)
{
if (index == 0)
{
return(center);
}
// given n an index in the squared spiral
// p the sum of point in inner square
// a the position on the current square
// n = p + a
var pos = UnityEngine.Vector2Int.zero;
var n = index;
var r = Mathf.FloorToInt((Mathf.Sqrt(n + 1) - 1) / 2) + 1;
// compute radius : inverse arithmetic sum of 8+16+24+...=
var p = (8 * r * (r - 1)) / 2;
// compute total point on radius -1 : arithmetic sum of 8+16+24+...
var en = r * 2;
// points by face
var a = (1 + n - p) % (r * 8);
// compute de position and shift it so the first is (-r,-r) but (-r+1,-r)
// so square can connect
//var pos = [0, 0, r];
switch (Mathf.FloorToInt(a / (r * 2f)))
{
// find the face : 0 top, 1 right, 2, bottom, 3 left
case 0: {
pos[0] = a - r;
pos[1] = -r;
}
break;
case 1: {
pos[0] = r;
pos[1] = (a % en) - r;
}
break;
case 2: {
pos[0] = r - (a % en);
pos[1] = r;
}
break;
case 3: {
pos[0] = -r;
pos[1] = r - (a % en);
}
break;
}
return(center + pos);
}
/// <summary>
/// Modeled after the piecewise circular function
/// y = (1/2)(1 - Math.Sqrt(1 - 4x^2)) ; [0, 0.5)
/// y = (1/2)(Math.Sqrt(-(2x - 3)*(2x - 1)) + 1) ; [0.5, 1]
/// </summary>
public static float CircularEaseInOut(float p)
{
if (p < 0.5f)
{
return(0.5f * (1 - Math.Sqrt(1 - 4 * (p * p))));
}
return(0.5f * (Math.Sqrt(-((2 * p) - 3) * ((2 * p) - 1)) + 1));
}
/// <summary>Normalize the normal of the plane</summary>
public void Normalize()
{
var magnitude = 1.0f / Mathf.Sqrt((a * a) + (b * b) + (c * c));
a *= magnitude;
b *= magnitude;
c *= magnitude;
d *= magnitude;
}
/// <summary>
/// Modeled after the piecewise circular function
/// y = (1/2)(1 - Math.Sqrt(1 - 4x^2)) ; [0, 0.5)
/// y = (1/2)(Math.Sqrt(-(2x - 3)*(2x - 1)) + 1) ; [0.5, 1]
/// </summary>
public static float EaseInOutCircular(this float This)
{
if (This < 0.5f)
{
return(0.5f * (1 - Math.Sqrt(1 - 4 * (This * This))));
}
return(0.5f * (Math.Sqrt(-(2 * This - 3) * (2 * This - 1)) + 1));
}
public static FLOAT2 GetPointOnCircle(FLOAT2 point, FLOAT2 center, float radius)
{
var vX = point.x - center.x;
var vY = point.y - center.y;
var magV = Mathf.Sqrt(vX * vX + vY * vY);
var aX = center.x + vX / magV * radius;
var aY = center.y + vY / magV * radius;
return(new FLOAT2(aX, aY));
}
/// <summary>
/// Modeled after the piecewise circ function
/// y = (1/2)(1 - Math.Sqrt(1 - 4x^2)) ; [0, 0.5)
/// y = (1/2)(Math.Sqrt(-(2x - 3)*(2x - 1)) + 1) ; [0.5, 1]
/// </summary>
internal static float EaseInOutCirc(float p)
{
if (p < 0.5f)
{
return(0.5f * (1 - Math.Sqrt(1 - (4 * (p * p)))));
}
else
{
return(0.5f * (Math.Sqrt(-((2 * p) - 3) * ((2 * p) - 1)) + 1));
}
}
public BoundsMap(Func <Cell, float> heightAt, int level)
{
minVals = new float[level + 1][];
maxVals = new float[level + 1][];
for (var i = 0; i <= level; i++)
{
var count = Triangle.CountAtLevel(i);
minVals[i] = new float[count];
maxVals[i] = new float[count];
}
var mins = minVals[level];
var maxs = maxVals[level];
foreach (var triangle in Triangle.AtLevel(level))
{
maxs[triangle.Index] = triangle.GetVertices(level).Max(heightAt);
var min = Mathf.Sqrt(triangle.GetVertices(level).EdgesCircular().Min(e => (e.First.Position * heightAt(e.First) + e.Second.Position * heightAt(e.Second)).sqrMagnitude)) / 2;
mins[triangle.Index] = Math.Min(min, triangle.GetVertices(level).Min(heightAt));
}
for (int i = level - 1; i >= 0; i--)
{
var childMins = mins;
var childMaxs = maxs;
mins = minVals[i];
maxs = maxVals[i];
foreach (var triangle in Triangle.AtLevel(i))
{
var min = float.PositiveInfinity;
var max = 0f;
foreach (var child in triangle.GetChildren(i + 1))
{
min = Math.Min(min, childMins[child.Index]);
max = Math.Max(max, childMaxs[child.Index]);
}
mins[triangle.Index] = min;
maxs[triangle.Index] = max;
}
}
}
static float ApproximateEdgeDelta(float gx, float gy, float a)
{
// (gx, gy) can be either the local pixel gradient or the direction to the pixel
if (gx == 0f || gy == 0f)
{
// linear function is correct if both gx and gy are zero
// and still fair if only one of them is zero
return(0.5f - a);
}
// normalize (gx, gy)
float length = (float)Math.Sqrt(gx * gx + gy * gy);
gx = gx / length;
gy = gy / length;
// reduce symmetrical equation to first octant only
// gx >= 0, gy >= 0, gx >= gy
gx = Math.Abs(gx);
gy = Math.Abs(gy);
if (gx < gy)
{
float temp = gx;
gx = gy;
gy = temp;
}
// compute delta
float a1 = 0.5f * gy / gx;
if (a < a1)
{
// 0 <= a < a1
return(0.5f * (gx + gy) - (float)Math.Sqrt(2f * gx * gy * a));
}
if (a < (1f - a1))
{
// a1 <= a <= 1 - a1
return((0.5f - a) * gx);
}
// 1-a1 < a <= 1
return(-0.5f * (gx + gy) + (float)Math.Sqrt(2f * gx * gy * (1f - a)));
}
static void UpdateDistance(Pixel p, int x, int y, int oX, int oY)
{
var neighbor = pixels[x + oX, y + oY];
var closest = pixels[x + oX - neighbor.dX, y + oY - neighbor.dY];
if (closest.alpha == 0f || closest == p) // neighbor has no closest yet
// or neighbor's closest is p itself
{
return;
}
var dX = neighbor.dX - oX;
var dY = neighbor.dY - oY;
var distance = Math.Sqrt(dX * dX + dY * dY) + ApproximateEdgeDelta(dX, dY, closest.alpha);
if (distance < p.distance)
{
p.distance = distance;
p.dX = dX;
p.dY = dY;
}
}
static void PostProcess(float maxDistance)
{
// adjust distances near edges based on the local edge gradient
for (int y = 0; y < height; y++)
{
for (int x = 0; x < width; x++)
{
Pixel p = pixels[x, y];
if ((p.dX == 0 && p.dY == 0) || p.distance >= maxDistance)
{
// ignore edge, inside, and beyond max distance
continue;
}
float
dX = p.dX,
dY = p.dY;
Pixel closest = pixels[x - p.dX, y - p.dY];
Vector2 g = closest.gradient;
if (g.x == 0f && g.y == 0f)
{
// ignore unknown gradients (inside)
continue;
}
// compute hit point offset on gradient inside pixel
float df = ApproximateEdgeDelta(g.x, g.y, closest.alpha);
float t = dY * g.x - dX * g.y;
float u = -df * g.x + t * g.y;
float v = -df * g.y - t * g.x;
// use hit point to compute distance
if (Math.Abs(u) <= 0.5f && Math.Abs(v) <= 0.5f)
{
p.distance = (float)Math.Sqrt((dX + u) * (dX + u) + (dY + v) * (dY + v));
}
}
}
}
public static bool Intersect(SphereXCollider src, SphereXCollider dst, out XContact?contact)
{
var dir = dst.Position - src.Position;
var sqrDist = dir.sqrMagnitude;
var space = src.Radius + dst.Radius;
var sqrSpace = space * space;
if (sqrDist < sqrSpace)
{
var dist = Mathf.Sqrt(sqrDist);
if (dist != 0)
{
contact = new XContact(src, dst, dir.normalized, space - dist);
}
else
{
contact = new XContact(src, dst, Vector3.up, src.Radius);
}
return(true);
}
contact = null;
return(false);
}
static void ComputeEdgeGradients()
{
float sqrt2 = (float)Math.Sqrt(2);
for (int y = 1; y < height - 1; y++)
{
for (int x = 1; x < width - 1; x++)
{
Pixel p = pixels[x, y];
if (p.alpha > 0f && p.alpha < 1f)
{
// estimate gradient of edge pixel using surrounding pixels
float g =
-pixels[x - 1, y - 1].alpha
- pixels[x - 1, y + 1].alpha
+ pixels[x + 1, y - 1].alpha
+ pixels[x + 1, y + 1].alpha;
p.gradient.x = g + (pixels[x + 1, y].alpha - pixels[x - 1, y].alpha) * sqrt2;
p.gradient.y = g + (pixels[x, y + 1].alpha - pixels[x, y - 1].alpha) * sqrt2;
p.gradient.Normalize();
}
}
}
}
public static float CircIn(float p)
{
return(-(Mathf.Sqrt(1f - (p * p)) - 1f));
}
/// <summary>
/// Modeled after shifted quadrant IV of unit circle
/// </summary>
public static float EaseInCircular(this float This)
{
return(1 - Math.Sqrt(1 - This * This));
}
/// <summary>
/// Modeled after shifted quadrant II of unit circle
/// </summary>
internal static float EaseOutCirc(float p)
{
return(Math.Sqrt((2 - p) * p));
}
/// <summary>
/// Modeled after shifted quadrant IV of unit circle
/// </summary>
internal static float EaseInCirc(float p)
{
return(1 - Math.Sqrt(1 - (p * p)));
}
/** Calculate an acceleration to move deltaPosition units and get there with approximately a velocity of targetVelocity */
public static Vector2 CalculateAccelerationToReachPoint(Vector2 deltaPosition, Vector2 targetVelocity, Vector2 currentVelocity, float forwardsAcceleration, float rotationSpeed, float maxSpeed, Vector2 forwardsVector)
{
// Guard against div by zero
if (forwardsAcceleration <= 0)
{
return(Vector2.zero);
}
float currentSpeed = currentVelocity.magnitude;
// Convert rotation speed to an acceleration
// See https://en.wikipedia.org/wiki/Centripetal_force
var sidewaysAcceleration = currentSpeed * rotationSpeed * Mathf.Deg2Rad;
// To avoid weird behaviour when the rotation speed is very low we allow the agent to accelerate sideways without rotating much
// if the rotation speed is very small. Also guards against division by zero.
sidewaysAcceleration = Mathf.Max(sidewaysAcceleration, forwardsAcceleration);
sidewaysAcceleration = forwardsAcceleration;
// Transform coordinates to local space where +X is the forwards direction
// This is essentially equivalent to Transform.InverseTransformDirection.
deltaPosition = VectorMath.ComplexMultiplyConjugate(deltaPosition.ToPFV2(), forwardsVector.ToPFV2()).ToUnityV2();
targetVelocity = VectorMath.ComplexMultiplyConjugate(targetVelocity.ToPFV2(), forwardsVector.ToPFV2()).ToUnityV2();
currentVelocity = VectorMath.ComplexMultiplyConjugate(currentVelocity.ToPFV2(), forwardsVector.ToPFV2()).ToUnityV2();
float ellipseSqrFactorX = 1 / (forwardsAcceleration * forwardsAcceleration);
float ellipseSqrFactorY = 1 / (sidewaysAcceleration * sidewaysAcceleration);
// If the target velocity is zero we can use a more fancy approach
// and calculate a nicer path.
// In particular, this is the case at the end of the path.
if (targetVelocity == Vector2.zero)
{
// Run a binary search over the time to get to the target point.
float mn = 0.01f;
float mx = 10;
while (mx - mn > 0.01f)
{
var time = (mx + mn) * 0.5f;
// Given that we want to move deltaPosition units from out current position, that our current velocity is given
// and that when we reach the target we want our velocity to be zero. Also assume that our acceleration will
// vary linearly during the slowdown. Then we can calculate what our acceleration should be during this frame.
//{ t = time
//{ deltaPosition = vt + at^2/2 + qt^3/6
//{ 0 = v + at + qt^2/2
//{ solve for a
// a = acceleration vector
// q = derivative of the acceleration vector
var a = (6 * deltaPosition - 4 * time * currentVelocity) / (time * time);
var q = 6 * (time * currentVelocity - 2 * deltaPosition) / (time * time * time);
// Make sure the acceleration is not greater than our maximum allowed acceleration.
// If it is we increase the time we want to use to get to the target
// and if it is not, we decrease the time to get there faster.
// Since the acceleration is described by acceleration = a + q*t
// we only need to check at t=0 and t=time.
// Note that the acceleration limit is described by an ellipse, not a circle.
var nextA = a + q * time;
if (a.x * a.x * ellipseSqrFactorX + a.y * a.y * ellipseSqrFactorY > 1.0f || nextA.x * nextA.x * ellipseSqrFactorX + nextA.y * nextA.y * ellipseSqrFactorY > 1.0f)
{
mn = time;
}
else
{
mx = time;
}
}
var finalAcceleration = (6 * deltaPosition - 4 * mx * currentVelocity) / (mx * mx);
// Boosting
{
// The trajectory calculated above has a tendency to use very wide arcs
// and that does unfortunately not look particularly good in some cases.
// Here we amplify the component of the acceleration that is perpendicular
// to our current velocity. This will make the agent turn towards the
// target quicker.
// How much amplification to use. Value is unitless.
const float Boost = 1;
finalAcceleration.y *= 1 + Boost;
// Clamp the velocity to the maximum acceleration.
// Note that the maximum acceleration constraint is shaped like an ellipse, not like a circle.
float ellipseMagnitude = finalAcceleration.x * finalAcceleration.x * ellipseSqrFactorX + finalAcceleration.y * finalAcceleration.y * ellipseSqrFactorY;
if (ellipseMagnitude > 1.0f)
{
finalAcceleration /= Mathf.Sqrt(ellipseMagnitude);
}
}
return(VectorMath.ComplexMultiply(finalAcceleration.ToPFV2(), forwardsVector.ToPFV2()).ToUnityV2());
}
else
{
// Here we try to move towards the next waypoint which has been modified slightly using our
// desired velocity at that point so that the agent will more smoothly round the corner.
// How much to strive for making sure we reach the target point with the target velocity. Unitless.
const float TargetVelocityWeight = 0.5f;
// Limit to how much to care about the target velocity. Value is in seconds.
// This prevents the character from moving away from the path too much when the target point is far away
const float TargetVelocityWeightLimit = 1.5f;
float targetSpeed;
var normalizedTargetVelocity = VectorMath.Normalize(targetVelocity.ToPFV2(), out targetSpeed);
var distance = deltaPosition.magnitude;
var targetPoint = deltaPosition.ToPFV2() - normalizedTargetVelocity * System.Math.Min(TargetVelocityWeight * distance * targetSpeed / (currentSpeed + targetSpeed), maxSpeed * TargetVelocityWeightLimit);
// How quickly the agent will try to reach the velocity that we want it to have.
// We need this to prevent oscillations and jitter which is what happens if
// we let the constant go towards zero. Value is in seconds.
const float TimeToReachDesiredVelocity = 0.1f;
// TODO: Clamp to ellipse using more accurate acceleration (use rotation speed as well)
var finalAcceleration = (targetPoint.normalized * maxSpeed - currentVelocity.ToPFV2()) * (1f / TimeToReachDesiredVelocity);
// Clamp the velocity to the maximum acceleration.
// Note that the maximum acceleration constraint is shaped like an ellipse, not like a circle.
float ellipseMagnitude = finalAcceleration.x * finalAcceleration.x * ellipseSqrFactorX + finalAcceleration.y * finalAcceleration.y * ellipseSqrFactorY;
if (ellipseMagnitude > 1.0f)
{
finalAcceleration /= Mathf.Sqrt(ellipseMagnitude);
}
return(VectorMath.ComplexMultiply(finalAcceleration, forwardsVector.ToPFV2()).ToUnityV2());
}
}
/// <summary>
/// Modeled after shifted quadrant II of unit circle
/// </summary>
public static float EaseOutCircular(this float This)
{
return(Math.Sqrt((2 - This) * This));
}
/// <summary> /// Modeled after shifted quadrant IV of unit circle /// </summary> internal static float EaseInCirc(float p) => 1 - Math.Sqrt(1 - (p * p));
/// <summary> /// Modeled after shifted quadrant II of unit circle /// </summary> internal static float EaseOutCirc(float p) => Math.Sqrt((2 - p) * p);
public static bool Intersect(CylinderXCollider src, SphereXCollider dst, out XContact?contact)
{
var n = dst.Position - src.Position;
Vector3 closest = n;
var extents = src.bounds.Extents;
closest.x = Mathf.Clamp(closest.x, -extents.x, extents.x);
closest.y = Mathf.Clamp(closest.y, -extents.y, extents.y);
closest.z = Mathf.Clamp(closest.z, -extents.z, extents.z);
var v = new Vector2(closest.x, closest.z);
if (v.sqrMagnitude > src.Radius * src.Radius)
{
v = v.normalized * src.Radius;
closest.x = v.x;
closest.z = v.y;
}
bool inside = false;
if (n == closest)
{
inside = true;
// 往上下移动的最短距离
var dist1 = extents.y - Mathf.Abs(closest.y);
// 往水平四周移动的最短距离
var dist2 = src.Radius - v.magnitude;
if (dist1 < dist2)
{
closest.y = closest.y > 0 ? extents.y : -extents.y;
}
else
{
v = v.normalized * src.Radius;
closest.x = v.x;
closest.z = v.y;
}
}
var dir = n - closest;
var sqrDist = dir.sqrMagnitude;
var space = dst.Radius;
var sqrSpace = space * space;
if (sqrDist < sqrSpace || inside)
{
var dist = Mathf.Sqrt(sqrDist);
var normal = dir.normalized;
var penetration = space - dist;
if (inside)
{
normal = -normal;
penetration = space + dist;
}
if (normal == Vector3.zero)
{
normal = Vector3.up;
}
contact = new XContact(src, dst, normal, penetration);
return(true);
}
contact = null;
return(false);
}
public static bool Intersect(CubeXCollider src, SphereXCollider dst, out XContact?contact)
{
// 反向旋转sphere的位置，使得可以在cube的坐标系下进行碰撞判断
var extents = src.Size * 0.5f;
var invQ = Quaternion.Inverse(src.Quaternion);
var invP = invQ * (dst.Position - src.Position);
// 以下所有操作都是在cube的坐标系下，随后的实际方向需要进行坐标系转换
var n = invP;
var closest = n;
closest.x = Mathf.Clamp(closest.x, -extents.x, extents.x);
closest.y = Mathf.Clamp(closest.y, -extents.y, extents.y);
closest.z = Mathf.Clamp(closest.z, -extents.z, extents.z);
var inside = false;
if (n == closest)
{
inside = true;
var disX = extents.x - Mathf.Abs(n.x);
var disY = extents.y - Mathf.Abs(n.y);
var disZ = extents.z - Mathf.Abs(n.z);
//找到最近的一个面
if (disX < disY && disX < disZ)
{
// 沿X轴
if (n.x > 0)
{
closest.x = extents.x;
}
else
{
closest.x = -extents.x;
}
}
else if (disY < disX && disY < disZ)
{
// 沿Y轴
if (n.y > 0)
{
closest.y = extents.y;
}
else
{
closest.y = -extents.y;
}
}
else
{
// 沿Z轴
if (n.z > 0)
{
closest.z = extents.z;
}
else
{
closest.z = -extents.z;
}
}
}
var dir = n - closest;
var sqrDist = dir.sqrMagnitude;
var space = dst.Radius;
var sqrSpace = space * space;
if (sqrDist < sqrSpace || inside)
{
var dist = Mathf.Sqrt(sqrDist);
var normal = (src.Quaternion * dir).normalized;
var penetration = space - dist;
if (inside)
{
normal = -normal;
penetration = space + dist;
}
if (normal == Vector3.zero)
{
normal = Vector3.up;
}
contact = new XContact(src, dst, normal, penetration);
return(true);
}
contact = null;
return(false);
}
/// <summary>
/// Modeled after shifted quadrant II of unit circle
/// </summary>
static internal float easeOutCirc(float p)
{
return(Math.Sqrt((2 - p) * p));
}
/// <summary>
/// Modeled after shifted quadrant IV of unit circle
/// </summary>
public static float CircularEaseIn(float p)
{
return(1 - Math.Sqrt(1 - (p * p)));
}
/// <summary>
/// Modeled after shifted quadrant II of unit circle
/// </summary>
public static float CircularEaseOut(float p)
{
return(Math.Sqrt((2 - p) * p));
}
/// <summary>
/// Modeled after shifted quadrant IV of unit circle
/// </summary>
static internal float easeInCirc(float p)
{
return(1 - Math.Sqrt(1 - (p * p)));
}
