/*
-----------------------------------------------------------------------------
This source file is part of OpenSpace3D
For the latest info, see http://www.openspace3d.com

Copyright (c) 2012 I-maginer

This program is free software; you can redistribute it and/or modify it under
the terms of the GNU Lesser General Public License as published by the Free Software
Foundation; either version 2 of the License, or (at your option) any later
version.

This program is distributed in the hope that it will be useful, but WITHOUT
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License for more details.

You should have received a copy of the GNU Lesser General Public License along with
this program; if not, write to the Free Software Foundation, Inc., 59 Temple
Place - Suite 330, Boston, MA 02111-1307, USA, or go to
http://www.gnu.org/copyleft/lesser.txt

-----------------------------------------------------------------------------
*/

/* Authors: Bastien Bourineau */
/* Last update: 21/10/2009 */
/* Version: 1.0 */

struct TobjMvto = [
                    OBJM_inst       : PInstance,
                    OBJM_h3d        : SO3_OBJECT,
                    OBJM_desth3d    : SO3_OBJECT,
                    OBJM_iSrcMode   : I,
                    OBJM_iDestMode  : I,
                    OBJM_TCB        : [F F F],
                    OBJM_tDir       : [F F F],
                    OBJM_inipos     : [[F F F] [F F F F]],
                    OBJM_srcpos     : [[F F F] [F F F F] [F F F] F],
                    OBJM_dstpos     : [[F F F] [F F F F] [F F F] F],
                    OBJM_posindex   : I,
                    OBJM_angcoef    : F,
                    OBJM_coef       : F,
                    OBJM_angstep    : F,
                    OBJM_step       : F,
                    OBJM_bezier     : F,
                    OBJM_fps        : I,
                    OBJM_speed      : F,
                    OBJM_rspeed     : F,
                    OBJM_bUseRotate : I,
                    OBJM_state      : I,
                    OBJM_bLocal     : I,
                    OBJM_iTick      : I
                    
                  ] mkTobjMvto;;


// 2t3 - 3t2 + 1
fun H1(coef)=
  (2.0 *. (pow coef 3.0)) -. (3.0 *. (pow coef 2.0)) +. 1.0;;


// -2t3 + 3t2
fun H2(coef)=
  (-. 2.0 *. (pow coef 3.0)) +. (3.0 *. (pow coef 2.0));;


// t3 - 2t2 +t
fun H3(coef)=
  (pow coef 3.0) -. (2.0 *. (pow coef 2.0)) +. coef;;


// t3 - t2
fun H4(coef)=
  (pow coef 3.0) -. (pow coef 2.0);;


fun getInterpolateCurve(vec1, vec2, ang1, ang2, coef)=
  let vec1 -> [x1 y1 z1] in
  let vec2 -> [x2 y2 z2] in
  let ang1 -> [ax1 ay1 az1] in
  let ang2 -> [ax2 ay2 az2] in

  let ((H1 coef) *. x1) +. ((H2 coef) *. x2) +. ((H3 coef) *. ax1) +. ((H4 coef) *. ax2) -> x in
  let ((H1 coef) *. y1) +. ((H2 coef) *. y2) +. ((H3 coef) *. ay1) +. ((H4 coef) *. ay2) -> y in
  let ((H1 coef) *. z1) +. ((H2 coef) *. z2) +. ((H3 coef) *. az1) +. ((H4 coef) *. az2) -> z in
  [x y z]
  ;;


fun getPosTime(pos)=
  let pos -> [[x y z] _ [t c b] ti] in
  ti;;


fun getPosTension(pos)=
  let pos -> [[x y z] _ [t c b] ti] in
  t;;


fun getPosContinuity(pos)=
  let pos -> [[x y z] _ [t c b] ti] in
  c;;


fun getPosBias(pos)=
  let pos -> [[x y z] _ [t c b] ti] in
  b;;


fun getPosX(pos)=
  let pos -> [[x y z] _ [t c b] ti] in
  x;;


fun getPosY(pos)=
  let pos -> [[x y z] _ [t c b] ti] in
  y;;


fun getPosZ(pos)=
  let pos -> [[x y z] _ [t c b] ti] in
  z;;


fun getPosXYZ(pos)=
  let pos -> [[x y z] _ [t c b] ti] in
  [x y z];;
  

