Another update. Before implementing more complex structures, I made it 
possible to solve multiple independen IK's with one Fabrik 'object'

ingo

----%<----%<----%<----%<----%<----
//==
// Pov-Ray   : 3.8
// Scene File: FABRIK.inc
// Author    : Ingo Janssen
// Date      : 01-12-2018
// Rev Date  : 05-12-2018
// Version   : 1a2
//--
// 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;

//==
// macros starting with an underscore should not be called 'from the 
outside'.
// only chains mentioned in the process dictionary will be initialised
// absence of the process dictionary will result in an error
//
// The FABRIK macro needs a rather complex datastructure as input, to be 
explained later.
// Create an 'instance' of the Fabrik 'object': #declare S = FABRIK(F); 
where F is the 
// datastructure. Fabrik 'returns' an updated/initialised version of the 
input structure. 
// 
// To run the IK solver use SetTarget(S, T) where S is the afore 
mentioned insatance of 
// Fabrik and T a datastructure holding the target(s)
//
#macro FABRIK(self)
  #ifndef (self.Proc)
    #error "\nNo Process 'Proc' dictionary defined\n"
  #end
  
  // add FABRIK global defaults, they can be overridden by adding a 
Default
  // dictionary to a Chain dictionary. The Chain dict is checked first 
for
  // these values, if not available the 'global' ones are used. 
  #local self.Default = dictionary {
    ["Tolerance"] : 0.1,
    ["MaxIter"] : 100
  }
  
  #macro _Init(self)
    #macro _Lengths(self, Chain)
      #local self[Chain]._Lengths = array[self[Chain]._N-1];
      #local self[Chain]._Length = 0;
      #local i = 0;
      #while (i < (self[Chain]._N-1))
        #local self[Chain]._Lengths[i] = vlength(self[Chain].Joint[i]-
self[Chain].Joint[i+1]);
        #local self[Chain]._Length = self[Chain]._Length + self[Chain].
_Lengths[i];
        #local i = i + 1; 
      #end
    #end
    
    #for (i,0,dimension_size(self.Proc,1)-1,1)
      #ifdef (self[self.Proc[i]].Joint)
        #local Chain = self.Proc[i];
        #debug concat("init chain : ", Chain, "\n")
        #local self[Chain]._O = self[Chain].Joint[0];
        #local self[Chain]._N = dimension_size(self[Chain].Joint, 1);
        #local self[Chain]._M = self[Chain]._N-1; //max index of array
        _Lengths(self, Chain)
      #end
    #end
  #end
  _Init(self)

  // internals  
  #macro _Reach(self, Chain) //SimpleSolver & EndPoint
    #debug "Reach : "
    #if (vlength(self[Chain]._O - self[Chain].Target) > self[Chain].
_Length)
      #local self[Chain]._Reachable = false;
      #debug "False\n"
    #else
      #local self[Chain]._Reachable = true;
      #debug "True\n"
    #end
  #end

  #macro _Backwards(self, Chain)
    #debug "  Backwards :\n"
    #local self[Chain].Joint[self[Chain]._M] = self[Chain].Target;
    #for (i, self[Chain]._M-1, 0, -1)
      #local l = self[Chain]._Lengths[i]/vlength(self[Chain].Joint[i+1]-
self[Chain].Joint[i]);
      #local Pos =  (1-l) * self[Chain].Joint[i+1] + l * self
[Chain].Joint[i];
      //if constraint
      //check constraint
      //if not within constraint adjust Pos
      //2B done
      #local self[Chain].Joint[i] = Pos;
      #debug concat("    i : ",str(i,0,0)," pos : ", vstr
(3,Pos,",",0,3),"\n")
    #end
  #end

  #macro _Forward(self, Chain)
    #debug "  Forward :\n"
    // set first point back to its original position
    #local self[Chain].Joint[0] = self[Chain]._O; 
    #for (i, 0, self[Chain]._M-1, 1)
      #local l = self[Chain]._Lengths[i]/vlength(self[Chain].Joint[i+1]-
