Did a quick search but nothing showed up for POV-Ray & FABRIK. So here's 
my first attempt at the most basic version. Next steps will be more 
complex skeletons with multiple ends and then joint constraints. I hope 
that the latter can nicely done with the trace function.

Dictionaries are nice.

//==
// Pov-Ray   : 3.8
// Scene File: FABRIK.inc
// Author    : Ingo Janssen
// Date      : 01-12-2018
// Version   : 1a
//--
// Forward And Backward Reaching Inverse Kinematics (FABRIK) by Andreas 
Aristidou.
// http://www.andreasaristidou.com/FABRIK.html
// http://www.andreasaristidou.com/publications/papers/FABRIK.pdf
// 
http://www.andreasaristidou.com/publications/papers/Extending_FABRIK_wit
h_Model_C%CE%BFnstraints.pdf
// Excellent code inspiration & guide, by unknown author:
// https://developer.roblox.com/articles/Inverse-Kinematics-for-
Animation#FABRIK
//

#version 3.8;

#macro FABRIK(VecArr, VecTarget)
  // init
  #local self = dictionary {
    ["Joints"]      : VecArr,
    ["_O"]          : VecArr[0],
    // should be replaced by SetTarget
    ["Target"]      : VecTarget,
    // use .SetTarget(vec) to set a new target and iterate
    // still to be done somehowe.
    ["SetTarget"]   : false, 
    ["Tolerance"]   : 0.1,
    ["MaxIter"]     : 100,
    ["_M"]          : false,
    ["_N"]          : false,
    ["_Reachable"]  : false,
    ["_Lengths"]    : false,
    ["_TotalLength"]: false,
  };
  #local self._N = dimension_size(self.Joints, 1);
  #local self._M = self._N-1; //max index of array

  #macro LengthsArr()
    //#debug "LengthsArr :\n"
    #local Lengths = array[self._N-1];
    #local i = 0;
    #while (i < (self._N-1))
      #local Lengths[i] = vlength(self.Joints[i]-self.Joints[i+1]);
      //#debug concat("i : ",str(i,0,0)," len : ",str(Lengths
[i],0,3),"\n")
      #local i = i + 1; 
    #end
    Lengths
  #end
  #local self._Lengths = LengthsArr();   

  #macro LengthsSum()    
    //#debug "LengthsSum :\n"
    #local Length = 0;
    #local i = 0;
    #while (i < dimension_size(self._Lengths,1))
       #local Length = Length + self._Lengths[i];
       //#debug concat("i : ",str(i,0,0)," len : ",str(self._Lengths
[i],0,3)," sum : ",str(Length,0,3),"\n")
       #local i = i + 1;
    #end
    Length        
  #end 
  #local self._TotalLength = LengthsSum();

  #macro Reach ()
    #debug "Reach : "
    #if (vlength(self._O-self.Target) > self._TotalLength)
      #local Reachable = false;
      #debug "False\n"
    #else
      #local Reachable = true;
      #debug "True\n"
    #end
    Reachable 
  #end
  #local self._Reachable = Reach();

  #macro Backwards(Joints)
    #debug "  Backwards :\n"
    #local Joints[self._M] = self.Target;
    #for (i, self._M-1, 0, -1)
      #local l = self._Lengths[i]/vlength(Joints[i+1]-Joints[i]);
      #local Pos =  (1-l) * Joints[i+1] + l * Joints[i];
      #local Joints[i] = Pos;
      #debug concat("    i : ",str(i,0,0)," pos : ", vstr
(3,Pos,",",0,3),"\n")
    #end
    Joints
  #end
 
  #macro Forward(Joints)
    #debug "  Forward :\n"
    // set first point back to its original position
    #local Joints[0] = self._O; 
    #for (i, 0, self._M-1, 1)
      #local l = self._Lengths[i]/vlength(Joints[i+1]-Joints[i]);
      #local Pos =  (1-l) * Joints[i] + l * Joints[i+1];
      #local Joints[i+1] = Pos;
      #debug concat("    i : ",str(i,0,0)," pos : ", vstr
(3,Pos,",",0,3),"\n")
    #end
    Joints
  #end
 
  #macro Stretch(Joints)
    #debug "Stretch :\n"
    #for (i, 0, self._M-1, 1)
      #local l = self._Lengths[i]/vlength(self.Target-Joints[i]);  
      #local Joints[i+1] = (1-l) * Joints[i] + l * self.Target;
      #debug concat("i : ",str(i,0,0), " pos : ", vstr(3,Joints[i+
1],",",0,3),"\n")
    #end
    Joints   
  #end
 
  #macro Solve(Joints)
   #debug "Solving :\n"
   #if (self._Reachable = false)
      #local Joints = Stretch(Joints);
    #else
      #local itercount = 0;
      #local dif = vlength(self.Joints[self._M] - self.Target); 
      #while (dif > self.Tolerance)
        #debug concat("  dif : ",str(dif,0,3)," iter : ",str
(itercount,0,0),"\n")         
        #local Joints = Backwards(Joints);
        #local Joints = Forward(Joints);
        #local dif = vlength(Joints[self._M] - self.Target);
        #local itercount = itercount + 1;
        #if (itercount > self.MaxIter)
          #local dif = 0;
        #end
      #end
      #debug concat("\n  dif : ",str(dif,0,3)," end iter\n\n")         
    #end
    Joints
  #end 
  #local self.Joints = Solve(self.Joints)

  self
#end  

//==
// testscene
//
#global_settings {assumed_gamma 1.0 }
#default {pigment{rgb 1} finish{ ambient 0.2 diffuse 0.9 }} 
light_source{<1000,1000,-1000>, rgb 1}
plane{-z,0 pigment{checker rgb 0.8 rgb 0.5}}
camera {location <0, 4.5,-15> look_at <0,4.5,0>}

#macro Connect(Arr, RGB)
  #local l = dimension_size(Arr,1);
  #local i = 0;
  union{
    #while (i < l-1)
      sphere{Arr[i],0.2 pigment{rgb RGB}}
      cylinder{Arr[i],Arr[i+1],0.1  pigment{rgb RGB}}
      #local i=i+1;
    #end
    sphere{Arr[l-1],0.2 pigment{rgb <0,1,0>}}
  }
#end

#declare S = array[10]{
  <0,0,0>,<1,1,0>,<2,2,0>,
  <3,3,0>,<4,4,0>,<5,5,0>,
  <6,6,0>,<7,7,0>,<8,8,0>,
  <9,9,0>
};

#for (i,9,-9,-1)
  #declare T = <i,9,0>;
  #declare Spine = FABRIK(S, T);
  Connect(Spine.Joints, <1,0,0>)
#end

Post a reply to this message