/*fun getViewDirection(ovec, oang, tvec, taxis)=
  let normalizeVectorF (subVectorF tvec ovec) -> dir in
  let normalizeVectorF (SO3MathsQuatGetDirection oang taxis) -> src in
  let (SO3MathsGetRotationTo src dir) -> diffq in
  let SO3MathsQuatAdd diffq oang -> nquat in
  (
    nquat;
  );;
  */
fun getViewDirection(ovec, oang, tvec, taxis)=
  let ovec -> [vx vy vz] in
  let tvec -> [dx dy dz] in
  let normalizeVectorF (subVectorF [dx vy dz] ovec) -> dir in
  let normalizeVectorF (SO3MathsQuatGetDirection oang taxis) -> src in
  let SO3MathsQuatToEulerPYR (SO3MathsGetRotationTo src dir) -> [pitch yaw roll] in
  let if ((1.0 +. dotVectorF src dir) <. 0.0001) then PIf *. 2.0 else yaw -> yaw in
  let SO3MathsQuatAdd SO3MathsEulerPYRToQuat [0.0 yaw 0.0] oang -> nquat in
  (
    nquat;
  );;


fun getLocalPoint(obj, tvec)=
  let (SO3ObjectGetParent obj) -> parent in
  if (parent != nil) then
  let divideVectorF [1.0 1.0 1.0] SO3ObjectGetGlobalScale parent -> invscale in
  let SO3ObjectGetGlobalPosition parent -> vec in
  let SO3ObjectGetGlobalOrientation parent -> ang in
  let subVectorF tvec vec -> diff in
  (
    SO3MathsQuatGetDirection quatInverse ang multiplyVectorF invscale diff;
  )
  else
    tvec;;


fun getInterpolateTCB(lpos, t)=
	let sizelist lpos -> size in
	let size - 1 -> sizeminus in
	let 0 -> i in
	let nil -> outvec in
	(
  	while i < size && outvec == nil do
  	(
  	  let nth_list lpos i -> nextpos in
      if t <. (getPosTime nextpos) then
      (
        let if i == 0 then nth_list lpos (sizeminus - 1) else nth_list lpos (i - 1) -> curpos in
        let if i == 0 then nth_list lpos (sizeminus - 2) else if i == 1 then nth_list lpos (sizeminus - 1) else nth_list lpos (i - 2) -> prevpos in
        let if i == sizeminus then nth_list lpos 0 else nth_list lpos (i + 1) -> nextnextpos in  
        
        let ((t -. (getPosTime curpos)) /. ((getPosTime nextpos) -. (getPosTime curpos))) -> coef in
        
        //Update values from TCB
        let (1.0 -. (getPosTension curpos)) -> OneMinusTension in
        let (1.0 -. (getPosContinuity curpos)) -> OneMinusContinuity in
        let (1.0 +. (getPosContinuity curpos)) -> OnePlusContinuity in
        let (1.0 -. (getPosBias curpos)) -> OneMinusBias in
        let (1.0 +. (getPosBias curpos)) -> OnePlusBias in
        
        let 0.5 *. ((OneMinusTension *. OneMinusContinuity *. OneMinusBias *. ((getPosX nextpos) -. (getPosX curpos) )) +. (OneMinusTension *. OnePlusContinuity *. OnePlusBias *. ( (getPosX curpos) -. (getPosX prevpos) ))) -> tancurposx in
        let 0.5 *. ((OneMinusTension *. OneMinusContinuity *. OneMinusBias *. ((getPosY nextpos) -. (getPosY curpos) )) +. (OneMinusTension *. OnePlusContinuity *. OnePlusBias *. ( (getPosY curpos) -. (getPosY prevpos) ))) -> tancurposy in
        let 0.5 *. ((OneMinusTension *. OneMinusContinuity *. OneMinusBias *. ((getPosZ nextpos) -. (getPosZ curpos) )) +. (OneMinusTension *. OnePlusContinuity *. OnePlusBias *. ( (getPosZ curpos) -. (getPosZ prevpos) ))) -> tancurposz in
  
        let 0.5 *. ((OneMinusTension *. OnePlusContinuity *. OneMinusBias *. ((getPosX nextpos) -. (getPosX curpos) )) +. (OneMinusTension *. OneMinusContinuity *. OnePlusBias *. ( (getPosX nextnextpos) -. (getPosX nextpos) ))) -> tannextposx in
        let 0.5 *. ((OneMinusTension *. OnePlusContinuity *. OneMinusBias *. ((getPosY nextpos) -. (getPosY curpos) )) +. (OneMinusTension *. OneMinusContinuity *. OnePlusBias *. ( (getPosY nextnextpos) -. (getPosY nextpos) ))) -> tannextposy in
        let 0.5 *. ((OneMinusTension *. OnePlusContinuity *. OneMinusBias *. ((getPosZ nextpos) -. (getPosZ curpos) )) +. (OneMinusTension *. OneMinusContinuity *. OnePlusBias *. ( (getPosZ nextnextpos) -. (getPosZ nextpos) ))) -> tannextposz in
        
        let getInterpolateCurve (getPosXYZ curpos) (getPosXYZ nextpos) [tancurposx tancurposy tancurposz] [tannextposx tannextposy tannextposz] coef -> [x y z] in
          set outvec = [i [x y z]];
        
        0;
      )
      else nil;
      
      set i = i + 1;
    );
    outvec;
  );;


