public static Matrix3D CalcRotationMatrix(double x, double y, double z, Point3D center, Vector3D up, Vector3D look, Transform3D transform, RotationType type)
{
//Transform3DGroup trm = new Transform3DGroup();
//trm.Children.Add(transform);
Vector3D realup = transform.Transform(up);
if (type != RotationType.LockAxisY)
{
up = realup;
}
if (type != RotationType.LockAxisZ)
{
look = transform.Transform(look);
}
center = transform.Transform(center);
Vector3D axisX = Vector3D.CrossProduct(up, look);
Matrix3D matrix = new Matrix3D();
//Quaternion q = new Quaternion();
//q.
double ang = AngleBetween(realup, YAxis) + x;
if (ang >= 90)
{
x = 90 - ang;
}
matrix.RotateAt(new Quaternion(axisX, x), center);
matrix.RotateAt(new Quaternion(up, y), center);
matrix.RotateAt(new Quaternion(look, z), center);
return matrix;
}
public static Matrix3D CalculateRotationMatrix(double x, double y, double z, Point3D center, Vector3D up, Vector3D look, Transform3D transform)
{
//Transform3DGroup trm = new Transform3DGroup();
//trm.Children.Add(transform);
up = transform.Transform(up);
look = transform.Transform(look);
center = transform.Transform(center);
Vector3D axisZ = Vector3D.CrossProduct(up, look);
Matrix3D matrix = new Matrix3D();
matrix.RotateAt(new Quaternion(axisZ, x), center);
matrix.RotateAt(new Quaternion(up, y), center);
matrix.RotateAt(new Quaternion(look, z), center);
return matrix;
}
public static MatrixTransform3D Rotate3D(Transform3D transform, Vector3D look, Vector3D dir, Point3D center)
{
Matrix3D m = new Matrix3D();
Vector3D realook = transform.Transform(look);
Vector3D axis = Vector3D.CrossProduct(realook, dir);
double angle = Math.Acos(Vector3D.DotProduct(realook, dir));
Quaternion q = new Quaternion(axis, angle);
m.RotateAt(q, center);
MatrixTransform3D rlt = transform as MatrixTransform3D;
return new MatrixTransform3D(Matrix3D.Multiply(rlt.Matrix, m));
}
public void TestOriginFinder(int sampleSize)
{
double error = 0;
double totalDur = 0;
Random randomMaker = new Random(DateTime.Now.Millisecond);
double maxErr = 0;
for (int j = 0; j < sampleSize; j++)
{
Point3D[] standardPoints = new Point3D[]
{
new Point3D(0, 0.5, 1.5), //top
new Point3D(0.5, 0.5, 1.5), //right
new Point3D(0, 0.5, 2), //front
new Point3D(-0.5, 0.25, 1.5) //back
};
Point3D[] capturePoints = new Point3D[standardPoints.Length];
Matrix3D matrix = new Matrix3D();
//matrix.Translate(new Vector3D(0, 10, 0));
double size = 1;
matrix.RotateAt(new Quaternion(new Vector3D((randomMaker.Next(1) - 1) * randomMaker.NextDouble() * size,
(randomMaker.Next(1) - 1) * randomMaker.NextDouble() * size,
randomMaker.NextDouble() * size),
(randomMaker.Next(1) - 1) * randomMaker.NextDouble() * 360),
new Point3D(0, 0, 0));
matrix.Translate(new Vector3D((randomMaker.Next(1) - 1) * randomMaker.NextDouble() * size,
(randomMaker.Next(1) - 1) * randomMaker.NextDouble() * size,
(randomMaker.Next(1) - 1) * randomMaker.NextDouble() * size));
for (int i = 0; i < standardPoints.Length; i++)
{
capturePoints[i] = matrix.Transform(standardPoints[i]);
}
testOrigin = matrix.Transform(new Point3D());
DateTime start = DateTime.Now.ToUniversalTime();
Point3D newOrigin = this.BruteForceEstimateOrigin(standardPoints, capturePoints, 5);
TimeSpan end = DateTime.Now.ToUniversalTime() - start;
int duration = (int)end.TotalSeconds;
totalDur += duration;
Vector3D og = new Vector3D(testOrigin.X, testOrigin.Y, testOrigin.Z);
Vector3D offset = new Vector3D(testOrigin.X - newOrigin.X, testOrigin.Y - newOrigin.Y, testOrigin.Z - newOrigin.Z);
error += offset.Length;
if (maxErr < offset.Length)
{
maxErr = offset.Length;
Console.WriteLine("Max Error: {1}", j, Math.Round(maxErr, 6));
}
}
double avgErr = error / sampleSize;
double avgDur = totalDur / sampleSize;
Console.WriteLine("Average Error: {0}\t Max Error: {1}", avgErr, maxErr);
}
/// <summary>
/// Rotates the point cloud by a given angle
/// </summary>
/// <param name="axis">The axis of rotation</param>
/// <param name="angle">The angle to which te point cloud is to be rotated</param>
public void rotate(double[] axis, double angle)
{
if (!(axis.Length != 3)) {
//centre of rotation
Point3D centre = new Point3D(axis[0], axis[1], axis[2]);
//pull out the entire tree
PARSE.ICP.PointRGB[] pts = this.getAllPoints();
//create a new kd tree
KdTree.KDTree newPoints = new KdTree.KDTree(3);
//iterate over every point and translate + jam in new tree
foreach (PARSE.ICP.PointRGB point in pts)
{
//create rot matrix
Matrix3D mtx = new Matrix3D();
Quaternion q = new Quaternion(new Vector3D(0, 1, 0), angle);
mtx.RotateAt(q, centre);
//complete rotation
Point3D newPoint = mtx.Transform(point.point);
//check if the x, y and z max and min coords need updating
//check min values
if (newPoint.X < minx) { minx = newPoint.X; }
if (newPoint.Y < miny) { miny = newPoint.Y; }
if (newPoint.Z < minz) { minz = newPoint.Z; }
//check max values
if (newPoint.X > maxx) { maxx = newPoint.X; }
if (newPoint.Y > maxy) { maxy = newPoint.Y; }
if (newPoint.Z > maxz) { maxz = newPoint.Z; }
//jam into the tree hole
double[] key = new double[3] { newPoint.X, newPoint.Y, newPoint.Z };
newPoints.insert(key, new PARSE.ICP.PointRGB(newPoint, point.r, point.g, point.b));
}
//replace the old kd tree with the new one
this.points = newPoints;
}
else{
//throw an exception and annoy Bernie in the process ;)
}
}
private Vector3D CalculateHeadOrientation(WagSkeleton skeleton)
{
Vector3D headOrientation = new Vector3D(0, 0, -1);
FaceTrackFrame face = skeleton.FaceFrame;
var FacePoints = face.Get3DShape();
Vector3DF eyeLeft = FacePoints[Constants.LEFT_EYE];
Vector3DF eyeRight = FacePoints[Constants.RIGHT_EYE];
Vector3DF faceTop = FacePoints[Constants.FACE_TOP];
Vector3DF faceBottom = FacePoints[Constants.FACE_BOTTOM];
Vector3D faceVectorHorizontal = new Vector3D(eyeLeft.X - eyeRight.X, eyeLeft.Y - eyeRight.Y, eyeLeft.Z - eyeRight.Z);
Vector3D faceVectorVertical = new Vector3D(faceTop.X - faceBottom.X, faceTop.Y - faceBottom.Y, faceTop.Z - faceBottom.Z);
headOrientation = Vector3D.CrossProduct(faceVectorHorizontal, faceVectorVertical);
headOrientation = Calibration.CalibrateTransform.Transform(headOrientation);
headOrientation.Normalize();
Matrix3D headPointsPointUpMatrix = new Matrix3D();
headPointsPointUpMatrix.RotateAt(new System.Windows.Media.Media3D.Quaternion(new Vector3D(int.MaxValue, 0, 0), -20), skeleton.TransformedJoints[JointType.Head].Position.ToPoint3D());
Vector3D lowered = headPointsPointUpMatrix.Transform(headOrientation);
return lowered;
}
private Vector3D CalculateHeadOrientation(WagSkeleton skeleton)
{
Vector3D headOrientation = new Vector3D(0, 0, -1);
FaceTrackFrame face = skeleton.FaceFrame;
var FacePoints = face.Get3DShape();
Vector3DF eyeLeft = FacePoints[LEFT_EYE];
Vector3DF eyeRight = FacePoints[RIGHT_EYE];
Vector3DF faceTop = FacePoints[FACE_TOP];
Vector3DF faceBottom = FacePoints[FACE_BOTTOM];
Vector3D faceVectorHorizontal = new Vector3D(eyeLeft.X - eyeRight.X, eyeLeft.Y - eyeRight.Y, eyeLeft.Z - eyeRight.Z);
Vector3D faceVectorVertical = new Vector3D(faceTop.X - faceBottom.X, faceTop.Y - faceBottom.Y, faceTop.Z - faceBottom.Z);
headOrientation = Vector3D.CrossProduct(faceVectorHorizontal, faceVectorVertical);
headOrientation = originTransform.Transform(headOrientation);
headOrientation.Normalize();
Matrix3D headPointsPointUpMatrix = new Matrix3D();
headPointsPointUpMatrix.RotateAt(new Quaternion(new Vector3D(1, 0, 0), -20), skeleton.TransformedJoints[JointType.Head].Position.ToPoint3D());
Vector3D lowered = headPointsPointUpMatrix.Transform(headOrientation);
if (headOrientation.Z > 0)
throw new Exception("Right hand rule violation");
return lowered;
}
private void MainForm_Click(object sender, EventArgs e)
{
Console.Clear();
#region Create source data.
for (int i = 0; i < pointsCountForFill; ++i) {
this.sourcePoints[i] = new Point3D(
this.rand.Next(Math.Max(this.ClientSize.Width, this.ClientSize.Height)),
this.rand.Next(Math.Max(this.ClientSize.Width, this.ClientSize.Height)),
this.rand.Next(Math.Max(this.ClientSize.Width, this.ClientSize.Height)));
}
for (int i = pointsCountForFill; i < pointsCount; ++i) {
this.sourcePoints[i] = new Point3D(
this.rand.Next(Math.Max(this.ClientSize.Width, this.ClientSize.Height)),
this.rand.Next(Math.Max(this.ClientSize.Width, this.ClientSize.Height)),
this.rand.Next(Math.Max(this.ClientSize.Width, this.ClientSize.Height)));
}
#endregion Create source data.
#region Create target data.
Matrix3D matrix3D = new Matrix3D();
matrix3D.Translate(new Vector3D(10, 50, 20));
matrix3D.RotateAt(new Quaternion(new Vector3D(2, 9, 1), 20), new Point3D(30, 30, 70));
matrix3D.Translate(new Vector3D(10, 50, 20));
matrix3D.RotateAt(new Quaternion(new Vector3D(1, 3, 5), -5), new Point3D(10, 0, 10));
Console.WriteLine("{0,30}{1,30}{2,30}{3,30}", matrix3D.M11, matrix3D.M21, matrix3D.M31, matrix3D.OffsetX);
Console.WriteLine("{0,30}{1,30}{2,30}{3,30}", matrix3D.M12, matrix3D.M22, matrix3D.M32, matrix3D.OffsetY);
Console.WriteLine("{0,30}{1,30}{2,30}{3,30}", matrix3D.M13, matrix3D.M23, matrix3D.M33, matrix3D.OffsetZ);
for (int i = 0; i < pointsCount; ++i) {
this.targetPoints[i] = matrix3D.Transform(this.sourcePoints[i]);
}
Point3D[] sourcePointsToFill = new Point3D[pointsCountForFill];
Point3D[] targetPointsToFill = new Point3D[pointsCountForFill];
for (int i = 0; i < pointsCountForFill; ++i) {
sourcePointsToFill[i] = this.sourcePoints[i];
targetPointsToFill[i] = this.targetPoints[i];
}
AffineCoorTransformator ct = new AffineCoorTransformator();
ct.Fill(sourcePointsToFill, targetPointsToFill);
double[] matrix = ct.MatrixCopy;
int index = 0;
for (int i = 0; i < AffineCoorTransformator.dim; ++i) {
for (int j = 0; j < AffineCoorTransformator.dimExt; ++j) {
Console.Write("{0,30}", matrix[index]);
++index;
}
Console.WriteLine();
}
Console.WriteLine();
#endregion Create target data.
double[] error = new double[pointsCount];
double totalError = 0;
for (int i = 0; i < pointsCount; ++i) {
this.targetPointsRes[i] = ct.Transform(this.sourcePoints[i]);
error[i] = (this.targetPointsRes[i] - this.targetPoints[i]).Length;
Console.WriteLine("Error {0}: {1}.", i, error[i]);
totalError += error[i];
}
double totalErrorMax = pointsCount * eps * Math.Sqrt(AffineCoorTransformator.dim);
Console.WriteLine();
Console.WriteLine("Max: {0}. Current: {1}. Middle: {2}", totalErrorMax, totalError, totalError / pointsCount);
Console.WriteLine();
this.Invalidate();
}
//taken from http://stackoverflow.com/questions/2042214/wpf-3d-rotate-a-model-around-its-own-axes
//adjusted for offset by Dawid Rusin
private Matrix3D CalculateRotationMatrix(double x, double y, double z)
{
Matrix3D matrix = new Matrix3D();
matrix.RotateAt(new Quaternion(new Vector3D(1, 0, 0), x), new Point3D(offsetX, offsetY, offsetZ));
matrix.RotateAt(new Quaternion(new Vector3D(0, 1, 0) * matrix, y), new Point3D(offsetX, offsetY, offsetZ));
matrix.RotateAt(new Quaternion(new Vector3D(0, 0, 1) * matrix, z), new Point3D(offsetX, offsetY, offsetZ));
return matrix;
}
public static MatrixTransform3D Rotate3D(Transform3D transform, double x, double y, double z, Point3D center, Vector3D up, Vector3D look, RotationType type)
{
if (type != RotationType.LockAxisY)
{
up = transform.Transform(up);
}
if (type != RotationType.LockAxisZ)
{
look = transform.Transform(look);
}
center = transform.Transform(center);
Vector3D axisX = Vector3D.CrossProduct(up, look);
Matrix3D matrix = new Matrix3D();
matrix.RotateAt(new Quaternion(axisX, x), center);
matrix.RotateAt(new Quaternion(up, y), center);
matrix.RotateAt(new Quaternion(look, z), center);
MatrixTransform3D mOriginTransform = transform as MatrixTransform3D;
//mOriginTransform.Matrix.RotateAt(
try
{
return new MatrixTransform3D(Matrix3D.Multiply(mOriginTransform.Matrix, matrix));
}
catch (Exception err)
{
Exceptions.LogOnly(err);
return null;
}
}
