// 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);
        }