fun usePhysic(strobj)=
  let SO3SceneNodeGetBody strobj.OBJM_h3d -> body in
  let SO3BodyGetMassMatrix body -> [mass _] in
    if ((strobj.OBJM_iSrcMode != 0) || (body == nil) || !(V3DphysGetState c3dXsession) || (SO3BodyGetFluid body) || (mass <=. 0.0)) then 0 else 1;;


fun cbMoveObj(inst, sessionstr, etime, strobj, cbend)=
  let ((itof strobj.OBJM_fps) *. ((itof (set strobj.OBJM_iTick = strobj.OBJM_iTick + etime)) /. 1000.0)) /. 1000.0 -> rtick in
  let (1000.0 /. (itof SO3WorldGetFPS sessionstr.V3D_session)) -> nbsec in
  let [nbsec nbsec nbsec] -> vtime in
  if (rtick == 0.0) then nil else
  (
    set strobj.OBJM_iTick = 0;
    if (strobj.OBJM_coef >=. 1.0) && (strobj.OBJM_angcoef >=. 1.0) then
    (
      if strobj.OBJM_state == 0 then nil else
      (
        set strobj.OBJM_state = 0;
        setPluginInstanceCbScenePreRenderPhysic strobj.OBJM_inst nil;
        
        let SO3SceneNodeGetBody strobj.OBJM_h3d -> body in
        if !(usePhysic strobj) then nil else
        (
          SO3BodySetVelocity body [0.0 0.0 0.0];
          SO3BodySetOmega body [0.0 0.0 0.0];
        );
        
        // event
        exec cbend with [strobj];
        0;
      );
    )
    else
    (
      let rtick *. strobj.OBJM_step -> nstep in    
        set strobj.OBJM_coef = if (strobj.OBJM_coef +. nstep) >. 1.0 then 1.0 else strobj.OBJM_coef +. nstep;
      
      let rtick *. strobj.OBJM_angstep -> nangstep in
        set strobj.OBJM_angcoef = if (strobj.OBJM_angcoef +. nangstep) >. 1.0 then 1.0 else strobj.OBJM_angcoef +. nangstep;
      
      let getInterpolateTCB strobj.OBJM_srcpos::strobj.OBJM_dstpos::nil strobj.OBJM_coef -> [index tvec] in
      let strobj.OBJM_srcpos -> [spos sang _ _] in
      let strobj.OBJM_dstpos -> [dpos dang _ _] in
      
      //get destination quat to direction
      
      let if strobj.OBJM_bLocal then SO3ObjectGetPosition strobj.OBJM_h3d else SO3ObjectGetGlobalPosition strobj.OBJM_h3d -> ovec in
      let /*if strobj.OBJM_bLocal then SO3ObjectGetOrientation strobj.OBJM_h3d else*/ SO3ObjectGetGlobalOrientation strobj.OBJM_h3d -> oang in
      
      /*
      // use direction for orientation
      let ovec -> [vx vy vz] in
      let tvec -> [dx dy dz] in
      let SO3MathsQuatGetDirection oang [0.0 0.0 (-.1.0)] -> src in
      let subVectorF [dx vy dz] ovec -> dir in
      let SO3MathsGetRotationTo src dir -> qrot in
      let SO3MathsQuatAdd oang qrot -> tang in
      */
      let SO3MathsQuatInterpolate sang dang strobj.OBJM_angcoef 1 -> tang in 
      (
        let SO3SceneNodeGetBody strobj.OBJM_h3d -> body in
        if !(usePhysic strobj) then
        (
          if (strobj.OBJM_bLocal) then
          ( 
            SO3ObjectSetPosition strobj.OBJM_h3d tvec;
            SO3ObjectSetGlobalOrientation strobj.OBJM_h3d tang; //SO3ObjectSetOrientation
          )
          else
          (
            SO3ObjectSetGlobalPosition strobj.OBJM_h3d tvec;
            SO3ObjectSetGlobalOrientation strobj.OBJM_h3d tang;   
          );
          0;
        )
        else
        let SO3ObjectGetGlobalPosition strobj.OBJM_h3d -> novec in
        let SO3ObjectGetParent strobj.OBJM_h3d -> parent in
        let if (parent == nil) then [0.0 0.0 0.0] else SO3ObjectGetGlobalPosition parent -> ppos in
        let if strobj.OBJM_bLocal then addVectorF ppos (SO3ObjectGetDerivedDirectionAxis parent tvec) else tvec -> ntvec in
        let subVectorF ntvec novec -> [vvx vvy vvz] in
        let if vvx == nil || vvy == nil || vvz == nil then [0.0 0.0 0.0] else multiplyVectorF [vvx vvy vvz] vtime -> vel in
        let SO3MathsQuatDiff tang oang -> nq in
        let [(SO3MathsQuatGetPitch nq 1) (SO3MathsQuatGetYaw nq 1) (SO3MathsQuatGetRoll nq 1)] -> [vtx vty vtz] in
        /*
        let if ((absf (SO3MathsRadianToDegree vtx)) >=. 180.0) then (-.vtx +. (SO3MathsDegreeToRadian 180.0)) else vtx -> vtx in 
        let if ((absf (SO3MathsRadianToDegree vty)) >=. 180.0) then (-.vty +. (SO3MathsDegreeToRadian 180.0)) else vty -> vty in
        let if ((absf (SO3MathsRadianToDegree vtz)) >=. 180.0) then (-.vtz +. (SO3MathsDegreeToRadian 180.0)) else vtz -> vtz in
        */
        let if vtx == nil || vty == nil || vtz == nil then [0.0 0.0 0.0] else multiplyVectorF [vtx vty vtz] vtime -> omega in
        (
          SO3BodySetFreeze body 0;
          SO3BodySetVelocity body vel;
          SO3BodySetOmega body omega;
          0;
        );
      );
      0;
    );
  );
  0;;


