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

//Now define the scene
camera {
	location  <1, 2, -12>
	look_at   <0,0,0>
	//rotate 30*y
	 
}

light_source {
	<40, 0, -40>
	color White
}
light_source {
	<-20, 40, -10>
	color White 
	shadowless
} 
light_source {
	<13, 90, -40>
	color rgb 0.5
}

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

// Define Matrix operation
#macro SetMatrix3by3(Object, Source, I)
 #declare J=0;
 #while (J<3)
   #declare K=0;
   #while (K<3)
      #declare Object[J][K] = Source[I][J][K];
      #declare K=K+1;
   #end
   #declare J=J+1;
 #end
#end

//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 
  }
}
/*The X Y Z axes of the fixed coordinate system*/
#declare XYZAxes = 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
   }                                 
}             

//Define the shape of a gripper
#declare  Gripper = 
 prism {
 /*
    linear_sweep
    linear_spline
    -0.2, // sweep the following shape from here ...
    0.2, // ... up through here
    13, // the number of points making up the shape ...
    <0.2, 0.1>, 
    <0.2, 0.3>, 
    <0.3, 0.3>, 
    <0.5, 0.1>, 
    <0.5, -0.2>, 
    <0.4, -0.3>, 
    <-0.4, -0.3>, 
    <-0.5, -0.2>, 
    <-0.5, 0.1>, 
    <-0.3, 0.3>, 
    <-0.2, 0.3>, 
    <-0.2, 0.1>, 
    <0.2, 0.1>   */
    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,-0.7>,
   0.1
//   texture {Gold_Metal}
  }   
}
#macro AlignThisVector(vector_S,radius_of_sphere)
#local zaxis = vector_S;

//#if(vlength(vcross(vector_S,<1,0,0>)))   
     #local anyvector = <0.123,3.234,1.1>;
//#else
//        #local anyvector = <0,1,0>; 
//#end    

#local xaxis = vnormalize(vcross(zaxis, anyvector));
#local yaxis = vnormalize(vcross(zaxis, xaxis));
matrix <xaxis.x,xaxis.y,xaxis.z, yaxis.x,yaxis.y, yaxis.z, -zaxis.x, -zaxis.y, -zaxis.z, radius_of_sphere*zaxis.x,radius_of_sphere*zaxis.y,radius_of_sphere*zaxis.z>
#end


#macro Sphere_Fixed_pivot(vector_S, radius_of_sphere, texture_of_this_pivot)
object{    
Fixed_pivot
texture{texture_of_this_pivot}
AlignThisVector(vector_S,radius_of_sphere)
}
#end

#macro ArcLink(V1,V2, radius_of_link, radius_of_sphere)
object{  
   torus { radius_of_sphere, radius_of_link 
      sturm 
      rotate 90*x
      AlignThisVector(vcross(V1,V2), 0)   
    }
    clipped_by{  
        plane {-V2 + V1*(vdot(V1,V2)),0}
        plane {-V1 + V2*(vdot(V1,V2)),0}
    }   
      texture {Brushed_Aluminum}    
  }
#end
#macro SphereTriangle(V1,V2,V3, radius_of_sphere)
object{  
   sphere { 
   <0,0,0>,
   radius_of_sphere
    }
    clipped_by{  
        plane {vcross(V1,V2),0}
        plane {vcross(V2,V3),0}
        plane {vcross(V3,V1),0}
    }   
      texture {Yellow_Glass}    
  }
#end

#macro SphereLink(V1, V2, radius_of_sphere, texture1) //Draw a link given 2 vectors
union
{
  cylinder {
  -0.25*<0,0,1>,
  +0.25*<0,0,1>,
  0.2
  texture {Copper_Metal}
  AlignThisVector(V1,radius_of_sphere)
  }
object{  
ArcLink(V1,V2,0.1, radius_of_sphere)
texture {Brushed_Aluminum}    
  }
} 

