/*****************************************
planar Fourbar linkage
by Haijun Su
October 2000
******************************************/
#include "colors.inc"	// Standard Color definitions
#include "textures.inc"	// Standard Texture definitions 

/*The following are some useful pre-defined objects */

//Define Axis. Default axis is along X axis;
//Specify the texture(color) of the axis
#declare  Axis =
union {
  cylinder {
    <0, 0, 0>,     // Center of one end
    <2, 0, 0>,     // Center of other end
    0.05           // Radius
}
   cone {           //Arrow of the axis
    <0, 0, 0>, 0.1    // Center and radius of one end
    <0.3, 0, 0>, 0.0    // Center and radius of other end
    translate 2*x 
  }
}

//Define the shape of a gripper

#declare  Gripper = 
 prism {
    linear_sweep
    cubic_spline
    -0.15, // sweep the following shape from here ...
    0.15, // ... up through here
    11, // the number of points making up the shape ...
    <-0.2, 0.3>, 
    <0.2, 0.1>, 
    <0.2, 0.3>, 
    <0.5, 0.3>, 
    <0.5, -0.3>, 
    <-0.5, -0.3>, 
    <-0.5, 0.3>, 
    <-0.2, 0.3>, 
    <-0.2, 0.1>, 
    <0.2, 0.1>,
    <0.5, 0.3> 
}


#declare Fixed_pivot = 
union
{
  superellipsoid {
    <0.25, 0.25>  
    texture {Gold_Metal}
    scale <0.6,0.6,0.2>
    } 
  cylinder {
   <0,0,0>,
   <0,0,-1>,
   0.1
   texture {Gold_Metal}
  }   
}
#macro Link(pos1, pos2, scalar) //Draw a link from position1 to position2 
union
{
  cylinder {
  <pos1[0],pos1[1],-0.25*scalar>,
  <pos1[0],pos1[1],0.25*scalar>,
  0.2*scalar
  texture {Copper_Metal}
  }      
  cylinder {
  <pos2[0],pos2[1],-0.25*scalar>,
  <pos2[0],pos2[1],0.25*scalar>,
  0.2*scalar
  texture {Copper_Metal}
  } 
  cylinder {
  <pos1[0],pos1[1],0>,
  <pos2[0],pos2[1],0>,
  0.1*scalar
  texture {Brushed_Aluminum}
  } 
  
  cylinder {
  <pos2[0],pos2[1],0>,
  <pos2[0],pos2[1],-1*scalar>,
  0.1*scalar
  texture {Gold_Metal}
  }
    
} 
#end
/*The X Y Z axes of the fixed coordinate system*/
#declare XYZ =
union {
   //X axis
   object {
      Axis  //Default axis is X axis
      texture {
        pigment { Red }
        finish {ambient .25 diffuse .6 specular .5}
      }
   }      
   // Y axis
   object{   
        Axis 
      texture {
        pigment { Green }
        finish {ambient .25 diffuse .6 specular .5}
      }
        rotate <0,0,90> //Note: POV-Ray is a left hand system
   } 
   // Z axis
   object{   
        Axis
      texture {
        pigment { Blue }
        finish {ambient .25 diffuse .6 specular .5}
      }
        rotate <0,-90,0>  //Note: POV-Ray is a left hand system
   }                                 
}            

#macro Coupler(pos1, pos2, pos_gripper,scalar)
union
{
  cylinder {
  <pos1[0],pos1[1],-0.25*scalar>,
  <pos1[0],pos1[1],0.25*scalar>,
  0.2*scalar
  texture {Copper_Metal}
  }      
  cylinder {
  <pos2[0],pos2[1],-0.25*scalar>,
  <pos2[0],pos2[1],0.25*scalar>,
  0.2*scalar
  texture {Copper_Metal}
  }
  
  cylinder {
  <pos1[0],pos1[1],0>,
  <pos2[0],pos2[1],0>,
  0.1*scalar
  texture {Brushed_Aluminum}
  } 
  cylinder {
  <pos2[0],pos2[1],0>,
  <pos_gripper[0][3],pos_gripper[1][3],0>,
  0.03*scalar
  texture {Brushed_Aluminum}
  } 
  cylinder {
  <pos1[0],pos1[1],0>,
  <pos_gripper[0][3],pos_gripper[1][3],0>,
  0.03*scalar
  texture {Brushed_Aluminum}
  } 
 
 smooth_triangle { 
    <pos1[0], pos1[1], 0>, <0,0,1>,    
    <pos2[0], pos2[1], 0>, <0,0,1>,    
    <pos_gripper[0][3], pos_gripper[1][3], 0>, <0,0,1>    
    texture {Yellow_Glass}
 }
 
 union{
  object{   
   Gripper
   texture {Silver_Metal}   
   rotate <-90,0,0>     
   }
   XYZ    
   scale scalar
   matrix <pos_gripper[0][0], pos_gripper[1][0], -pos_gripper[2][0],pos_gripper[0][1], pos_gripper[1][1], -pos_gripper[2][1],pos_gripper[0][2], pos_gripper[1][2], -pos_gripper[2][2], pos_gripper[0][3], pos_gripper[1][3], pos_gripper[2][3] >   
   }          
}
#end 

//Draw a four bar linkage given O, A, B, C and pos_Gripper. All are 4x4 matrices
#macro FourBarLinkage(O, A, B, C, pos_Gripper scalar)
//the fixed pivot at point O
object{    
Fixed_pivot
scale scalar
translate <O[0][3],O[1][3],0>
}

//the fixed pivot at point C
object{    
Fixed_pivot
scale scalar
translate <C[0][3],C[1][3],0>
}

