/// <summary>
/// Sets the rotation and transformation matrix so that it is 3 roations about
/// the given center point. Rotation angles are applied in the order Z, Y, X
/// </summary>
/// <param name="angles">Amount of rotation (in radians) for rotating about the X, Y, and Z axis</param>
/// <param name="center">The center point about which to rotate</param>
/// <exception cref="System.ArgumentException">Invalid parameters</exception>
public void rotateAboutCenter(double[] angles, double[] center)
{
if (angles.Length != 3)
{
throw new ArgumentException("Must pass 3 angles");
}
if (center.Length != 3)
{
throw new ArgumentException("Center point must have 3 values");
}
TransformMatrix rotMat;
TransformMatrix transMat = new TransformMatrix();
TransformMatrix transMatInverse = new TransformMatrix();
TransformMatrix rotX = new TransformMatrix();
TransformMatrix rotY = new TransformMatrix();
TransformMatrix rotZ = new TransformMatrix();
rotX.rotate(Axes.X, angles[0]);
rotY.rotate(Axes.Y, angles[1]);
rotZ.rotate(Axes.Z, angles[2]);
rotMat = rotX * rotY * rotZ;
transMat.setTranslation(center);
transMatInverse.setTranslation(-center[0], -center[1], -center[2]);
TransformMatrix final = transMat * rotMat * transMatInverse;
SetMatrix(final);
}
public void createFromQuaternions(double q0, double q1, double q2, double q3, double[] center)
{
if (center.Length != 3)
{
throw new ArgumentException("Center point must have 3 values");
}
TransformMatrix rotMat = new TransformMatrix();
TransformMatrix transMat = new TransformMatrix();
TransformMatrix transMatInverse = new TransformMatrix();
rotMat.quaternionToRotationMatrix(q0, q1, q2, q3);
transMat.setTranslation(center);
transMatInverse.setTranslation(-center[0], -center[1], -center[2]);
TransformMatrix final = transMat * rotMat * transMatInverse;
SetMatrix(final);
}
private TransformMatrix readSingleTransform(StreamReader filestream)
{
const string centerRotationRegex = @"^\s*center\ of\ rotation\:\s+([-\d\.e+]+)\s+([-\d\.e+]+)\s+([-\d\.e+]+)\s*$";
const string rotationAnglesRegex = @"^\s*rotation\ angles\:\s+([-\d\.e+]+)\s+([-\d\.e+]+)\s+([-\d\.e+]+)\s*$";
const string translationRegex = @"^\s*translation\ value\:\s+([-\d\.e+]+)\s+([-\d\.e+]+)\s+([-\d\.e+]+)\s*$";
const string translationBothRegex = @"^\s*translation\:\s+([-\d\.e+]+)\s+([-\d\.e+]+)\s+([-\d\.e+]+)\s*$";
//lets figure out the transform type
TransformMatrix rt = new TransformMatrix();
string transformLine = filestream.ReadLine();
string transformType = Regex.Match(transformLine, @"^Transform\ type\:\ (.*)$").Groups[1].Value.Trim();
switch (transformType)
{
case "default rotation":
_lastTransformWasOptimizedBoth = false;
double[] centerRotation = new double[3];
double[] rotationAngles = new double[3];
string centRotationLine = filestream.ReadLine();
Match m = Regex.Match(centRotationLine, centerRotationRegex);
centerRotation[0] = Double.Parse(m.Groups[1].Value);
centerRotation[1] = Double.Parse(m.Groups[2].Value);
centerRotation[2] = Double.Parse(m.Groups[3].Value);
string rotationAnglesLines = filestream.ReadLine();
m = Regex.Match(rotationAnglesLines, rotationAnglesRegex);
rotationAngles[0] = Double.Parse(m.Groups[1].Value);
rotationAngles[1] = Double.Parse(m.Groups[2].Value);
rotationAngles[2] = Double.Parse(m.Groups[3].Value);
rt.rotateAboutCenter(rotationAngles, centerRotation);
break;
case "default translation":
_lastTransformWasOptimizedBoth = false;
double[] translation = new double[3];
string translationLine = filestream.ReadLine();
m = Regex.Match(translationLine, translationRegex);
translation[0] = Double.Parse(m.Groups[1].Value);
translation[1] = Double.Parse(m.Groups[2].Value);
translation[2] = Double.Parse(m.Groups[3].Value);
rt.setTranslation(translation);
break;
case "optimized both":
_lastTransformWasOptimizedBoth = true;
centerRotation = new double[3];
rotationAngles = new double[3];
translation = new double[3];
centRotationLine = filestream.ReadLine();
m = Regex.Match(centRotationLine, centerRotationRegex);
centerRotation[0] = Double.Parse(m.Groups[1].Value);
centerRotation[1] = Double.Parse(m.Groups[2].Value);
centerRotation[2] = Double.Parse(m.Groups[3].Value);
rotationAnglesLines = filestream.ReadLine();
m = Regex.Match(rotationAnglesLines, rotationAnglesRegex);
rotationAngles[0] = Double.Parse(m.Groups[1].Value);
rotationAngles[1] = Double.Parse(m.Groups[2].Value);
rotationAngles[2] = Double.Parse(m.Groups[3].Value);
translationLine = filestream.ReadLine();
m = Regex.Match(translationLine, translationBothRegex);
translation[0] = Double.Parse(m.Groups[1].Value);
translation[1] = Double.Parse(m.Groups[2].Value);
translation[2] = Double.Parse(m.Groups[3].Value);
TransformMatrix rot = new TransformMatrix();
TransformMatrix t = new TransformMatrix();
rot.rotateAboutCenter(rotationAngles, centerRotation);
t.setTranslation(translation);
rt = t * rot;
_currentOptimizedBothTransform = new EuclideanTransform();
_currentOptimizedBothTransform.CenterRotation = centerRotation;
_currentOptimizedBothTransform.Rotation = rotationAngles;
_currentOptimizedBothTransform.Translation = translation;
break;
default:
//error, there should be no other type
throw new FormatException("Invalid format for transform file. Unknown transform type: " + transformType);
}
return(rt);
}