self[Chain].Joint[i]);
      #local Pos =  (1-l) * self[Chain].Joint[i] + l * self[Chain].Joint
[i+1];
      //if constraint
      //check constraint
      //if not within constraint adjust Pos
      //2B done
      #local self[Chain].Joint[i+1] = Pos;
      #debug concat("    i : ",str(i,0,0)," pos : ", vstr
(3,Pos,",",0,3),"\n")
    #end
  #end

  #macro _Stretch(self, Chain)
    #debug "Stretch :\n"
    #for (i, 0, self[Chain]._M-1, 1)
      #local l = self[Chain]._Lengths[i]/vlength(self[Chain].Target-self
[Chain].Joint[i]);  
      #local self[Chain].Joint[i+1] = (1-l) * self[Chain].Joint[i] + l * 
self[Chain].Target;
      #debug concat("i : ",str(i,0,0), " pos : ", vstr(3,self
[Chain].Joint[i+1],",",0,3),"\n")
    #end
  #end

  #macro _SolveSimple(self, Chain)
    #debug "Solving :\n"
    #if (self[Chain]._Reachable = false)
      _Stretch(self, Chain)
    #else
      #local itercount = 0;
      #local dif = vlength(self[Chain].Joint[self[Chain]._M] - self
[Chain].Target); 
      #while (dif > self.Default.Tolerance)  //2B done also check chain 
local defaults
        #debug concat("  dif : ",str(dif,0,3)," iter : ",str
(itercount,0,0),"\n")         
        _Backwards(self, Chain)
        _Forward(self, Chain)
        #local dif = vlength(self[Chain].Joint[self[Chain]._M] - self
[Chain].Target);
        #local itercount = itercount + 1;
        #if (itercount > self.Default.MaxIter)//2B done also check chain 
local defaults
          #local dif = 0;
        #end
      #end
      #debug concat("\n  dif : ",str(dif,0,3)," end iter\n\n")         
    #end
  #end 

  // external
  #macro SetTarget (self, T)
    #for (i, 0, dimension_size(T.Index,1)-1,1)
      #local Chain = T.Index[i];
      #debug concat("set target : ", Chain, "\n")
      #local self[Chain].Target = T[Chain];
      _Reach(self, T.Index[i])
      _SolveSimple(self, T.Index[i])
    #end
  #end

  self
#end


/*test scene*/

#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,0,-8> look_at <0,0,0>}


// Two 'independent' chains
#declare F = dictionary{
  ["Proc"]: array[2]{"Base1", "Base2"},
  ["Base1"]   : dictionary {
      ["Joint"] : array[6]{<0,0,0>, < 0,1,0>, < 0,2,0>, < 0,3,0>, 
<0,4,0>, <0,5,0>},
  }
  ["Base2"]   : dictionary {
      ["Joint"] : array[6]{<0,0,0>, < 0,1,0>, < 0,2,0>, < 0,3,0>, 
<0,4,0>, <0,5,0>},
  }
};
#declare S = FABRIK(F);


#declare T = dictionary {
  ["Index"] : array[2]{"Base1", "Base2"},
  ["Base1"] : <4,0,0>,
  ["Base2"] : <-4,0,0>
}
SetTarget(S, T)

#for (i,0,dimension_size(S.Proc,1)-1,1)
  #ifdef (S[S.Proc[i]].Joint)
    #local Chain = S.Proc[i];
    #for (n,0,S[Chain]._M-1,1)    
      sphere{S[Chain].Joint[n], 0.2}
      cylinder{S[Chain].Joint[n],S[Chain].Joint[n+1],0.2}
    #end
    sphere{S[Chain].Joint[n], 0.2 pigment{rgb x}} //end points
  #end 
#end

Post a reply to this message