Attribute VB_Name = "PlaneMath"

Option Explicit

'Invariant: |Normal| = 1
Public Type Plane
    Point As Vector3
    Normal As Vector3
End Type

Public Type PlaneIntersectionResult
    Collided As Boolean
    PointOfImpact As Vector3
    SurfaceNormal As Vector3
End Type

Public Function MakePlane(Point As Vector3, Normal As Vector3) As Plane
    MakePlane.Point = Point
    MakePlane.Normal = Normal
End Function

Public Function FlipPlane(P As Plane) As Plane
    FlipPlane.Point = P.Point
    FlipPlane.Normal = NegVector3(P.Normal)
End Function

'Negative distance if inside
Public Function PointToSurfaceDistance(Point As Vector3, Surface As Plane) As Double
    PointToSurfaceDistance = DotProduct(SubVector3(Point, Surface.Point), Surface.Normal)
End Function

Public Function LineToPlaneIntersection(Line As Line3, Surface As Plane) As PlaneIntersectionResult
    Dim StartDist As Double
    Dim EndDist As Double
    StartDist = PointToSurfaceDistance(Line.StartPoint, Surface)
    EndDist = PointToSurfaceDistance(Line.EndPoint, Surface)
    If (StartDist < 0) Xor (EndDist < 0) Then
        LineToPlaneIntersection.Collided = True
        LineToPlaneIntersection.PointOfImpact = LerpVector3(Line.StartPoint, Line.EndPoint, StartDist / (StartDist - EndDist))
        LineToPlaneIntersection.SurfaceNormal = Surface.Normal
    Else
        LineToPlaneIntersection.Collided = False
    End If
End Function

'The result is normalized and orthogonal so that it's transpose equals it's inverse.
Public Function MakeAxisSystem_Orthogonalized(Forward As Vector3, Up As Vector3) As Matrix3
    Dim Forward_Normalized As Vector3
    Forward_Normalized = NormalVector3(Forward)
    MakeAxisSystem_Orthogonalized.ZAxis = Forward_Normalized
    MakeAxisSystem_Orthogonalized.XAxis = NormalVector3(CrossProduct(NormalVector3(Up), Forward_Normalized))
    MakeAxisSystem_Orthogonalized.YAxis = NormalVector3(CrossProduct(Forward_Normalized, MakeAxisSystem_Orthogonalized.XAxis))
End Function

'The result is normalized and orthogonal so that it's transpose equals it's inverse.
Public Function MakeAxisSystem_Directed(Direction As Vector3) As Matrix3
    If Abs(Direction.Y) > 0.9 Then
        MakeAxisSystem_Directed = MakeAxisSystem_Orthogonalized(Direction, MakeVector3(0, 0, 1))
    Else
        MakeAxisSystem_Directed = MakeAxisSystem_Orthogonalized(Direction, MakeVector3(0, 1, 0))
    End If
End Function

'Use the result as the PlaneSystem argument to ProjectPointToPlaneSystem for multiple points with the same plane
Public Function GetPlaneAxisSystem(P As Plane) As Matrix3
    GetPlaneAxisSystem = MakeAxisSystem_Directed(P.Normal)
End Function

'Get a point relative to a plane's coordinate system
Public Function ProjectPointToPlaneSystem(Point As Vector3, PlaneCenter As Vector3, PlaneSystem As Matrix3) As Vector3
    ProjectPointToPlaneSystem = MulVecTransposedMat3(SubVector3(Point, PlaneCenter), PlaneSystem)
End Function

'Does the same as ProjectPointToPlaneSystem but easier
Public Function ProjectPointToPlane(Point As Vector3, ReferencePlane As Plane) As Vector3
    ProjectPointToPlane = ProjectPointToPlaneSystem(Point, ReferencePlane.Point, GetPlaneAxisSystem(ReferencePlane))
End Function