union{  
  cylinder {
  -0.25*<0,0,1>,
  +0.25*<0,0,1>,
  0.2
  texture {Copper_Metal}
  } 
  cylinder {
  0*<0,0,1>,
  -1.0*<0,0,1>,
  0.1
  texture{texture1}
  }
  AlignThisVector(V2,radius_of_sphere)
}
    
 
#end

#macro SphereCoupler(V1, V2, Matrix_Gripper ,radius_of_sphere)
#local v_gripper=<Matrix_Gripper[0][2], Matrix_Gripper[1][2],Matrix_Gripper[2][2]>;
union
{
  cylinder {
  -0.25*<0,0,1>,
  +0.25*<0,0,1>,
  0.2
  texture {Copper_Metal}
  AlignThisVector(V1,radius_of_sphere)
  }

  cylinder {
  -0.25*<0,0,1>,
  +0.25*<0,0,1>,
  0.2
  texture {Copper_Metal}
  AlignThisVector(V2,radius_of_sphere)
  }
  
  union{  
  ArcLink(V1,V2,0.1 radius_of_sphere)
  ArcLink(V1,v_gripper,0.05 radius_of_sphere)
  ArcLink(V2,v_gripper,0.05 radius_of_sphere)
 
  texture {Brushed_Aluminum}    
  }
 SphereTriangle(V1,V2,v_gripper, radius_of_sphere) 
union{ 
 object{
 Gripper
 rotate 90*x
 rotate 90*z
 }
  object{
 XYZAxes
 scale 0.5
 matrix <1,0,0,0,1,0,0,0,-1,0,0,0>
}

   texture {Silver_Metal}                     
matrix <Matrix_Gripper[0][0],Matrix_Gripper[1][0], Matrix_Gripper[2][0],Matrix_Gripper[0][1],Matrix_Gripper[1][1], Matrix_Gripper[2][1], Matrix_Gripper[0][2],Matrix_Gripper[1][2], Matrix_Gripper[2][2], radius_of_sphere*Matrix_Gripper[0][2],radius_of_sphere*Matrix_Gripper[1][2], radius_of_sphere*Matrix_Gripper[2][2]>

  }
 
  
}
#end 

/*Now define all the objects in the scene*/ 
sky_sphere
{
  pigment
  {
    gradient y
    color_map { [0.2 0.8 color Blue color White] [0.8 1.0 color White color White] }
    scale 2
    translate -1

  }
} 
#declare position_floor=-6;
// The floor
  #declare Tile = superellipsoid { 
  <0.5, 0.1>    scale <1, .05, 1>
  }
   #declare Offset = 2.1;

   #declare Row = union {
    object { Tile }
    object { Tile translate z*Offset }
    object { Tile translate z*Offset*2 }
    object { Tile translate z*Offset*3 }
    object { Tile translate z*Offset*4 }
    object { Tile translate z*Offset*5 }
    object { Tile translate z*Offset*6 }
    object { Tile translate z*Offset*7 }
    object { Tile translate z*Offset*8 }
    object { Tile translate z*Offset*9 }
    object { Tile translate z*Offset*10 }
    object { Tile translate -z*Offset }
    object { Tile translate -z*Offset*2 }
    object { Tile translate -z*Offset*3 }
    object { Tile translate -z*Offset*4 }
    object { Tile translate -z*Offset*5 }
    object { Tile translate -z*Offset*6 }
  }

    union{
    object { Row }
    object { Row translate x*Offset }
    object { Row translate x*Offset*2 }
    object { Row translate x*Offset*3 }
    object { Row translate x*Offset*4 }
    object { Row translate x*Offset*5 }
    object { Row translate x*Offset*6 }
    object { Row translate x*Offset*7 }
    object { Row translate -x*Offset }
    object { Row translate -x*Offset*2 }
    object { Row translate -x*Offset*3 }
    object { Row translate -x*Offset*4 }
    object { Row translate -x*Offset*5 }
    object { Row translate -x*Offset*6 }
    object { Row translate -x*Offset*7 }

      texture {
        pigment { rgb 0.5 }
        finish {ambient .25 diffuse .6 specular .5}
      }

    translate <0.0,position_floor,0>
  }

 #declare size_Of_floor = 20;  
