// This file contains no image output
// It displays debug messages to demonstrate the FindObjectProperties macro.

// matrix sum (helper, usually left in an include, this one from matrices.inc)
#macro summat(array1, array2,array3)
   #local dime=0;
   #local a1=dimension_size(array1,1);
   #if (a1=dimension_size(array2,1))    
       #local a2=dimension_size(array1,2);
       #if (a2=dimension_size(array2,2))
            #local i=0;
            #declare array3= array[a1][a2];
            #while (i<=a1-1)
                  #local j=0;
                  #while (j<=a2-1)
                       #declare array3[i][j]=array1[i][j]+array2[i][j]; 
                       #local j=j+1;
                  #end              
                  #local i=i+1;
            #end
        #else #local dime=1;
        #end
    #else #local dime=1;
    #end
    #if (dime=1) #debug "********Dimension mismatch (Sum)********\n" 
    #end
#end

// Finds the center of mass, volume, and moment of inertia tensor for an arbitrary object
// Moment tensor is in relation to the global x,y,z directions, with origin of the CoM
//   Math for tensor translation is from Young-Hoo Kwon, 1998 (http://www.kwon3d.com/theory/rigid.html)
// 2009.09.09 Christopher Shake (cshake+pov@gmail.com)
#macro FindObjectProperties(Object_Identifier, X_Resolution, Y_Resolution, Z_Resolution, Center_Of_Mass, Volume, MomentTensor)
  // Set up reference vars
  #local maxcorner = max_extent(Object_Identifier);
  #local mincorner = min_extent(Object_Identifier);
  #local max_x = max(maxcorner.x,mincorner.x);
  #local min_x = min(maxcorner.x,mincorner.x);
  #local max_y = max(maxcorner.y,mincorner.y);
  #local min_y = min(maxcorner.y,mincorner.y);
  #local max_z = max(maxcorner.z,mincorner.z);
  #local min_z = min(maxcorner.z,mincorner.z);
  #local dx = abs(max_x-min_x)/X_Resolution;
  #local dy = abs(max_y-min_y)/Y_Resolution;
  #local dz = abs(max_z-min_z)/Z_Resolution;
  #local dv = dx*dy*dz;
  #local Moments = array[3][3]{{0,0,0},{0,0,0},{0,0,0}};
  #local CoM = <0,0,0>;
  #declare Volume = 0;
  // Go through entire 3D matrix
  #local curr_x = min_x+dx/2;
  #while (curr_x < max_x)
    #local curr_y = min_y+dy/2;
    #while (curr_y < max_y)
      #local curr_z = min_z+dz/2;
      #while (curr_z < max_z)
        #if(inside(Object_Identifier,<curr_x,curr_y,curr_z>))
          #declare Volume = Volume+dv;
          // Add to the moment of inertia tensor
          #local Moments[0][0] = Moments[0][0]+dv*(pow(curr_y,2)+pow(curr_z,2));
          #local Moments[0][1] = Moments[0][1]-dv*curr_x*curr_y;
          #local Moments[0][2] = Moments[0][2]-dv*curr_x*curr_z;
          #local Moments[1][0] = Moments[1][0]-dv*curr_y*curr_x;
          #local Moments[1][1] = Moments[1][1]+dv*(pow(curr_z,2)+pow(curr_x,2));
          #local Moments[1][2] = Moments[1][2]-dv*curr_y*curr_z;
          #local Moments[2][0] = Moments[2][0]-dv*curr_z*curr_x;
          #local Moments[2][1] = Moments[2][1]-dv*curr_z*curr_y;
          #local Moments[2][2] = Moments[2][2]+dv*(pow(curr_x,2)+pow(curr_y,2));
          // Add to CoM
          #local CoM = CoM + dv*<curr_x,curr_y,curr_z>;
        #end
        #local curr_z = curr_z+dz;
      #end
      #local curr_y = curr_y+dy;
    #end
    #local curr_x = curr_x+dx;
  #end
  #declare Center_Of_Mass = CoM/Volume; 
  // Now that CoM is found, transform the tensor to be about the CoM
  // (but still in the global x,y,z frame) (note that this is negative, intentionally)
  #local MomentTranslation = array[3][3]{
    {-Volume*(pow(Center_Of_Mass.y,2)+pow(Center_Of_Mass.z,2)), Volume*Center_Of_Mass.x*Center_Of_Mass.y, Volume*Center_Of_Mass.x*Center_Of_Mass.z},
    {Volume*Center_Of_Mass.y*Center_Of_Mass.x, -Volume*(pow(Center_Of_Mass.z,2)+pow(Center_Of_Mass.x,2)), Volume*Center_Of_Mass.y*Center_Of_Mass.z},
    {Volume*Center_Of_Mass.z*Center_Of_Mass.x, Volume*Center_Of_Mass.z*Center_Of_Mass.y, -Volume*(pow(Center_Of_Mass.x,2)+pow(Center_Of_Mass.y,2))}
  }
  // Apply translation matrix, which stores result to tensor return variable
  summat(Moments,MomentTranslation,MomentTensor)
#end

	// Test cases:
#declare BoxObj=box{-1,1}
#declare SphereObj=sphere{0,1.5}
#declare TorusObj=torus{1,0.5 translate <0,1,1>}

#local returnCoM=<0,5,0>;
#local returnVolume=0;
#local returnTensor=array[3][3];

#debug "Box: \n"
#debug concat("Real moment about any principle axis: ",str(1/12*8*(pow(2,2)+pow(2,2)),0,2),"\n")
#debug concat("Real volume: ",str(pow(2,3),0,2),"\n")
FindObjectProperties(BoxObj,20,20,20,returnCoM,returnVolume,returnTensor)
#debug "Moment Tensor:\n"
DebugMatrix(returnTensor)
#debug concat("Calculated volume: ",str(returnVolume,0,2),"\n")
DebugVector3("Calculated CoM",returnCoM)
#debug "\n"

#debug "Sphere: \n"
#local sphereMass = 4/3*pi*pow(1.5,3);
#debug concat("Real moment about any priciple axis: ",str(2/5*sphereMass*pow(1.5,2),0,2),"\n")
#debug concat("Real volume: ",str(sphereMass,0,2),"\n")
FindObjectProperties(SphereObj,20,20,20,returnCoM,returnVolume,returnTensor)
#debug "Moment Tensor:\n"
DebugMatrix(returnTensor)
#debug concat("Calculated volume: ",str(returnVolume,0,2),"\n")
DebugVector3("Calculated CoM",returnCoM)
#debug "\n"

#debug "Torus: \n"
#local torusMass = 2*pow(pi,2)*1*pow(0.5,2);
#debug concat("Real moment about y axis: ",str(torusMass*(pow(1,2)+3/4*pow(0.5,2)),0,2),"\n")
#debug concat("Real volume: ",str(torusMass,0,2),"\n")
FindObjectProperties(TorusObj,20,20,20,returnCoM,returnVolume,returnTensor)
#debug "Moment Tensor:\n"
DebugMatrix(returnTensor)
#debug concat("Calculated volume: ",str(returnVolume,0,2),"\n")
DebugVector3("Calculated CoM",returnCoM)
#debug "\n"