fun getTCBLength(strobj, speed)=
  let 0.0 -> ndata in
  let 0.0 -> i in
  let 1.0 /. ((itof strobj.OBJM_fps) /. speed) -> stp in
  let nil -> prevpos in
  (
    while i <=. 1.0 do
    (
      let getInterpolateTCB strobj.OBJM_srcpos::strobj.OBJM_dstpos::nil i -> [index tvec] in
      (
        if prevpos == nil || tvec == nil then nil else
          let prevpos -> [xa ya za] in
          let tvec -> [xb yb zb] in
          let sqrt ( (sqr(xa -. xb)) +. (sqr(ya -. yb)) +. (sqr(za -. zb)) ) -> dist in
            set ndata = ndata +. dist;
        
        if tvec == nil then nil else
          set prevpos = tvec;
      );
      set i = i +. stp;
    );
    ndata;
  );;


fun cbObjEnd(strobj)=
  SendPluginEvent strobj.OBJM_inst "End" nil nil;
  0;;


fun cbObjStop(o, from, action, param, reply, strobj)=
  setPluginInstanceCbScenePreRenderPhysic strobj.OBJM_inst nil;  
  set strobj.OBJM_state = 0;
  set strobj.OBJM_iTick = 0;
	0;;


fun cbSetSpeed(o, from, action, param, reply, strobj)=
  if (!strcmp "" (strtrim param)) || (param == nil) then nil else
    set strobj.OBJM_speed = atof param;
	0;;


