Arc calculations

library CMMath

    //: Some important functions for calculating new coordinates
    //:     h = maximum height
    //:     d = distance (absolute from start to end)
    //:     x = current distance
    //:
    //: Note: Thanks for moyack for this function
    private function ParabolaZ takes real h, real d, real x returns real
      return (4 * h / d) * (d - x) * (x / d)
    endfunction
    
    struct Arc
        private 3DLoc   start   = 0     //: Startlocation
        private 3DLoc   end     = 0     //: Endlocation
        private 3DLoc   cur     = 0     //: Current location
        
        private real    f       = 0.    //: Facing of the points together
        private real    mdist   = 0.    //: Maximum distance (to the target)
        private real    cdist   = 0.    //: Current distance (to the target)
        
        private real    vArc    = 0.    //: Horizontal Arc
        private real    hArc    = 0.    //: Vertical Arc
        
        //: Method to create an arc movement object.
        //: Parameter:
        //:     startLoc:   Object with the X,Y &Z coordinates of the startpoint
        //:     endLoc:     Object with the X,Y &Z coordinates of the endpoint
        //:     hArc:       Horizontal arc value
        //:     vArc:       Vertical arc value
        public static method create takes 3DLoc startLoc, 3DLoc endLoc, real hArc, real vArc returns thistype
            local thistype this = thistype.allocate()
            
            if startLoc == 0 or endLoc == 0 then
                call BJDebugMsg(SCOPE_PREFIX + "ERROR: Start and/or target loc objects are null.")
                return 0
            endif
            
            set .start  = startLoc
            set .end    = endLoc
            set .cur    = 3DLoc.create(startLoc.x, startLoc.x, startLoc.z)
            set .mdist  = .start.distToLoc(.end)
            set .cdist  = .start.distToLoc(.cur)
            set .f      = .start.angleToLoc(.end)
            
            return this
        endmethod
        
        //: Update the current height
        public method vArc takes nothing returns nothing
            local real v = 0.
            
            if .cdist < .mdist / 2. then
                set v = vArc * .cdist
            else
                set v = vArc * .mdist -.cdist
            endif
            
            set .cur.z = .cur.z + v
        endmethod
        
        //: Update the current position
        public method hArc takes nothing returns nothing
            local real h = 0.
            
            if .cdist < .mdist / 2. then
                set h = .hArc * .cdist
            else
                set h = .hArc * .mdist -.cdist
            endif
            
            set .cur.x = .cur.x + h
            set .cur.y = .cur.y + h
        endmethod
        
        //: Force the arc methods to run an update
        public method update takes real dist returns thistype
            set .d = .d + dist
            
            if d. < 0 then
                set .d = 0.
            endif
            
            call .cur.destroy()
            set .cur = .start.clone()
            call .cur.move(dist, .f)
            
            call .vArc()
            call .hArc()
            
            return this
        endmethod
        
        //: Get a new 3DLoc object with the current location.
        public method getLoc takes nothing returns 3DLoc
            return .cur.clone()
        endmethod
    endstruct
    
    struct 3DLoc
        real x
        real y
        real z
        
        public static method create takes real x, real y, real z returns thistype
            local thistype this = thistype.allocate()
            
            set .x = x
            set .y = y
            set .z = z
            
            return this
        endmethod
        
        public method distToLoc takes thistype loc returns real
            local real dx = loc.x - .x
            local real dy = loc.y - .y
            
            return SquareRoot(dx * dx + dy * dy)
        endmethod
        
        public method angleToLoc takes thistype loc returns real
            return Atan2(loc.y - .y, loc.x - .x)
        endmethod
        
        public method move takes real d, real r returns thistype
            set .x = .x + d * Cos(r)
            set .y = .y + d * Sin(r)
            
            return this
        endmethod
        
        public method clone takes nothing returns thistype
            local thistype clone = thistype.allocate()
            
            set clone.x = .x
            set clone.y = .y
            set clone.z = .z
            
            return clone
        endmethod
    endstruct
    
endlibrary