/*
* Returns the circle center position of the new path that starts at given intersection and uses given curve and radius.
* */
private Vector3 calculateCircleCenterOfPath(VirtualIntersection startIntersection, Curve curve, float radius)
{
int newPathIndex = this.getPathIndex(startIntersection.getJoint(), curve);
Vector3 directionToCurveCircleCenter = (curve.getCircleCenter() - startIntersection.getJoint().getWalkingStartPosition(newPathIndex)).normalized;
Vector3 directionToPathCircleCenter = new Vector3();
// Check which paths are connected to the intersection and pick one (first) for calculation.
// New path circle center can be calculated by using the direction to the curve circle center and rotating it.
for (int i = 0; i < 4; i++)
{
if (startIntersection.getPath(i) != null)
{
Vector3 directionToStartWalkingPosOnPath = (startIntersection.getWalkingStartPosition(i) - startIntersection.getPosition()).normalized;
Vector3 directionToStartWalkingPosOnCurve = (startIntersection.getJoint().getWalkingStartPosition(i) - startIntersection.getJoint().getPosition()).normalized;
float angle = -angle360(directionToStartWalkingPosOnPath, directionToStartWalkingPosOnCurve);
directionToPathCircleCenter = Quaternion.AngleAxis(angle, Vector3.up) * directionToCurveCircleCenter;
break;
}
if (i == 3)
{
// If no case was chosen this is the start joint. Then,
// we can use the curve circle center for calculating the path circle center without rotating.
directionToPathCircleCenter = directionToCurveCircleCenter;
}
}
Vector3 result = startIntersection.getWalkingStartPosition(newPathIndex) + directionToPathCircleCenter.normalized * radius;
return(result);
}
/*
* Calculates walking start positions for new intersection
* */
private void calculateWalkingStartPositions(int pathIndex, JointPoint endJointPoint, VirtualIntersection endIntersection, Vector3 circleCenter, Vector3 directionToEndPath)
{
endIntersection.setWalkingStartPosition(pathIndex, circleCenter + directionToEndPath);
Vector3 directionToCurve0 = endJointPoint.getWalkingStartPosition(0) - endJointPoint.getPosition();
Vector3 directionToCurve1 = endJointPoint.getWalkingStartPosition(1) - endJointPoint.getPosition();
Vector3 directionToCurve2 = endJointPoint.getWalkingStartPosition(2) - endJointPoint.getPosition();
Vector3 directionToCurve3 = endJointPoint.getWalkingStartPosition(3) - endJointPoint.getPosition();
float angleBetweenCurves01 = angle360(directionToCurve0, directionToCurve1);
float angleBetweenCurves02 = angle360(directionToCurve0, directionToCurve2);
float angleBetweenCurves03 = angle360(directionToCurve0, directionToCurve3);
float angleBetweenCurves12 = angle360(directionToCurve1, directionToCurve2);
float angleBetweenCurves13 = angle360(directionToCurve1, directionToCurve3);
float angleBetweenCurves10 = angle360(directionToCurve1, directionToCurve0);
float angleBetweenCurves23 = angle360(directionToCurve2, directionToCurve3);
float angleBetweenCurves20 = angle360(directionToCurve2, directionToCurve0);
float angleBetweenCurves21 = angle360(directionToCurve2, directionToCurve1);
float angleBetweenCurves30 = angle360(directionToCurve3, directionToCurve0);
float angleBetweenCurves31 = angle360(directionToCurve3, directionToCurve1);
float angleBetweenCurves32 = angle360(directionToCurve3, directionToCurve2);
Vector3 directionToFirstStartPosition = (circleCenter + directionToEndPath) - endIntersection.getPosition();
Vector3 directionToOtherStartPosition = new Vector3();
switch (pathIndex)
{
case 0:
endIntersection.setWalkingStartPosition(0, circleCenter + directionToEndPath);
directionToOtherStartPosition = Quaternion.AngleAxis(angleBetweenCurves01, Vector3.up) * directionToFirstStartPosition;
endIntersection.setWalkingStartPosition(1, endIntersection.getPosition() + directionToOtherStartPosition);
directionToOtherStartPosition = Quaternion.AngleAxis(angleBetweenCurves02, Vector3.up) * directionToFirstStartPosition;
endIntersection.setWalkingStartPosition(2, endIntersection.getPosition() + directionToOtherStartPosition);
directionToOtherStartPosition = Quaternion.AngleAxis(angleBetweenCurves03, Vector3.up) * directionToFirstStartPosition;
endIntersection.setWalkingStartPosition(3, endIntersection.getPosition() + directionToOtherStartPosition);
break;
case 1:
endIntersection.setWalkingStartPosition(1, circleCenter + directionToEndPath);
directionToOtherStartPosition = Quaternion.AngleAxis(angleBetweenCurves12, Vector3.up) * directionToFirstStartPosition;
endIntersection.setWalkingStartPosition(2, endIntersection.getPosition() + directionToOtherStartPosition);
directionToOtherStartPosition = Quaternion.AngleAxis(angleBetweenCurves13, Vector3.up) * directionToFirstStartPosition;
endIntersection.setWalkingStartPosition(3, endIntersection.getPosition() + directionToOtherStartPosition);
directionToOtherStartPosition = Quaternion.AngleAxis(angleBetweenCurves10, Vector3.up) * directionToFirstStartPosition;
endIntersection.setWalkingStartPosition(0, endIntersection.getPosition() + directionToOtherStartPosition);
break;
case 2:
endIntersection.setWalkingStartPosition(2, circleCenter + directionToEndPath);
directionToOtherStartPosition = Quaternion.AngleAxis(angleBetweenCurves23, Vector3.up) * directionToFirstStartPosition;
endIntersection.setWalkingStartPosition(3, endIntersection.getPosition() + directionToOtherStartPosition);
directionToOtherStartPosition = Quaternion.AngleAxis(angleBetweenCurves20, Vector3.up) * directionToFirstStartPosition;
endIntersection.setWalkingStartPosition(0, endIntersection.getPosition() + directionToOtherStartPosition);
directionToOtherStartPosition = Quaternion.AngleAxis(angleBetweenCurves21, Vector3.up) * directionToFirstStartPosition;
endIntersection.setWalkingStartPosition(1, endIntersection.getPosition() + directionToOtherStartPosition);
break;
case 3:
endIntersection.setWalkingStartPosition(3, circleCenter + directionToEndPath);
directionToOtherStartPosition = Quaternion.AngleAxis(angleBetweenCurves30, Vector3.up) * directionToFirstStartPosition;
endIntersection.setWalkingStartPosition(0, endIntersection.getPosition() + directionToOtherStartPosition);
directionToOtherStartPosition = Quaternion.AngleAxis(angleBetweenCurves31, Vector3.up) * directionToFirstStartPosition;
endIntersection.setWalkingStartPosition(1, endIntersection.getPosition() + directionToOtherStartPosition);
directionToOtherStartPosition = Quaternion.AngleAxis(angleBetweenCurves32, Vector3.up) * directionToFirstStartPosition;
endIntersection.setWalkingStartPosition(2, endIntersection.getPosition() + directionToOtherStartPosition);
break;
}
}