package edu.stanford.rsl.conrad.phantom.xcat;
import java.util.ArrayList;
import edu.stanford.rsl.conrad.geometry.General;
import edu.stanford.rsl.conrad.geometry.Rotations;
import edu.stanford.rsl.conrad.geometry.shapes.simple.PointND;
import edu.stanford.rsl.conrad.numerics.SimpleMatrix;
import edu.stanford.rsl.conrad.numerics.SimpleOperators;
import edu.stanford.rsl.conrad.numerics.SimpleVector;
import edu.stanford.rsl.conrad.utils.CONRAD;
/**
* @author jhchoi21
*
*/
public class ViconAffineTransform{
private SimpleMatrix scalingMatrix;
private SimpleMatrix translationMatrix1;
private SimpleMatrix translationMatrix2;
private SimpleMatrix translationMatrix3;
private SimpleMatrix rotationAboutAxisMatrix;
private SimpleMatrix rotationAboutOwnAxisMatrix;
private SimpleMatrix affineTransformMatrix;
//private SimpleMatrix rotationAboutAxisMatrixPatella;
public ViconAffineTransform(){
}
public ViconAffineTransform(PointND[] beforePts, PointND[] afterPts) {
}
public ViconAffineTransform(PointND[] beforePts, PointND[] afterPts, double deg) {
// shift starting vector to origin
double[][] translationArray1 =
{{1, 0, 0, -beforePts[0].get(0)},
{0, 1, 0, -beforePts[0].get(1)},
{0, 0, 1, -beforePts[0].get(2)},
{0, 0, 0, 1}};
translationMatrix1 = new SimpleMatrix(translationArray1);
// rotate onto director of reference vector
SimpleVector beforeVector = // XCAT
new SimpleVector(beforePts[1].get(0)-beforePts[0].get(0), beforePts[1].get(1)-beforePts[0].get(1), beforePts[1].get(2)-beforePts[0].get(2));
SimpleVector afterVector = // VICON
new SimpleVector(afterPts[1].get(0)-afterPts[0].get(0), afterPts[1].get(1)-afterPts[0].get(1),afterPts[1].get(2)-afterPts[0].get(2));
SimpleVector beforeVectorNormed = beforeVector.normalizedL2();
SimpleVector afterVectorNormed = afterVector.normalizedL2();
SimpleVector crossVector = General.crossProduct(beforeVectorNormed, afterVectorNormed);
SimpleVector crossVectorNormed = crossVector.normalizedL2();
SimpleVector crossVectorCrossBeforeNormed = General.crossProduct(crossVectorNormed, beforeVectorNormed);
SimpleVector crossVectorCrossAfterNormed = General.crossProduct(crossVectorNormed, afterVectorNormed);
double[][] subRotationArray1 =
{{beforeVectorNormed.getElement(0), beforeVectorNormed.getElement(1), beforeVectorNormed.getElement(2)},
{crossVectorCrossBeforeNormed.getElement(0), crossVectorCrossBeforeNormed.getElement(1), crossVectorCrossBeforeNormed.getElement(2)},
{crossVectorNormed.getElement(0), crossVectorNormed.getElement(1), crossVectorNormed.getElement(2)}};
SimpleMatrix subRotationMatrix1 = new SimpleMatrix(subRotationArray1);
double[][] subRotationArray2 =
{{afterVectorNormed.getElement(0), crossVectorCrossAfterNormed.getElement(0), crossVectorNormed.getElement(0)},
{afterVectorNormed.getElement(1), crossVectorCrossAfterNormed.getElement(1), crossVectorNormed.getElement(1)},
{afterVectorNormed.getElement(2), crossVectorCrossAfterNormed.getElement(2), crossVectorNormed.getElement(2)}};
SimpleMatrix subRotationMatrix2 = new SimpleMatrix(subRotationArray2);
SimpleMatrix rotationAboutAxisMatrixSub = SimpleOperators.multiplyMatrixProd(subRotationMatrix2, subRotationMatrix1);
rotationAboutAxisMatrix = new SimpleMatrix(4, 4);
rotationAboutAxisMatrix.setElementValue(0, 3, 0);
rotationAboutAxisMatrix.setElementValue(1, 3, 0);
rotationAboutAxisMatrix.setElementValue(2, 3, 0);
rotationAboutAxisMatrix.setElementValue(3, 3, 1);
rotationAboutAxisMatrix.setElementValue(3, 0, 0);
rotationAboutAxisMatrix.setElementValue(3, 1, 0);
rotationAboutAxisMatrix.setElementValue(3, 2, 0);
rotationAboutAxisMatrix.setSubMatrixValue(0, 0, rotationAboutAxisMatrixSub);
// rotate about axis of two vectors
//SimpleVector rotationAxisVector = General.crossProduct(beforeVector, afterVector);
//double angleVectors = General.angle(beforeVector, afterVector);
double angleVectors = General.toRadians(deg);
SimpleMatrix rotationAboutOwnAxisMatrixSub = Rotations.createRotationMatrixAboutAxis(afterVectorNormed, angleVectors);
rotationAboutOwnAxisMatrix = new SimpleMatrix(4, 4);
rotationAboutOwnAxisMatrix.setElementValue(0, 3, 0);
rotationAboutOwnAxisMatrix.setElementValue(1, 3, 0);
rotationAboutOwnAxisMatrix.setElementValue(2, 3, 0);
rotationAboutOwnAxisMatrix.setElementValue(3, 3, 1);
rotationAboutOwnAxisMatrix.setElementValue(3, 0, 0);
rotationAboutOwnAxisMatrix.setElementValue(3, 1, 0);
rotationAboutOwnAxisMatrix.setElementValue(3, 2, 0);
rotationAboutOwnAxisMatrix.setSubMatrixValue(0, 0, rotationAboutOwnAxisMatrixSub);
// scale to length of reference vector
double scalingFactor = afterVector.normL2()/beforeVector.normL2();
double[][] scalingArray =
{{scalingFactor , 0, 0, 0},
{0, scalingFactor, 0, 0},
{0, 0, scalingFactor, 0},
{0, 0, 0, 1}};
scalingMatrix = new SimpleMatrix(scalingArray);
// shift to start point of reference vector
double[][] translationArray2 =
{{1, 0, 0, afterPts[0].get(0)},
{0, 1, 0, afterPts[0].get(1)},
{0, 0, 1, afterPts[0].get(2)},
{0, 0, 0, 1}};
translationMatrix2 = new SimpleMatrix(translationArray2);
affineTransformMatrix = new SimpleMatrix(4, 4);
//affineTransformMatrix = SimpleOperators.multiplyMatrixProd(translationMatrix2, SimpleOperators.multiplyMatrixProd(scalingMatrix, SimpleOperators.multiplyMatrixProd(rotationAboutAxisMatrix, translationMatrix1)));
affineTransformMatrix = SimpleOperators.multiplyMatrixProd(translationMatrix2, SimpleOperators.multiplyMatrixProd(scalingMatrix, SimpleOperators.multiplyMatrixProd(rotationAboutOwnAxisMatrix, SimpleOperators.multiplyMatrixProd(rotationAboutAxisMatrix, translationMatrix1))));
}
public static void main(String[] args) {
// read global configuration
CONRAD.setup();
PointND[] beforePts = new PointND[2];
beforePts[0] = new PointND(273.8, 273.65,-649.57);
beforePts[1] = new PointND(309.89, 287.18,-1033.07);
// after affine transformation
PointND[] afterPts = new PointND[2];
afterPts[0] = new PointND(-292.17, 27.22, 445.38);
afterPts[1] = new PointND(-156.79, 41.466, 75.859);
// PointND[] beforePts = new PointND[2];
// beforePts[0] = new PointND(1, 1, 0);
// beforePts[1] = new PointND(1, 2, 0);
//
// // after affine transformation
// PointND[] afterPts = new PointND[2];
// afterPts[0] = new PointND(2, 1, 5);
// afterPts[1] = new PointND(4, 1, 0);
ViconAffineTransform affineTrans = new ViconAffineTransform(beforePts, afterPts);
// // test
// PointND rlt = affineTrans.getTransformedPoints(beforePts[1]);
// System.out.println(afterPts[1]);
// System.out.println(rlt);
}
public ArrayList<PointND> getTransformedPoints(ArrayList<PointND> beforePts){
ArrayList<PointND> afterPts = new ArrayList<PointND>();
SimpleVector beforeVector = new SimpleVector(0, 0, 0, 0);
SimpleVector afterVector = new SimpleVector(0, 0, 0, 0);
for (int i=0; i<beforePts.size(); i++){
beforeVector.setElementValue(0, beforePts.get(i).get(0));
beforeVector.setElementValue(1, beforePts.get(i).get(1));
beforeVector.setElementValue(2, beforePts.get(i).get(2));
beforeVector.setElementValue(3, 1);
afterVector = SimpleOperators.multiply(affineTransformMatrix, beforeVector);
afterPts.add(i, new PointND(afterVector.getElement(0)/afterVector.getElement(3), afterVector.getElement(1)/afterVector.getElement(3), afterVector.getElement(2)/afterVector.getElement(3)));
}
return afterPts;
}
public PointND getTransformedPoints(PointND beforePts){
PointND afterPts;
SimpleVector beforeVector = new SimpleVector(0, 0, 0, 0);
SimpleVector afterVector = new SimpleVector(0, 0, 0, 0);
beforeVector.setElementValue(0, beforePts.get(0));
beforeVector.setElementValue(1, beforePts.get(1));
beforeVector.setElementValue(2, beforePts.get(2));
beforeVector.setElementValue(3, 1);
afterVector = SimpleOperators.multiply(affineTransformMatrix, beforeVector);
afterPts = new PointND(afterVector.getElement(0)/afterVector.getElement(3), afterVector.getElement(1)/afterVector.getElement(3), afterVector.getElement(2)/afterVector.getElement(3));
return afterPts;
}
public SimpleMatrix getAffineTransformationMatrix (){
return affineTransformMatrix;
}
public void setPatellaRotationMatrix(PointND KNE, PointND KJC, double deg){
SimpleVector axisVector = new SimpleVector(KJC.get(0)-KNE.get(0), KJC.get(1)-KNE.get(1), KJC.get(2)-KNE.get(2));
SimpleVector axisVectorNormed = axisVector.normalizedL2();
SimpleMatrix rotationAboutOwnAxisMatrix;
double angle = General.toRadians(deg);
SimpleMatrix rotationAboutOwnAxisMatrixSub = Rotations.createRotationMatrixAboutAxis(axisVectorNormed, angle);
rotationAboutOwnAxisMatrix = new SimpleMatrix(4, 4);
rotationAboutOwnAxisMatrix.setElementValue(0, 3, 0);
rotationAboutOwnAxisMatrix.setElementValue(1, 3, 0);
rotationAboutOwnAxisMatrix.setElementValue(2, 3, 0);
rotationAboutOwnAxisMatrix.setElementValue(3, 3, 1);
rotationAboutOwnAxisMatrix.setElementValue(3, 0, 0);
rotationAboutOwnAxisMatrix.setElementValue(3, 1, 0);
rotationAboutOwnAxisMatrix.setElementValue(3, 2, 0);
rotationAboutOwnAxisMatrix.setSubMatrixValue(0, 0, rotationAboutOwnAxisMatrixSub);
affineTransformMatrix = SimpleOperators.multiplyMatrixProd(rotationAboutOwnAxisMatrix, affineTransformMatrix);
}
public SimpleMatrix getScalingMatrix() {
return scalingMatrix;
}
}
/*
* Copyright (C) 2010-2014 Jang-Hwan Choi
* CONRAD is developed as an Open Source project under the GNU General Public License (GPL).
*/