/// <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); }