public static VInt3 NearestPointStrict(ref VInt3 lineStart, ref VInt3 lineEnd, ref VInt3 point)
{
VInt3 rhs = lineEnd - lineStart;
long sqrMagnitudeLong = rhs.sqrMagnitudeLong;
if (sqrMagnitudeLong == 0)
{
return(lineStart);
}
long m = IntMath.Clamp(VInt3.DotLong(point - lineStart, rhs), 0L, sqrMagnitudeLong);
return(IntMath.Divide(rhs, m, sqrMagnitudeLong) + lineStart);
}
public static VFactor NearestPointFactor(ref VInt3 lineStart, ref VInt3 lineEnd, ref VInt3 point)
{
VInt3 rhs = lineEnd - lineStart;
long sqrMagnitudeLong = rhs.sqrMagnitudeLong;
VFactor zero = VFactor.zero;
zero.nom = VInt3.DotLong(point - lineStart, rhs);
if (sqrMagnitudeLong != 0)
{
zero.den = sqrMagnitudeLong;
}
return(zero);
}
/** Returns the closest point on the segment.
* The segment is NOT treated as infinite.
* \see ClosestPointOnLine
* \see ClosestPointOnSegmentXZ
*/
public static VInt3 ClosestPointOnSegment(VInt3 lineStart, VInt3 lineEnd, VInt3 point)
{
var dir = lineEnd - lineStart;
long sqrMagn = dir.sqrMagnitudeLong;
if (sqrMagn <= 1)
{
return(lineStart);
}
long factor = VInt3.DotLong(point - lineStart, dir) / sqrMagn;
return(lineStart + dir * IntMath.Clamp(0, 1000, factor));
}
public static VInt3 NearestPointStrict(ref VInt3 lineStart, ref VInt3 lineEnd, ref VInt3 point)
{
VInt3 vInt = lineEnd - lineStart;
long sqrMagnitudeLong = vInt.sqrMagnitudeLong;
if (sqrMagnitudeLong == 0L)
{
return(lineStart);
}
long num = VInt3.DotLong(point - lineStart, vInt);
num = IntMath.Clamp(num, 0L, sqrMagnitudeLong);
return(IntMath.Divide(vInt, num, sqrMagnitudeLong) + lineStart);
}
public void Apply(bool forceNewCheck)
{
//TODO
//This function assumes that connections from the n1,n2 nodes never need to be removed in the future (e.g because the nodes move or something)
NNConstraint nn = NNConstraint.None;
nn.distanceXZ = true;
int graph = (int)startNode.GraphIndex;
//Search all graphs but the one which start and end nodes are on
nn.graphMask = ~(1 << graph);
bool same = true;
{
//Good Game
//var info = AstarPath.active.GetNearest(StartTransform.position, nn);
var info = AstarPath.active.GetNearest((VInt3)StartTransform.position, nn);
same &= info.node == connectedNode1 && info.node != null;
connectedNode1 = info.node as MeshNode;
clamped1 = info.position;
if (connectedNode1 != null)
{
Debug.DrawRay((Vector3)connectedNode1.position, Vector3.up * 5, Color.red);
}
}
{
//Good Game
//var info = AstarPath.active.GetNearest(EndTransform.position, nn);
var info = AstarPath.active.GetNearest((VInt3)EndTransform.position, nn);
same &= info.node == connectedNode2 && info.node != null;
connectedNode2 = info.node as MeshNode;
clamped2 = info.position;
if (connectedNode2 != null)
{
Debug.DrawRay((Vector3)connectedNode2.position, Vector3.up * 5, Color.cyan);
}
}
if (connectedNode2 == null || connectedNode1 == null)
{
return;
}
startNode.SetPosition((VInt3)StartTransform.position);
endNode.SetPosition((VInt3)EndTransform.position);
if (same && !forceNewCheck)
{
return;
}
RemoveConnections(startNode);
RemoveConnections(endNode);
uint cost = (uint)Mathf.RoundToInt(((VInt3)(StartTransform.position - EndTransform.position)).costMagnitude * costFactor);
startNode.AddConnection(endNode, cost);
endNode.AddConnection(startNode, cost);
VInt3 dir = connectedNode2.position - connectedNode1.position;
for (int a = 0; a < connectedNode1.GetVertexCount(); a++)
{
VInt3 va1 = connectedNode1.GetVertex(a);
VInt3 va2 = connectedNode1.GetVertex((a + 1) % connectedNode1.GetVertexCount());
if (VInt3.DotLong((va2 - va1).Normal2D(), dir) > 0)
{
continue;
}
for (int b = 0; b < connectedNode2.GetVertexCount(); b++)
{
VInt3 vb1 = connectedNode2.GetVertex(b);
VInt3 vb2 = connectedNode2.GetVertex((b + 1) % connectedNode2.GetVertexCount());
if (VInt3.DotLong((vb2 - vb1).Normal2D(), dir) < 0)
{
continue;
}
if (VInt3.Angle((vb2 - vb1), (va2 - va1)) > (170.0 / 360.0f) * Mathf.PI * 2)
{
float t1 = 0;
float t2 = 1;
t2 = System.Math.Min(t2, VectorMath.ClosestPointOnLineFactor(va1, va2, vb1));
t1 = System.Math.Max(t1, VectorMath.ClosestPointOnLineFactor(va1, va2, vb2));
if (t2 < t1)
{
Debug.LogError("Something went wrong! " + t1 + " " + t2 + " " + va1 + " " + va2 + " " + vb1 + " " + vb2 + "\nTODO, how can this happen?");
}
else
{
//Good Game
/*Vector3 pa = (Vector3)(va2-va1)*t1 + (Vector3)va1;
* Vector3 pb = (Vector3)(va2-va1)*t2 + (Vector3)va1;*/
VInt3 pa = (va2 - va1) * t1 + va1;
VInt3 pb = (va2 - va1) * t2 + va1;
startNode.portalA = pa;
startNode.portalB = pb;
endNode.portalA = pb;
endNode.portalB = pa;
//Add connections between nodes, or replace old connections if existing
//Good Game
/*connectedNode1.AddConnection(startNode, (uint)Mathf.RoundToInt(((VInt3)(clamped1 - StartTransform.position)).costMagnitude*costFactor));
* connectedNode2.AddConnection(endNode, (uint)Mathf.RoundToInt(((VInt3)(clamped2 - EndTransform.position)).costMagnitude*costFactor));
*
* startNode.AddConnection(connectedNode1, (uint)Mathf.RoundToInt(((VInt3)(clamped1 - StartTransform.position)).costMagnitude*costFactor));
* endNode.AddConnection(connectedNode2, (uint)Mathf.RoundToInt(((VInt3)(clamped2 - EndTransform.position)).costMagnitude*costFactor));*/
connectedNode1.AddConnection(startNode, (uint)Mathf.RoundToInt(((clamped1 - (VInt3)StartTransform.position)).costMagnitude * costFactor));
connectedNode2.AddConnection(endNode, (uint)Mathf.RoundToInt(((clamped2 - (VInt3)EndTransform.position)).costMagnitude * costFactor));
startNode.AddConnection(connectedNode1, (uint)Mathf.RoundToInt(((clamped1 - (VInt3)StartTransform.position)).costMagnitude * costFactor));
endNode.AddConnection(connectedNode2, (uint)Mathf.RoundToInt(((clamped2 - (VInt3)EndTransform.position)).costMagnitude * costFactor));
return;
}
}
}
}
}