box{
        size_Of_floor*<-0.78,0,-1.1>,
        size_Of_floor*<0.78,-0.001,1.1>
        pigment {rgb 0.02}
        translate <0,position_floor-0.2,0>
} 


#include "SpFourBar.inc"  

//The positions of the joints of a four-bar linkage
#declare radius_of_sphere=4;     
#declare radius_of_sphere_key = radius_of_sphere+1.2
//the fixed pivot at point O
#declare O= <baseO[0][2],baseO[1][2], baseO[2][2]>
#declare C= <baseC[0][2],baseC[1][2], baseC[2][2]>  
#declare Matrix_Gripper_I = array[3][3]

#declare textureO = texture {
	pigment { Green }
	finish {ambient .25 diffuse .6 specular .5}}
#declare textureC = texture {
        pigment { Red }
        finish {ambient .25 diffuse .6 specular .5}
        }
#declare textureA = texture {
        pigment { Green }
        finish {ambient .25 diffuse .6 specular .5}
        }
        
#declare textureB = texture {
        pigment { Red }
        finish {ambient .25 diffuse .6 specular .5}
        }
#declare textureTrans1 = texture {
        pigment { Red }
        finish {ambient .25 diffuse .6 specular .5 phong 1  phong_size 200 }
        }
#declare textureTrans2 = texture {
        pigment { Blue }
        finish {ambient .25 diffuse .6 specular .5 phong 1  phong_size 200 }
        }
#declare textureTrans3 = texture {
        pigment { Green }
        finish {ambient .25 diffuse .6 specular .5 phong 1  phong_size 200 }
        }

XYZAxes

sphere{
<0,0,0>,
radius_of_sphere 

texture {
    pigment { rgbf <0.98, 0.8, 0.8, 0.7> }         
    finish {ambient .25 diffuse .6 specular .5         
    phong 1 
    phong_size 200
}
  } 
//texture {Green_Glass}
}


Sphere_Fixed_pivot(O, radius_of_sphere, textureO)
//the fixed pivot at point C
Sphere_Fixed_pivot(C, radius_of_sphere, textureC)

//#declare I = 0;
#declare I = clock;
 
#declare PointA_I= <DA[I][0][2], DA[I][1][2], DA[I][2][2]>;
#declare PointB_I= <DB[I][0][2], DB[I][1][2], DB[I][2][2]>;

//Link OA
SphereLink(O, PointA_I, radius_of_sphere+0.4, textureA)
//Link CB
SphereLink(C, PointB_I, radius_of_sphere+0.4, textureB)

//The coupler AB   
SetMatrix3by3(Matrix_Gripper_I, GripA, I)
SphereCoupler(PointA_I, PointB_I, Matrix_Gripper_I, radius_of_sphere_key) 

union{
 object{
 Gripper
 rotate 90*x
 rotate 90*z
  }
 object{
 XYZAxes
 scale 0.5
 matrix <1,0,0,0,1,0,0,0,-1,0,0,0>
}
 texture {textureTrans1}                     
 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], radius_of_sphere_key*Trans1[0][2],radius_of_sphere_key*Trans1[1][2], radius_of_sphere_key*Trans1[2][2]>
}  
union{
 object{
 Gripper
 rotate 90*x
 rotate 90*z 
 }
 object{
 XYZAxes
 scale 0.5
 matrix <1,0,0,0,1,0,0,0,-1,0,0,0>
}

 texture {textureTrans2}                     
 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], radius_of_sphere_key*Trans2[0][2],radius_of_sphere_key*Trans2[1][2], radius_of_sphere_key*Trans2[2][2]>
  }
  
union{
 object{
 Gripper
 rotate 90*x
 rotate 90*z
  }
 object{
 XYZAxes
 scale 0.5
 matrix <1,0,0,0,1,0,0,0,-1,0,0,0>
}
 texture {textureTrans3}                     
 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], radius_of_sphere_key*Trans3[0][2],radius_of_sphere_key*Trans3[1][2], radius_of_sphere_key*Trans3[2][2]>
 }