//Link OA
#local tmp1 = array [2] {O[0][3], O[1][3]}
#local tmp2 = array [2] {A[0][3], A[1][3]}
object{ 
Link(tmp1,tmp2, scalar)  //OA
translate <0,0,-0.6*scalar>
}
 
//Link CB
#local tmp1 = array [2] {C[0][3], C[1][3]}
#local tmp2 = array [2] {B[0][3], B[1][3]}
object{ 
Link(tmp1,tmp2, scalar)  //CB
translate <0,0,-0.6*scalar>
}      

//The coupler AB
#local tmp1 = array [2] {A[0][3], A[1][3]}
#local tmp2 = array [2] {B[0][3], B[1][3]}
object{ 
Coupler(tmp1, tmp2, pos_Gripper, scalar)
translate <0,0,-1.2*scalar>
}  
#end

//Set object[4][4] = Source[I][4][4]
#macro SetMatrix(Object, Source, I)
 #declare J=0;
 #while (J<4)
   #declare K=0;
   #while (K<4)
      #declare Object[J][K] = Source[I][J][K];
      #declare K=K+1;
   #end
   #declare J=J+1;
 #end
#end

//Now define the scene

camera {
	location  <-10, 5, -50>
	angle	45	
	look_at   <-8, 5, 0>
 }

/*camera {  
        right <1,0,0>
        up <0,0,1> 
	location  <-10, 0, -40>
        orthographic
 }*/

light_source {
	<-14, 0, -40>
	color White
}

/*Now define all the objects in the scene*/ 
// The floor
plane
{
  z, 2
  texture
  {
    //pigment {checker color White color Blue}    
    pigment { checker color White color rgb 0.9}
    finish {ambient 0.3 reflection 0.1}
  }
}


 
#declare scalar= 3;

#include "FourBar.inc"

union{
// Position 1
union{
 object{   
   Gripper
  texture {
        pigment { Red }
        finish {ambient .25 diffuse .6 specular .5}
      }
   rotate <-90,0,0>
   }
 object{  XYZ   }
   scale scalar
   matrix <Trans1[0][0], Trans1[1][0], Trans1[2][0],Trans1[0][1], Trans1[1][1], Trans1[2][1],Trans1[0][2], Trans1[1][2], Trans1[2][2], Trans1[0][3], Trans1[1][3], Trans1[2][3] >
   }
// Position 2
union{
 object{   
   Gripper
   texture {
        pigment { Green }
        finish {ambient .25 diffuse .6 specular .5}
      }
   rotate <-90,0,0>
      }
 object{  XYZ   }
   scale scalar
   matrix <Trans2[0][0], Trans2[1][0], Trans2[2][0],Trans2[0][1], Trans2[1][1], Trans2[2][1],Trans2[0][2], Trans2[1][2], Trans2[2][2], Trans2[0][3], Trans2[1][3], Trans2[2][3] >
   }
// Position 3
union{
 object{   
   Gripper
   texture {
        pigment { Blue }
        finish {ambient .25 diffuse .6 specular .5}
      }
   rotate <-90,0,0>
   }
 object{  XYZ   }
   scale scalar
   matrix <Trans3[0][0], Trans3[1][0], Trans3[2][0],Trans3[0][1], Trans3[1][1], Trans3[2][1],Trans3[0][2], Trans3[1][2], Trans3[2][2], Trans3[0][3], Trans3[1][3], Trans3[2][3] >
   }
   translate -1.2*scalar*z
   }
/*
union{
  union{
  object{   
   Gripper
   texture {Silver_Metal}
   rotate <-90,0,0>   }
   XYZ    
   scale scalar
   matrix <Trans1[0][0], Trans1[1][0], Trans1[2][0],Trans1[0][1], Trans1[1][1], Trans1[2][1],Trans1[0][2], Trans1[1][2], Trans1[2][2], Trans1[0][3], Trans1[1][3], Trans1[2][3] >   
   }          
   object{   XYZ    
   scale scalar
   }  

  union{
  object{   
   Gripper
   texture {Silver_Metal}
   rotate <-90,0,0>   }
   XYZ         
   scale scalar
   matrix <Trans2[0][0], Trans2[1][0], Trans2[2][0],Trans2[0][1], Trans2[1][1], Trans2[2][1],Trans2[0][2], Trans2[1][2], Trans2[2][2], Trans2[0][3], Trans2[1][3], Trans2[2][3] > }          

  union{
  object{   
   Gripper
   texture {Silver_Metal}
   rotate <-90,0,0>   }
   XYZ         
   scale scalar
   matrix <Trans3[0][0], Trans3[1][0], Trans3[2][0],Trans3[0][1], Trans3[1][1], Trans3[2][1],Trans3[0][2], Trans3[1][2], Trans3[2][2], Trans3[0][3], Trans3[1][3], Trans3[2][3] > }          
   
  translate -1.2*scalar*z
}
*/
#declare O_I = array [4][4]
#declare A_I = array [4][4] 
#declare B_I = array [4][4] 
#declare C_I = array [4][4] 
#declare pos_gripper_I = array[4][4] 


#if(clock<500)
 #declare I=0;
 merge{
 
 #declare I=clock
   #if(I<dimension_size(DA,1))
       
        SetMatrix(A_I, DA, I)  
        SetMatrix(B_I, DB, I)  
        SetMatrix(pos_gripper_I, GripA, I)  
       
    FourBarLinkage(baseO, A_I, B_I, baseC, pos_gripper_I, scalar)    
   #end     
 }
#end