fun cbObjGoto(o, from, action, param, reply, strobj)=
  let if param == nil then nil else SO3SceneGetObject (V3DgetSession c3dXsession) param -> dstobject in
  let 
    let hd strextr param -> lp in
    let atof hd lp -> px in
    let atof hd tl lp -> py in
    let atof hd tl tl lp -> pz in
      if (px != nil) && (py != nil) && (pz != nil) then 1 else 0
  -> isvec in
  let if dstobject == nil then
      if isvec then
        nil
      else
        strobj.OBJM_desth3d
    else
      dstobject
  -> dstobject in
  (
    if strobj.OBJM_iSrcMode == 0 then nil else
      set strobj.OBJM_h3d = V3DgetCameraByType c3dXsession V3DgetDefaultCamera c3dXsession strobj.OBJM_iSrcMode;
    
    if (isvec) then nil else
    (
      if strobj.OBJM_iDestMode == 0 then nil else
        set dstobject = V3DgetCameraByType c3dXsession V3DgetDefaultCamera c3dXsession strobj.OBJM_iDestMode;
    );
        
    let if strobj.OBJM_bLocal then SO3ObjectGetPosition strobj.OBJM_h3d else SO3ObjectGetGlobalPosition strobj.OBJM_h3d -> vec in
    let /*if strobj.OBJM_bLocal then SO3ObjectGetOrientation strobj.OBJM_h3d else*/ SO3ObjectGetGlobalOrientation strobj.OBJM_h3d -> ang in
    
    let strextr param -> lp in
    let if (dstobject != nil) then 
          SO3ObjectGetGlobalPosition dstobject
        else
          let if (atof (nth_list (hd lp) 0)) == nil then 0.0 else (atof (nth_list (hd lp) 0)) -> px in
          let if (atof (nth_list (hd lp) 1)) == nil then 0.0 else (atof (nth_list (hd lp) 1)) -> py in
          let if (atof (nth_list (hd lp) 2)) == nil then 0.0 else (atof (nth_list (hd lp) 2)) -> pz in
            [px py pz]
    -> dstvec in
    let getLocalPoint strobj.OBJM_h3d dstvec -> dstvec in
    let if (dstobject != nil) then 
          /*if strobj.OBJM_bLocal then SO3ObjectGetOrientation dstobject else*/ SO3ObjectGetGlobalOrientation dstobject
        else if ((atof (nth_list (hd tl lp) 0)) == nil) then
        //get destination quat to direction
          getViewDirection vec ang dstvec strobj.OBJM_tDir
        else
          let if (atof (nth_list (hd tl lp) 0)) == nil then 0.0 else SO3MathsDegreeToRadian (atof (nth_list (hd tl lp) 0)) -> px in
          let if (atof (nth_list (hd tl lp) 1)) == nil then 0.0 else SO3MathsDegreeToRadian (atof (nth_list (hd tl lp) 1)) -> py in
          let if (atof (nth_list (hd tl lp) 2)) == nil then 0.0 else SO3MathsDegreeToRadian (atof (nth_list (hd tl lp) 2)) -> pz in
            SO3MathsEulerXYZToQuat [py px pz]
    -> dstang in
    (
      set strobj.OBJM_srcpos = [vec ang strobj.OBJM_TCB 0.0];
      set strobj.OBJM_dstpos = [dstvec dstang [0.0 0.0 0.0] 1.0];
      
      // calculate the tcb curve distance
      let getTCBLength strobj strobj.OBJM_speed -> tcbdist in
      (
        if tcbdist == 0.0 then 
        (
          set strobj.OBJM_coef = 1.0;
          set strobj.OBJM_angcoef = 1.0;
          set strobj.OBJM_angstep = 1.0;
          set strobj.OBJM_step = 1.0;
        )
        else
        // calculate the step with the angular and tcb curve distance average
        let SO3MathsQuatDiff ang dstang -> qdiff in
        let SO3MathsQuatToEulerXYZ qdiff -> [dx dy dz] in
        let sqrt((sqr dx) +. (sqr dy) +. (sqr dz)) -> angdist in
        let if angdist == 0.0 then 1.0 else angdist -> angdist in
        let if strobj.OBJM_bUseRotate then 
              (1.0 /. ((tcbdist /. 2.0) *. ((itof strobj.OBJM_fps) /. strobj.OBJM_speed)))
            else
              (1.0 /. (((tcbdist +. angdist) /. 2.0) *. ((itof strobj.OBJM_fps) /. strobj.OBJM_speed)))
        -> step in
        let (1.0 /. ((angdist /. 2.0) *. ((itof strobj.OBJM_fps) /. strobj.OBJM_rspeed))) -> rstep in
        (
          set strobj.OBJM_coef = 0.0;
          set strobj.OBJM_angcoef = 0.0;
          set strobj.OBJM_step = step;
          set strobj.OBJM_angstep = if strobj.OBJM_bUseRotate then rstep else step;
        );
      );
    );
    
    set strobj.OBJM_state = 1;
    set strobj.OBJM_iTick = 0;
    
    setPluginInstanceCbScenePreRenderPhysic strobj.OBJM_inst mkfun4 mkfun5 @cbMoveObj @cbObjEnd strobj;
  );
  0;;


