/**
*
*/
package edu.stanford.rsl.conrad.geometry.motion;
import java.util.ArrayList;
import java.util.TreeMap;
import edu.stanford.rsl.conrad.geometry.General;
import edu.stanford.rsl.conrad.geometry.motion.timewarp.IdentityTimeWarper;
import edu.stanford.rsl.conrad.geometry.shapes.simple.PointND;
import edu.stanford.rsl.conrad.numerics.SimpleMatrix;
import edu.stanford.rsl.conrad.numerics.SimpleVector;
import edu.stanford.rsl.conrad.phantom.xcat.ViconAffineTransform;
import edu.stanford.rsl.conrad.phantom.xcat.ViconMarkerBuilder;
import edu.stanford.rsl.conrad.utils.Configuration;
import edu.stanford.rsl.conrad.utils.RegKeys;
/**
* @author Jang CHOI
*
*/
public class VICONMarkerMotionField extends SimpleMotionField {
/**
*
*/
private static final long serialVersionUID = -719945083192209758L;
protected ArrayList<ArrayList<Double>> markers;
protected int fieldNo;
private PointND[] beforePts = new PointND[2]; //===== XCAT
private PointND[] afterPts= new PointND[2]; //===== VICON
private ViconAffineTransform tmpTransformation1 = null;
private ViconAffineTransform tmpTransformation2 = null; // for dual transformation
private String part;
private String splineTitle;
private ViconMarkerBuilder vBuilder = null;
private double weight;
private boolean staticScene = false;
private int refProjection = 0;
private static TreeMap<String, TreeMap<Integer, Double>> boneScalingMemorizer;
private static TreeMap<String, TreeMap<Integer, SimpleMatrix>> boneGlobalTransformMemorizer;
public VICONMarkerMotionField(String transformPart, String spTitle, ViconMarkerBuilder viconBuilder, boolean staticScene, int projReference){
vBuilder = viconBuilder;
warp = new IdentityTimeWarper();
markers = vBuilder.getVICONMarkers();
part = transformPart;
splineTitle = spTitle;
weight = 0;
if (boneScalingMemorizer==null)
boneScalingMemorizer = new TreeMap<String, TreeMap<Integer,Double>>();
if (boneGlobalTransformMemorizer==null)
boneGlobalTransformMemorizer = new TreeMap<String, TreeMap<Integer,SimpleMatrix>>();
this.staticScene = staticScene;
refProjection = projReference;
//ctrlPnts = points;
}
private void addElementToScalingMemorizer(String part, int projectionNr, double scaling){
if (!boneScalingMemorizer.containsKey(part))
boneScalingMemorizer.put(part, new TreeMap<Integer,Double>());
boneScalingMemorizer.get(part).put(projectionNr, scaling);
}
private void addElementToTransformMemorizer(String part, int projectionNr, SimpleMatrix tform){
if (!boneGlobalTransformMemorizer.containsKey(part))
boneGlobalTransformMemorizer.put(part, new TreeMap<Integer,SimpleMatrix>());
boneGlobalTransformMemorizer.get(part).put(projectionNr, tform);
}
@Override
public PointND getPosition(PointND initialPosition, double initialTime, double time) {
boolean fKneeGapElimination = true; // want to eliminate the gap between the knee??
double kneeGapEliWeightKnee = 0.71; // for Subj5, Static60 = 0.67
double kneeGapEliWeightAnkle = 0.74; // for Subj5, Static60 = 0.70
double projections = Configuration.getGlobalConfiguration().getGeometry().getNumProjectionMatrices();
double viconSamplingRate = Double.parseDouble(Configuration.getGlobalConfiguration().getRegistryEntry(RegKeys.VICON_SAMPLING_RATE)); // 60Hz
double viconSkip = Double.parseDouble(Configuration.getGlobalConfiguration().getRegistryEntry(RegKeys.VICON_SKIP_SAMPLES)); // subj5, static60=200
// get 248 views for 8 seconds -> 248/8 = 31 Hz (For 400 views, 47.75??)
double projectionSamplingRate = Double.parseDouble(Configuration.getGlobalConfiguration().getRegistryEntry(RegKeys.PROJECTION_SAMPLING_RATE));
if (staticScene){
double currTime = ((double)refProjection+1.0)/(double)projections;
fieldNo = (int) (viconSkip + projections*(warp.warpTime(currTime) - warp.warpTime(initialTime))*(viconSamplingRate/projectionSamplingRate)); // vicon field #
}
else{
fieldNo = (int) (viconSkip + projections*(warp.warpTime(time) - warp.warpTime(initialTime))*(viconSamplingRate/projectionSamplingRate)); // vicon field #
}
int projectionNumber = (int) Math.round(warp.warpTime(time)*projections-1);
// System.out.println("Field #:\t" + fieldNo);
// fieldNo = 201; // subj5, static60=201, Static reference
// fieldNo = 651; // subj2, static60=651, Static reference
PointND p1 = null;
PointND p2 = null;
// for subj 5, squat 60
// double leftAxialRotDeg = -80.0;
// double rightAxialRotDeg = -95.0;
// for subj 2, squat 60
double leftAxialRotDeg = -85.0;
double rightAxialRotDeg = -90.0;
// temporary points
PointND leftPts = new PointND();
PointND rightPts = new PointND();
// // XCAT FemurTop
// PointND[] XcatFemurTop = new PointND[2];
// XcatFemurTop[0] = new PointND(233.3, 259.99, -166.63); // LeftFemurTop
// XcatFemurTop[1] = new PointND(57.72, 259.99, -166.63); // RightFemurTop
// // VICON FemurTop
// PointND[] ViconHJC= new PointND[2];
// ViconHJC[0] = new PointND(markers.get(fieldNo).get(vBuilder.colNoLHJCX()), markers.get(fieldNo).get(vBuilder.colNoLHJCY()), markers.get(fieldNo).get(vBuilder.colNoLHJCZ()));
// ViconHJC[1] = new PointND(markers.get(fieldNo).get(vBuilder.colNoRHJCX()), markers.get(fieldNo).get(vBuilder.colNoRHJCY()), markers.get(fieldNo).get(vBuilder.colNoRHJCZ()));
//
// additional transformation for patella
if (splineTitle.toLowerCase().contains("l_patella") || splineTitle.toLowerCase().contains("leftpatella")){
// conversion factor from "V Eijden 1986_Math model of patellofemoral joint Fig.5"
double viconKneeFlexionAngle = markers.get(fieldNo).get(vBuilder.colNoLKneeAngleX());
//double anglePatellaVsFemor = 0.34304*viconKneeFlexionAngle + 0.0063*Math.pow(viconKneeFlexionAngle, 2) - 3.07859*Math.pow(10,-5)*Math.pow(viconKneeFlexionAngle, 3) + 2.06145;
//double anglePatellaVsFemor = General.toRadians(0.6912*viconKneeFlexionAngle - 1.84588);
double anglePatellaVsFemor = General.toRadians(0.9*viconKneeFlexionAngle - 1.84588);
//System.out.println("[Flexion angle]\t(" + fieldNo + ")\t" + viconKneeFlexionAngle);
PointND leftPatellaRotCenter = new PointND(264.8, 264.6,-590.92);
//PointND leftPatellaRotCenter = new PointND(264.8, 264.6,-580.92);
RotationMotionField leftFemurMotion = new RotationMotionField(leftPatellaRotCenter, new SimpleVector(1,0,0), anglePatellaVsFemor);
initialPosition = leftFemurMotion.getPosition(initialPosition, 0, 1);
} else if (splineTitle.toLowerCase().contains("r_patella") || splineTitle.toLowerCase().contains("rightpatella")){
// conversion factor from "V Eijden 1986_Math model of patellofemoral joint Fig.5"
double viconKneeFlexionAngle = markers.get(fieldNo).get(vBuilder.colNoRKneeAngleX());
//double anglePatellaVsFemor = 0.34304*viconKneeFlexionAngle + 0.0063*Math.pow(viconKneeFlexionAngle, 2) - 3.07859*Math.pow(10,-5)*Math.pow(viconKneeFlexionAngle, 3) + 2.06145;
//double anglePatellaVsFemor = General.toRadians(0.6912*viconKneeFlexionAngle - 1.84588);
double anglePatellaVsFemor = General.toRadians(0.9*viconKneeFlexionAngle - 1.84588);
PointND rightPatellaRotCenter = new PointND(21.1, 269.1,-590.92);
//PointND rightPatellaRotCenter = new PointND(25.1, 269.1,-590.92);
RotationMotionField rightFemurMotion = new RotationMotionField(rightPatellaRotCenter, new SimpleVector(1,0,0), anglePatellaVsFemor);
initialPosition = rightFemurMotion.getPosition(initialPosition, 0, 1);
}
if (part == "LeftLower") {
beforePts[0] = new PointND(316.71, 293.02, -1011.74); // LeftTibiaBottom
beforePts[1] = new PointND(274.87, 275.59, -649.16+30); // LeftTibiaTop
//beforePts[1] = new PointND(274.87, 275.59, -632.25); // (LeftTibiaTop+LeftFemurBottom)/2 in Z
afterPts[0] = new PointND(markers.get(fieldNo).get(vBuilder.colNoLAJCX()), markers.get(fieldNo).get(vBuilder.colNoLAJCY()), markers.get(fieldNo).get(vBuilder.colNoLAJCZ()));
afterPts[1] = new PointND(markers.get(fieldNo).get(vBuilder.colNoLKJCX()), markers.get(fieldNo).get(vBuilder.colNoLKJCY()), markers.get(fieldNo).get(vBuilder.colNoLKJCZ()));
if (fKneeGapElimination)
{
leftPts = afterPts[1];
rightPts = new PointND(markers.get(fieldNo).get(vBuilder.colNoRKJCX()), markers.get(fieldNo).get(vBuilder.colNoRKJCY()), markers.get(fieldNo).get(vBuilder.colNoRKJCZ()));
afterPts[1].set(0, leftPts.get(0)*kneeGapEliWeightKnee + rightPts.get(0)*(1-kneeGapEliWeightKnee));
afterPts[1].set(1, leftPts.get(1)*kneeGapEliWeightKnee + rightPts.get(1)*(1-kneeGapEliWeightKnee));
afterPts[1].set(2, leftPts.get(2)*kneeGapEliWeightKnee + rightPts.get(2)*(1-kneeGapEliWeightKnee));
//System.out.println("[##LKJC## "+ fieldNo +"] " + afterPts[1].get(0) + ", " + afterPts[1].get(1) + ", " + afterPts[1].get(2));
leftPts = afterPts[0];
rightPts = new PointND(markers.get(fieldNo).get(vBuilder.colNoRAJCX()), markers.get(fieldNo).get(vBuilder.colNoRAJCY()), markers.get(fieldNo).get(vBuilder.colNoRAJCZ()));;
afterPts[0].set(0, leftPts.get(0)*kneeGapEliWeightAnkle + rightPts.get(0)*(1-kneeGapEliWeightAnkle));
afterPts[0].set(1, leftPts.get(1)*kneeGapEliWeightAnkle + rightPts.get(1)*(1-kneeGapEliWeightAnkle));
afterPts[0].set(2, leftPts.get(2)*kneeGapEliWeightAnkle + rightPts.get(2)*(1-kneeGapEliWeightAnkle));
}
// if (fieldNo==651){
// System.out.println(afterPts[1].get(0) + "\t" + afterPts[1].get(1) + "\t" + afterPts[1].get(2));
// }
tmpTransformation1 = new ViconAffineTransform(beforePts, afterPts, leftAxialRotDeg);
addElementToScalingMemorizer("LeftLower", projectionNumber, tmpTransformation1.getScalingMatrix().getElement(0, 0));
addElementToTransformMemorizer("LeftLower", projectionNumber, tmpTransformation1.getAffineTransformationMatrix());
p1 = tmpTransformation1.getTransformedPoints(initialPosition);
} else if (part == "RightLower") {
beforePts[0] = new PointND(-28.12, 295.11, -1013.33); // RightTibiaBottom
beforePts[1] = new PointND(10.90, 275.60, -650.46+30); // RightTibiaTop
//beforePts[1] = new PointND(10.90, 275.60, -632.9); // (RightTibiaTop+RightFemurBottom)/2 in Z
afterPts[0] = new PointND(markers.get(fieldNo).get(vBuilder.colNoRAJCX()), markers.get(fieldNo).get(vBuilder.colNoRAJCY()), markers.get(fieldNo).get(vBuilder.colNoRAJCZ()));
afterPts[1] = new PointND(markers.get(fieldNo).get(vBuilder.colNoRKJCX()), markers.get(fieldNo).get(vBuilder.colNoRKJCY()), markers.get(fieldNo).get(vBuilder.colNoRKJCZ()));
if (fKneeGapElimination)
{
leftPts = new PointND(markers.get(fieldNo).get(vBuilder.colNoLKJCX()), markers.get(fieldNo).get(vBuilder.colNoLKJCY()), markers.get(fieldNo).get(vBuilder.colNoLKJCZ()));
rightPts = afterPts[1];
afterPts[1].set(0, rightPts.get(0)*kneeGapEliWeightKnee + leftPts.get(0)*(1-kneeGapEliWeightKnee));
afterPts[1].set(1, rightPts.get(1)*kneeGapEliWeightKnee + leftPts.get(1)*(1-kneeGapEliWeightKnee));
afterPts[1].set(2, rightPts.get(2)*kneeGapEliWeightKnee + leftPts.get(2)*(1-kneeGapEliWeightKnee));
//System.out.println("[##RKJC## "+ fieldNo +"] " + afterPts[1].get(0) + ", " + afterPts[1].get(1) + ", " + afterPts[1].get(2));
leftPts = new PointND(markers.get(fieldNo).get(vBuilder.colNoLAJCX()), markers.get(fieldNo).get(vBuilder.colNoLAJCY()), markers.get(fieldNo).get(vBuilder.colNoLAJCZ()));
rightPts = afterPts[0];
afterPts[0].set(0, rightPts.get(0)*kneeGapEliWeightAnkle + leftPts.get(0)*(1-kneeGapEliWeightAnkle));
afterPts[0].set(1, rightPts.get(1)*kneeGapEliWeightAnkle + leftPts.get(1)*(1-kneeGapEliWeightAnkle));
afterPts[0].set(2, rightPts.get(2)*kneeGapEliWeightAnkle + leftPts.get(2)*(1-kneeGapEliWeightAnkle));
}
// if (fieldNo==651){
// System.out.println(afterPts[1].get(0) + "\t" + afterPts[1].get(1) + "\t" + afterPts[1].get(2));
// }
tmpTransformation1 = new ViconAffineTransform(beforePts, afterPts, rightAxialRotDeg);
addElementToScalingMemorizer("RightLower",projectionNumber,tmpTransformation1.getScalingMatrix().getElement(0, 0));
addElementToTransformMemorizer("RightLower", projectionNumber, tmpTransformation1.getAffineTransformationMatrix());
p1 = tmpTransformation1.getTransformedPoints(initialPosition);
} else if (part == "LeftUpper") {
//beforePts[0] = new PointND(264.52, 271.70, -615.34); // LeftFemurBottom
beforePts[0] = new PointND(264.52, 271.70, -632.25); // (LeftTibiaTop+LeftFemurBottom)/2 in Z
beforePts[1] = new PointND(233.3, 259.99, -166.63); // LeftFemurTop
afterPts[0] = new PointND(markers.get(fieldNo).get(vBuilder.colNoLKJCX()), markers.get(fieldNo).get(vBuilder.colNoLKJCY()), markers.get(fieldNo).get(vBuilder.colNoLKJCZ()));
afterPts[1] = new PointND(markers.get(fieldNo).get(vBuilder.colNoLHJCX()), markers.get(fieldNo).get(vBuilder.colNoLHJCY()), markers.get(fieldNo).get(vBuilder.colNoLHJCZ()));
if (fKneeGapElimination)
{
leftPts = afterPts[0];
rightPts = new PointND(markers.get(fieldNo).get(vBuilder.colNoRKJCX()), markers.get(fieldNo).get(vBuilder.colNoRKJCY()), markers.get(fieldNo).get(vBuilder.colNoRKJCZ()));
afterPts[0].set(0, leftPts.get(0)*kneeGapEliWeightKnee + rightPts.get(0)*(1-kneeGapEliWeightKnee));
afterPts[0].set(1, leftPts.get(1)*kneeGapEliWeightKnee + rightPts.get(1)*(1-kneeGapEliWeightKnee));
afterPts[0].set(2, leftPts.get(2)*kneeGapEliWeightKnee + rightPts.get(2)*(1-kneeGapEliWeightKnee));
}
tmpTransformation1 = new ViconAffineTransform(beforePts, afterPts, leftAxialRotDeg);
addElementToScalingMemorizer("LeftUpper",projectionNumber,tmpTransformation1.getScalingMatrix().getElement(0, 0));
addElementToTransformMemorizer("LeftUpper", projectionNumber, tmpTransformation1.getAffineTransformationMatrix());
p1 = tmpTransformation1.getTransformedPoints(initialPosition);
} else if (part == "RightUpper") {
//beforePts[0] = new PointND(22.60, 271.70, -615.34); // RightFemurBottom
beforePts[0] = new PointND(22.60, 271.70, -632.9); // (RightTibiaTop+RightFemurBottom)/2 in Z
beforePts[1] = new PointND(57.72, 259.99, -166.63); // RightFemurTop
afterPts[0] = new PointND(markers.get(fieldNo).get(vBuilder.colNoRKJCX()), markers.get(fieldNo).get(vBuilder.colNoRKJCY()), markers.get(fieldNo).get(vBuilder.colNoRKJCZ()));
afterPts[1] = new PointND(markers.get(fieldNo).get(vBuilder.colNoRHJCX()), markers.get(fieldNo).get(vBuilder.colNoRHJCY()), markers.get(fieldNo).get(vBuilder.colNoRHJCZ()));
if (fKneeGapElimination)
{
leftPts = new PointND(markers.get(fieldNo).get(vBuilder.colNoLKJCX()), markers.get(fieldNo).get(vBuilder.colNoLKJCY()), markers.get(fieldNo).get(vBuilder.colNoLKJCZ()));
rightPts = afterPts[0];
afterPts[0].set(0, rightPts.get(0)*kneeGapEliWeightKnee + leftPts.get(0)*(1-kneeGapEliWeightKnee));
afterPts[0].set(1, rightPts.get(1)*kneeGapEliWeightKnee + leftPts.get(1)*(1-kneeGapEliWeightKnee));
afterPts[0].set(2, rightPts.get(2)*kneeGapEliWeightKnee + leftPts.get(2)*(1-kneeGapEliWeightKnee));
}
tmpTransformation1 = new ViconAffineTransform(beforePts, afterPts, rightAxialRotDeg);
addElementToScalingMemorizer("RightUpper",projectionNumber,tmpTransformation1.getScalingMatrix().getElement(0, 0));
addElementToTransformMemorizer("RightUpper", projectionNumber, tmpTransformation1.getAffineTransformationMatrix());
p1 = tmpTransformation1.getTransformedPoints(initialPosition);
} else if (part == "LeftDual") {
double skinningWeightLimit = 70; // mm vertical distance from the joint (-642)
double jointPositionInZaxis = -642;
double ZLimitTop = jointPositionInZaxis + skinningWeightLimit;
double ZLimitBottom = jointPositionInZaxis - skinningWeightLimit;
double Zcurrent = initialPosition.get(2);
// left upper
//beforePts[0] = new PointND(264.52, 271.70, -615.34); // LeftFemurBottom
beforePts[0] = new PointND(264.52, 271.70, -632.25); // (LeftTibiaTop+LeftFemurBottom)/2 in Z
beforePts[1] = new PointND(233.3, 259.99, -166.63); // LeftFemurTop
afterPts[0] = new PointND(markers.get(fieldNo).get(vBuilder.colNoLKJCX()), markers.get(fieldNo).get(vBuilder.colNoLKJCY()), markers.get(fieldNo).get(vBuilder.colNoLKJCZ()));
afterPts[1] = new PointND(markers.get(fieldNo).get(vBuilder.colNoLHJCX()), markers.get(fieldNo).get(vBuilder.colNoLHJCY()), markers.get(fieldNo).get(vBuilder.colNoLHJCZ()));
if (fKneeGapElimination)
{
leftPts = afterPts[0];
rightPts = new PointND(markers.get(fieldNo).get(vBuilder.colNoRKJCX()), markers.get(fieldNo).get(vBuilder.colNoRKJCY()), markers.get(fieldNo).get(vBuilder.colNoRKJCZ()));
afterPts[0].set(0, leftPts.get(0)*kneeGapEliWeightKnee + rightPts.get(0)*(1-kneeGapEliWeightKnee));
afterPts[0].set(1, leftPts.get(1)*kneeGapEliWeightKnee + rightPts.get(1)*(1-kneeGapEliWeightKnee));
afterPts[0].set(2, leftPts.get(2)*kneeGapEliWeightKnee + rightPts.get(2)*(1-kneeGapEliWeightKnee));
}
tmpTransformation1 = new ViconAffineTransform(beforePts, afterPts, leftAxialRotDeg);
// left lower
beforePts[0] = new PointND(316.71, 293.02, -1011.74); // LeftTibiaBottom
beforePts[1] = new PointND(274.87, 275.59, -649.16+30); // LeftTibiaTop
//beforePts[1] = new PointND(274.87, 275.59, -632.25); // (LeftTibiaTop+LeftFemurBottom)/2 in Z
afterPts[0] = new PointND(markers.get(fieldNo).get(vBuilder.colNoLAJCX()), markers.get(fieldNo).get(vBuilder.colNoLAJCY()), markers.get(fieldNo).get(vBuilder.colNoLAJCZ()));
afterPts[1] = new PointND(markers.get(fieldNo).get(vBuilder.colNoLKJCX()), markers.get(fieldNo).get(vBuilder.colNoLKJCY()), markers.get(fieldNo).get(vBuilder.colNoLKJCZ()));
if (fKneeGapElimination)
{
leftPts = afterPts[1];
rightPts = new PointND(markers.get(fieldNo).get(vBuilder.colNoRKJCX()), markers.get(fieldNo).get(vBuilder.colNoRKJCY()), markers.get(fieldNo).get(vBuilder.colNoRKJCZ()));
afterPts[1].set(0, leftPts.get(0)*kneeGapEliWeightKnee + rightPts.get(0)*(1-kneeGapEliWeightKnee));
afterPts[1].set(1, leftPts.get(1)*kneeGapEliWeightKnee + rightPts.get(1)*(1-kneeGapEliWeightKnee));
afterPts[1].set(2, leftPts.get(2)*kneeGapEliWeightKnee + rightPts.get(2)*(1-kneeGapEliWeightKnee));
leftPts = afterPts[0];
rightPts = new PointND(markers.get(fieldNo).get(vBuilder.colNoRAJCX()), markers.get(fieldNo).get(vBuilder.colNoRAJCY()), markers.get(fieldNo).get(vBuilder.colNoRAJCZ()));;
afterPts[0].set(0, leftPts.get(0)*kneeGapEliWeightAnkle + rightPts.get(0)*(1-kneeGapEliWeightAnkle));
afterPts[0].set(1, leftPts.get(1)*kneeGapEliWeightAnkle + rightPts.get(1)*(1-kneeGapEliWeightAnkle));
afterPts[0].set(2, leftPts.get(2)*kneeGapEliWeightAnkle + rightPts.get(2)*(1-kneeGapEliWeightAnkle));
}
tmpTransformation2 = new ViconAffineTransform(beforePts, afterPts, leftAxialRotDeg);
p1 = tmpTransformation1.getTransformedPoints(initialPosition);
p2 = tmpTransformation2.getTransformedPoints(initialPosition);
if (Zcurrent >= ZLimitTop) weight = 1;
else if (Zcurrent < ZLimitBottom) weight = 0;
else weight = (Zcurrent - ZLimitBottom) / (ZLimitTop - ZLimitBottom);
p1.set(0, p1.get(0)*weight + p2.get(0)*(1-weight));
p1.set(1, p1.get(1)*weight + p2.get(1)*(1-weight));
p1.set(2, p1.get(2)*weight + p2.get(2)*(1-weight));
} else if (part == "RightDual") {
double skinningWeightLimit = 70; // mm vertical distance from the joint (-642)
double jointPositionInZaxis = -642;
double ZLimitTop = jointPositionInZaxis + skinningWeightLimit;
double ZLimitBottom = jointPositionInZaxis - skinningWeightLimit;
double Zcurrent = initialPosition.get(2);
// right upper
//beforePts[0] = new PointND(22.60, 271.70, -615.34); // RightFemurBottom
beforePts[0] = new PointND(22.60, 271.70, -632.9); // (RightTibiaTop+RightFemurBottom)/2 in Z
beforePts[1] = new PointND(57.72, 259.99, -166.63); // RightFemurTop
afterPts[0] = new PointND(markers.get(fieldNo).get(vBuilder.colNoRKJCX()), markers.get(fieldNo).get(vBuilder.colNoRKJCY()), markers.get(fieldNo).get(vBuilder.colNoRKJCZ()));
afterPts[1] = new PointND(markers.get(fieldNo).get(vBuilder.colNoRHJCX()), markers.get(fieldNo).get(vBuilder.colNoRHJCY()), markers.get(fieldNo).get(vBuilder.colNoRHJCZ()));
if (fKneeGapElimination)
{
leftPts = new PointND(markers.get(fieldNo).get(vBuilder.colNoLKJCX()), markers.get(fieldNo).get(vBuilder.colNoLKJCY()), markers.get(fieldNo).get(vBuilder.colNoLKJCZ()));
rightPts = afterPts[0];
afterPts[0].set(0, rightPts.get(0)*kneeGapEliWeightKnee + leftPts.get(0)*(1-kneeGapEliWeightKnee));
afterPts[0].set(1, rightPts.get(1)*kneeGapEliWeightKnee + leftPts.get(1)*(1-kneeGapEliWeightKnee));
afterPts[0].set(2, rightPts.get(2)*kneeGapEliWeightKnee + leftPts.get(2)*(1-kneeGapEliWeightKnee));
}
tmpTransformation1 = new ViconAffineTransform(beforePts, afterPts, rightAxialRotDeg);
// right lower
beforePts[0] = new PointND(-28.12, 295.11, -1013.33); // RightTibiaBottom
beforePts[1] = new PointND(10.90, 275.60, -650.46+30); // RightTibiaTop
//beforePts[1] = new PointND(10.90, 275.60, -632.9); // (RightTibiaTop+RightFemurBottom)/2 in Z
afterPts[0] = new PointND(markers.get(fieldNo).get(vBuilder.colNoRAJCX()), markers.get(fieldNo).get(vBuilder.colNoRAJCY()), markers.get(fieldNo).get(vBuilder.colNoRAJCZ()));
afterPts[1] = new PointND(markers.get(fieldNo).get(vBuilder.colNoRKJCX()), markers.get(fieldNo).get(vBuilder.colNoRKJCY()), markers.get(fieldNo).get(vBuilder.colNoRKJCZ()));
if (fKneeGapElimination)
{
leftPts = new PointND(markers.get(fieldNo).get(vBuilder.colNoLKJCX()), markers.get(fieldNo).get(vBuilder.colNoLKJCY()), markers.get(fieldNo).get(vBuilder.colNoLKJCZ()));
rightPts = afterPts[1];
afterPts[1].set(0, rightPts.get(0)*kneeGapEliWeightKnee + leftPts.get(0)*(1-kneeGapEliWeightKnee));
afterPts[1].set(1, rightPts.get(1)*kneeGapEliWeightKnee + leftPts.get(1)*(1-kneeGapEliWeightKnee));
afterPts[1].set(2, rightPts.get(2)*kneeGapEliWeightKnee + leftPts.get(2)*(1-kneeGapEliWeightKnee));
leftPts = new PointND(markers.get(fieldNo).get(vBuilder.colNoLAJCX()), markers.get(fieldNo).get(vBuilder.colNoLAJCY()), markers.get(fieldNo).get(vBuilder.colNoLAJCZ()));
rightPts = afterPts[0];
afterPts[0].set(0, rightPts.get(0)*kneeGapEliWeightAnkle + leftPts.get(0)*(1-kneeGapEliWeightAnkle));
afterPts[0].set(1, rightPts.get(1)*kneeGapEliWeightAnkle + leftPts.get(1)*(1-kneeGapEliWeightAnkle));
afterPts[0].set(2, rightPts.get(2)*kneeGapEliWeightAnkle + leftPts.get(2)*(1-kneeGapEliWeightAnkle));
}
tmpTransformation2 = new ViconAffineTransform(beforePts, afterPts, rightAxialRotDeg);
p1 = tmpTransformation1.getTransformedPoints(initialPosition);
p2 = tmpTransformation2.getTransformedPoints(initialPosition);
if (Zcurrent >= ZLimitTop) weight = 1;
else if (Zcurrent < ZLimitBottom) weight = 0;
else weight = (Zcurrent - ZLimitBottom) / (ZLimitTop - ZLimitBottom);
p1.set(0, p1.get(0)*weight + p2.get(0)*(1-weight));
p1.set(1, p1.get(1)*weight + p2.get(1)*(1-weight));
p1.set(2, p1.get(2)*weight + p2.get(2)*(1-weight));
}
return p1;
}
public TreeMap<String, TreeMap<Integer, Double>> getBoneScalingMemorizer() {
return boneScalingMemorizer;
}
public TreeMap<String, TreeMap<Integer, SimpleMatrix>> getBoneGlobalTransformMemorizer() {
return boneGlobalTransformMemorizer;
}
}
/*
* Copyright (C) 2010-2014 Jang-Hwan Choi
* CONRAD is developed as an Open Source project under the GNU General Public License (GPL).
*/