BodyUtilities.h

#ifndef BODYUTITILIES_H_INCLUDED
#define BODYUTITILIES_H_INCLUDED

#include "SpaceVector.h"
#include "SpaceMatrix.h"
#include "MatrixUtilities.h"
#include "SpaceBody.h"

SpaceBody transformBody(SpaceBody &source,SpaceMatrix &transformation){
    SpaceBody target(source.getRowCount(),source.getColumnCount());
    for(int row=0;row<source.getRowCount();row++){
         for(int column=0;column<source.getColumnCount();column++){
            SpaceVector to= transformation * source.get(row,column);
            target.set(row,column,to.getX(),to.getY(),to.getZ(),to.getW());
        }
    }
    return target;
}
SpaceBody createCube(SpaceVector &center,SpaceVector &magnitude){
    SpaceBody body(2,4);
    SpaceVector from= center - magnitude/2;
    SpaceVector to= center + magnitude/2;
    body.set(0,0,from.getX(),from.getY(),from.getZ());
    body.set(0,1,to.getX(),from.getY(),from.getZ());
    body.set(0,2,from.getX(),to.getY(),from.getZ());
    body.set(0,3,to.getX(),to.getY(),from.getZ());
    body.set(1,0,from.getX(),from.getY(),to.getZ());
    body.set(1,1,to.getX(),from.getY(),to.getZ());
    body.set(1,2,from.getX(),to.getY(),to.getZ());
    body.set(1,3,to.getX(),to.getY(),to.getZ());
    return body;
}


#endif