fun cbObjTranslate(o, from, action, param, reply, strobj)=
  let if param == nil then nil else SO3SceneGetObject (V3DgetSession c3dXsession) param -> dstobject in
  let 
    let hd strextr param -> lp in
    let atof hd lp -> px in
    let atof hd tl lp -> py in
    let atof hd tl tl lp -> pz in
      if (px != nil) && (py != nil) && (pz != nil) then 1 else 0
  -> isvec in
  let if dstobject == nil then
      if isvec then
        nil
      else
        strobj.OBJM_desth3d
    else
      dstobject
  -> dstobject in
  (
    if strobj.OBJM_iSrcMode == 0 then nil else
      set strobj.OBJM_h3d = V3DgetCameraByType c3dXsession V3DgetDefaultCamera c3dXsession strobj.OBJM_iSrcMode;
      
    if (isvec) then nil else
    (
      if strobj.OBJM_iDestMode == 0 then nil else
        set dstobject = V3DgetCameraByType c3dXsession V3DgetDefaultCamera c3dXsession strobj.OBJM_iDestMode;
    );
    
    let SO3ObjectGetGlobalPosition strobj.OBJM_h3d -> vec in
    let SO3ObjectGetGlobalOrientation strobj.OBJM_h3d -> ang in

    let strextr param -> lp in
    let if (dstobject != nil) then
          SO3ObjectGetGlobalPosition dstobject
        else
          let if (atof (nth_list (hd lp) 0)) == nil then 0.0 else (atof (nth_list (hd lp) 0)) -> px in
          let if (atof (nth_list (hd lp) 1)) == nil then 0.0 else (atof (nth_list (hd lp) 1)) -> py in
          let if (atof (nth_list (hd lp) 2)) == nil then 0.0 else (atof (nth_list (hd lp) 2)) -> pz in
            addVectorF vec (SO3MathsQuatGetDirection ang [px py pz])
    -> dstvec in
    let ang -> dstang in
    (
      set strobj.OBJM_srcpos = [vec ang strobj.OBJM_TCB 0.0];
      set strobj.OBJM_dstpos = [dstvec dstang [0.0 0.0 0.0] 1.0];
      
      // calculate the tcb curve distance
      let getTCBLength strobj strobj.OBJM_speed -> tcbdist in
      (
        if tcbdist == 0.0 then 
        (
          set strobj.OBJM_coef = 1.0;
          set strobj.OBJM_angcoef = 1.0;
          set strobj.OBJM_angstep = 1.0;
          set strobj.OBJM_step = 1.0;
        )
        else
        // calculate the step with the angular and tcb curve distance average
        let SO3MathsQuatDiff ang dstang -> qdiff in
        let SO3MathsQuatToEulerXYZ qdiff -> [dx dy dz] in
        let sqrt((sqr dx) +. (sqr dy) +. (sqr dz)) -> angdist in
        let if angdist == 0.0 then 1.0 else angdist -> angdist in
        let if strobj.OBJM_bUseRotate then 
              (1.0 /. ((tcbdist /. 2.0) *. ((itof strobj.OBJM_fps) /. strobj.OBJM_speed)))
            else
              (1.0 /. (((tcbdist +. angdist) /. 2.0) *. ((itof strobj.OBJM_fps) /. strobj.OBJM_speed)))
        -> step in
        let (1.0 /. ((angdist /. 2.0) *. ((itof strobj.OBJM_fps) /. strobj.OBJM_rspeed))) -> rstep in
        (
          set strobj.OBJM_coef = 0.0;
          set strobj.OBJM_angcoef = 0.0;
          set strobj.OBJM_step = step;
          set strobj.OBJM_angstep = if strobj.OBJM_bUseRotate then rstep else step;
        );
      );
    );
    
    set strobj.OBJM_state = 1;
    
    setPluginInstanceCbScenePreRenderPhysic strobj.OBJM_inst mkfun4 mkfun5 @cbMoveObj @cbObjEnd strobj;
  );
  0;;


