// Create a principalSpace3D and PrincipalTrajectory from a given Trajectory3D
public static PrincipalSpace3D Create(
Trajectory3D trajectory3D, // trajectory in world space
out PrincipalTrajectory principalTrajectory) // in principal space
{
PrincipalProjectile principalProjectile;
// The following position should always lie on the principal plane as well
Vector3 worldP1 = trajectory3D.p0 + trajectory3D.v0;
PrincipalSpace3D principalSpace3D = Create(trajectory3D, trajectory3D.p0,
worldP1, out principalProjectile);
Vector2 principalVelocity = principalSpace3D.ToPrincipalVelocity(trajectory3D.v0);
principalTrajectory = new PrincipalTrajectory(principalProjectile, principalVelocity);
return principalSpace3D;
}
// Find a trajectory for which the trajectory's y is at least as high as any
// of the vertices remaining in _edgesOnPrincipalPlane.
private float GetTimeToTargetOnIntersection(
PrincipalSpace3D principalSpace3D,
PrincipalTrajectory principalTrajectory,
Vector2 principalTargetPosition)
{
for (int index = 0; index < _edgesOnPrincipalPlane.vertexCount; index += 2)
{
if (_edgeClassifier.CrossesXBound(
_edgesOnPrincipalPlane.vertices[index],
_edgesOnPrincipalPlane.vertices[index + 1]))
{
return 0;
}
}
float timeToTarget = 0.0f;
for (int index = 0; index < _edgesOnPrincipalPlane.vertexCount; index += 2)
{
Vector2 vertex = _edgesOnPrincipalPlane.vertices[index];
if (principalTrajectory.PositionYAtX(vertex.x) < vertex.y)
{
/*
Debug.DrawLine(principalSpace3D.p0,
principalSpace3D.p0 + principalSpace3D.xAxis * vertex.x + principalSpace3D.yAxis * vertex.y,
Color.grey);
*/
timeToTarget = PrincipalTimePlanners.GetTimeToTargetRGivenIntermediatePositionQ(
principalTrajectory,
principalTargetPosition,
vertex);
principalTrajectory.v0 =
principalTrajectory.GetInitialVelocityGivenRelativeTargetAndTime(
principalTargetPosition, timeToTarget);
}
}
return timeToTarget;
}
public float GetTimeToTargetOverTriangleSoup(
PrincipalSpace3D principalSpace3D,
PrincipalTrajectory principalTrajectory,
Vector2 principalTargetPosition,
Vector3[] principalVertices,
int[] indexTriplets)
{
int numTriangles = indexTriplets.Length / 3;
bool foundIntersection = false;
_edgesOnPrincipalPlane.Initialize(numTriangles);
for (int index = 0; index < indexTriplets.Length; index += 3)
{
// Get the vertices of the current triangle
Vector3 v0 = principalVertices[indexTriplets[index]];
Vector3 v1 = principalVertices[indexTriplets[index + 1]];
Vector3 v2 = principalVertices[indexTriplets[index + 2]];
// Test if triangle intersects the principal plane
if (_edgesOnPrincipalPlane.TryAddTriangle(v0, v1, v2))
{
bool withinXRange;
Debug.DrawLine(principalSpace3D.p0 +
principalSpace3D.xAxis * _edgesOnPrincipalPlane.vertices[_edgesOnPrincipalPlane.vertexCount - 2].x +
principalSpace3D.yAxis * _edgesOnPrincipalPlane.vertices[_edgesOnPrincipalPlane.vertexCount - 2].y,
principalSpace3D.p0 +
principalSpace3D.xAxis * _edgesOnPrincipalPlane.vertices[_edgesOnPrincipalPlane.vertexCount - 1].x +
principalSpace3D.yAxis * _edgesOnPrincipalPlane.vertices[_edgesOnPrincipalPlane.vertexCount - 1].y,
_edgeClassifier.IntersectsTrajectory(
_edgesOnPrincipalPlane.vertices[_edgesOnPrincipalPlane.vertexCount - 2],
_edgesOnPrincipalPlane.vertices[_edgesOnPrincipalPlane.vertexCount - 1],
out withinXRange) ? Color.red : ((v1.z - v0.z) * (v2.x - v0.x) - (v1.x - v0.x) * (v2.z - v0.z) > 0 ? Color.blue : Color.green));
// Test if the triangle intersects the trajectory
if (_edgeClassifier.IntersectsTrajectory(
_edgesOnPrincipalPlane.vertices[_edgesOnPrincipalPlane.vertexCount - 2],
_edgesOnPrincipalPlane.vertices[_edgesOnPrincipalPlane.vertexCount - 1],
out withinXRange))
{
foundIntersection = true;
}
// Discard all segments for down-facing triangles and those
// segments that are outside the trajectory's valid x range.
// What remains is kept around for the case a trajectory over
// the obstacles needs to be calculated later on.
bool facesDown = (v1.z - v0.z) * (v2.x - v0.x) - (v1.x - v0.x) * (v2.z - v0.z) < 0;
if (!withinXRange || facesDown)
{
_edgesOnPrincipalPlane.RemoveLastAddedEdge();
}
}
}
if (foundIntersection)
{
return GetTimeToTargetOnIntersection(principalSpace3D, principalTrajectory, principalTargetPosition);
}
return -1;
}
public bool IntersectsPrincipalRectangle(
PrincipalSpace3D principalSpace3D,
PrincipalTrajectory principalTrajectory,
Vector2 principalTargetPosition,
Matrix4x4 localToWorldMatrix,
Vector3 boundsCenter,
Vector3 boundsExtents,
float marginDistance,
ref Matrix4x4 localToPrincipalMatrix)
{
_localToWorldMatrix = localToWorldMatrix;
Vector3 axisZ = Vector3.Cross(principalSpace3D.xAxis, principalSpace3D.yAxis);
// Get the non-uniform scale of the matrix based on the bounding box size
// and marginDistance
Vector3 scale = GetMarginMatrixScale(boundsExtents, marginDistance);
// Get the data necessary to get the z component in principal space
// of any local vertex (not considering translation yet).
localToPrincipalMatrix.m20 = DotMatrixColumn0(axisZ) * scale.x;
localToPrincipalMatrix.m21 = DotMatrixColumn1(axisZ) * scale.y;
localToPrincipalMatrix.m22 = DotMatrixColumn2(axisZ) * scale.z;
Vector3 relBoundsCenter = _localToWorldMatrix.MultiplyPoint(boundsCenter) - principalSpace3D.p0;
float principalBoundsCenterZ = Vector3.Dot(relBoundsCenter, axisZ);
// Get the bounding box size projected onto the principal z axis
float extentsZ = Mathf.Abs(localToPrincipalMatrix.m20) * boundsExtents.x +
Mathf.Abs(localToPrincipalMatrix.m21) * boundsExtents.y +
Mathf.Abs(localToPrincipalMatrix.m22) * boundsExtents.z;
if (Mathf.Abs(principalBoundsCenterZ) > extentsZ)
{
return false; // bounding box doens't intersect the principal plane
}
// Get the data necessary to get the x component in principal space
// of any local vertex (not considering translation yet).
localToPrincipalMatrix.m00 = DotMatrixColumn0(principalSpace3D.xAxis) * scale.x;
localToPrincipalMatrix.m01 = DotMatrixColumn1(principalSpace3D.xAxis) * scale.y;
localToPrincipalMatrix.m02 = DotMatrixColumn2(principalSpace3D.xAxis) * scale.z;
// Test the bounding box projected on the x axis of the principal space
// against the initial (i.e. min) and target (i.e. max) x interval.
float principalBoundsCenterX = Vector3.Dot(relBoundsCenter, principalSpace3D.xAxis);
float extentsX = Mathf.Abs(localToPrincipalMatrix.m00) * boundsExtents.x +
Mathf.Abs(localToPrincipalMatrix.m01) * boundsExtents.y +
Mathf.Abs(localToPrincipalMatrix.m02) * boundsExtents.z;
if (principalBoundsCenterX + extentsX < 0 ||
principalBoundsCenterX - extentsX > principalTargetPosition.x)
{
return false;
}
// Get the data necessary to get the y component in principal space
// of any local vertex (not considering translation yet).
localToPrincipalMatrix.m10 = DotMatrixColumn0(principalSpace3D.yAxis) * scale.x;
localToPrincipalMatrix.m11 = DotMatrixColumn1(principalSpace3D.yAxis) * scale.y;
localToPrincipalMatrix.m12 = DotMatrixColumn2(principalSpace3D.yAxis) * scale.z;
// Test the bounding box projected on the y axis of the principal space
// against the min and max y interval.
float principalBoundsCenterY = Vector3.Dot(relBoundsCenter, principalSpace3D.yAxis);
float extentsY = Mathf.Abs(localToPrincipalMatrix.m10) * boundsExtents.x +
Mathf.Abs(localToPrincipalMatrix.m11) * boundsExtents.y +
Mathf.Abs(localToPrincipalMatrix.m12) * boundsExtents.z;
if (principalBoundsCenterY + extentsY < Mathf.Min(0.0f, principalTargetPosition.y) ||
principalBoundsCenterY - extentsY > principalTrajectory.PositionAtTime(
principalTrajectory.GetTimeAtMaximumHeight()).y)
{
return false;
}
// Get the data necessary to do the translation part from local to
// principal space
Vector3 delta = new Vector3(_localToWorldMatrix.m03 - principalSpace3D.p0.x,
_localToWorldMatrix.m13 - principalSpace3D.p0.y,
_localToWorldMatrix.m23 - principalSpace3D.p0.z);
localToPrincipalMatrix.m03 = Vector3.Dot(delta, principalSpace3D.xAxis);
localToPrincipalMatrix.m13 = Vector3.Dot(delta, principalSpace3D.yAxis);
localToPrincipalMatrix.m23 = Vector3.Dot(delta, axisZ);
// No projection components
localToPrincipalMatrix.m30 = 0;
localToPrincipalMatrix.m31 = 0;
localToPrincipalMatrix.m32 = 0;
localToPrincipalMatrix.m33 = 1;
return true;
}
public void SetupTrajectory(PrincipalSpace3D principalSpace3D,
PrincipalTrajectory principalTrajectory,
Vector3 principalTargetPosition,
float marginDistance)
{
_principalSpace3D = principalSpace3D;
_principalTrajectory = principalTrajectory;
_principalTargetPosition = principalTargetPosition;
_marginDistance = marginDistance;
_triangleOverTriangleSoupHelper.SetupTrajectory(principalTrajectory, principalTargetPosition);
}