fun objRelease(strobj)=
  setPluginInstanceCbScenePreRenderPhysic strobj.OBJM_inst nil;
  
  if (strobj.OBJM_state == 0) then nil else
  (
    let strobj.OBJM_inipos -> [vec ang] in
    (
      SO3ObjectSetPosition strobj.OBJM_h3d vec;
      SO3ObjectSetOrientation strobj.OBJM_h3d ang;
    );
    
    set strobj.OBJM_state = 0;
  );
  0;;


fun deleteOb(inst, strobj)=
  objRelease strobj;
  0;;


fun newOb(inst)=
  let (getPluginInstanceParam inst "destobject") -> destname in
  let (getPluginInstanceParam inst "sourceobject") -> sourcename in
  let atoi (getPluginInstanceParam inst "fps") -> fps in
  let atof (getPluginInstanceParam inst "speed") -> speed in
  let if speed == 0.0 then 0.01 else speed -> speed in
  let atof (getPluginInstanceParam inst "rspeed") -> rspeed in
  let if rspeed == 0.0 then 0.01 else rspeed -> rspeed in
  let atoi (getPluginInstanceParam inst "brotate") -> brotate in
  let atof (getPluginInstanceParam inst "T") -> ft in
  let atof (getPluginInstanceParam inst "C") -> fc in
  let atof (getPluginInstanceParam inst "B") -> fb in
  
  let atof (getPluginInstanceParam inst "xdir") -> xdir in
  let atof (getPluginInstanceParam inst "ydir") -> ydir in
  let atof (getPluginInstanceParam inst "zdir") -> zdir in
  let if xdir == nil then 0.0 else xdir -> xdir in
  let if ydir == nil then 0.0 else ydir -> ydir in
  let if zdir == nil then (-.1.0) else zdir -> zdir in
  
  //seems to work in all cases ?!
  let atoi (getPluginInstanceParam inst "local") -> local in
  let if local == nil then 1 else local -> local in
  
  let if rspeed == nil then speed else rspeed -> rspeed in
  let if brotate == nil then 0 else brotate -> brotate in
  
  // for compatibility
  let if ((atoi (getPluginInstanceParam inst "iscamera")) == 1) then "Current camera" else sourcename -> sourcename in
  
  let V3DgetObjectByName c3dXsession sourcename -> source in
  let V3DgetObjectTypeByName sourcename -> isourcemode in
  let V3DgetObjectByName c3dXsession destname -> objdest in
  let V3DgetObjectTypeByName destname -> idestmode in

  let mkTobjMvto [inst source objdest isourcemode idestmode nil nil nil nil nil 0 0.0 0.0 nil nil 0.0 nil nil nil brotate 0 local 0] -> strobj in
  (
    set strobj.OBJM_TCB = [ft fc fb];
    set strobj.OBJM_tDir = [xdir ydir zdir];
    set strobj.OBJM_fps = fps;
    set strobj.OBJM_speed = speed;
    set strobj.OBJM_rspeed = rspeed;
    set strobj.OBJM_inipos = [(SO3ObjectGetPosition strobj.OBJM_h3d) (SO3ObjectGetOrientation strobj.OBJM_h3d)];
    
    PluginRegisterAction inst "Move to" mkfun6 @cbObjGoto strobj;
    PluginRegisterAction inst "Translate to" mkfun6 @cbObjTranslate strobj;
		PluginRegisterAction inst "Stop" mkfun6 @cbObjStop strobj;
		PluginRegisterAction inst "Set speed" mkfun6 @cbSetSpeed strobj;
		
    setPluginInstanceCbDel inst mkfun2 @deleteOb strobj;
  );
  0;;


fun IniPlug(file)=
  PlugRegister @newOb nil;
  setPluginEditor @dynamicedit;
  0;;