public static System.Windows.Media.Media3D.Matrix3D SetTilt(
this System.Windows.Media.Media3D.Matrix3D matrix, double tilt)
{
Vector3D translation = new System.Windows.Media.Media3D.Vector3D(matrix.OffsetX, matrix.OffsetY, matrix.OffsetZ);
/*
* var orientation = matrix.GetOrientation();
* var spin = matrix.GetSpin();
*
* var newMatrix = new Matrix3D();
* newMatrix = newMatrix.RotateOTS(new Vector3D(orientation, tilt,spin));
* newMatrix.Translate(translation);
* return newMatrix;*/
// backoff translation
matrix.Translate(new System.Windows.Media.Media3D.Vector3D(-translation.X, -translation.Y, -translation.Z));
Vector3D localX = matrix.Transform(new System.Windows.Media.Media3D.Vector3D(1, 0, 0));
// backoff exiting tilt
matrix.Rotate(new System.Windows.Media.Media3D.Quaternion(localX, matrix.GetTilt() * -1.0));
matrix.Rotate(new System.Windows.Media.Media3D.Quaternion(localX, tilt));
// add back translation
matrix.Translate(translation);
return(matrix);
}
public static System.Windows.Media.Media3D.Matrix3D SetOrientation(this System.Windows.Media.Media3D.Matrix3D matrix, double orientationDegrees)
{
Vector3D translation = new System.Windows.Media.Media3D.Vector3D(matrix.OffsetX, matrix.OffsetY, matrix.OffsetZ);
// backoff translation
matrix.Translate(new System.Windows.Media.Media3D.Vector3D(-translation.X, -translation.Y, -translation.Z));
// back off any existing Z rotation
matrix.Rotate(new System.Windows.Media.Media3D.Quaternion(new System.Windows.Media.Media3D.Vector3D(0, 0, 1), matrix.GetOrientation() * -1.0));
matrix.Rotate(new System.Windows.Media.Media3D.Quaternion(new System.Windows.Media.Media3D.Vector3D(0, 0, 1), orientationDegrees));
// add back translation
matrix.Translate(translation);
return(matrix);
}
static void Main(string[] args)
{
navigationMatrix = new Matrix3D();
navigationMatrix.Translate(new Vector3D(0, 100, 110));
navigationMatrix.Scale(new Vector3D((double)1 / 5, (double)1 / 5, (double)1 / 5));
displayProfile = new Bin[Bin.RANGEL, Bin.RANGEA, Bin.RANGEB];
for (int l = 0; l < Bin.RANGEL; l++)
for (int a = 0; a < Bin.RANGEA; a++)
for (int b = 0; b < Bin.RANGEB; b++)
displayProfile[l, a, b] = new Bin(l, a, b);
PopulateProfile(displayProfile, navigationMatrix);
String path = Environment.CurrentDirectory + PATH_TO_VIDEO;
if (!System.IO.File.Exists(path))
return;
//Opens the movie file
capture = new Capture(path);
double fps = capture.GetCaptureProperty(Emgu.CV.CvEnum.CAP_PROP.CV_CAP_PROP_FPS);
//Reads frame by frame
Timer timer = new Timer(1000 / fps);
timer.Elapsed += new ElapsedEventHandler(timer_Elapsed);
timer.Start();
Console.Read();
}
/// <summary>
/// LookAtRH
/// </summary>
/// http://msdn.microsoft.com/en-us/library/bb281711(v=vs.85).aspx
/// <returns></returns>
public static Matrix3D SetViewMatrix(Point3D cameraPosition, Vector3D lookDirection, Vector3D upDirection)
{
// Normalize vectors:
lookDirection.Normalize();
upDirection.Normalize();
double dotProduct = Vector3D.DotProduct(lookDirection, upDirection);
// Define vectors, XScale, YScale, and ZScale:
double denom = Math.Sqrt(1 - Math.Pow(dotProduct, 2));
Vector3D XScale = Vector3D.CrossProduct(lookDirection, upDirection) / denom;
Vector3D YScale = (upDirection - dotProduct * lookDirection) / denom;
Vector3D ZScale = lookDirection;
// Construct M matrix:
Matrix3D M = new Matrix3D()
{
M11 = XScale.X, M12 = YScale.X, M13 = ZScale.X,
M21 = XScale.Y, M22 = YScale.Y, M23 = ZScale.Y,
M31 = XScale.Z, M32 = YScale.Z, M33 = ZScale.Z
};
// Translate the camera position to the origin:
Matrix3D translateMatrix = new Matrix3D();
translateMatrix.Translate(new Vector3D(-cameraPosition.X, -cameraPosition.Y, -cameraPosition.Z));
// Define reflect matrix about the Z axis:
Matrix3D reflectMatrix = new Matrix3D();
reflectMatrix.M33 = -1;
// Construct the View matrix:
Matrix3D viewMatrix = translateMatrix * M * reflectMatrix;
return viewMatrix;
}
public static System.Windows.Media.Media3D.Matrix3D SetSpin(
this System.Windows.Media.Media3D.Matrix3D matrix, double spin)
{
Vector3D translation = new System.Windows.Media.Media3D.Vector3D(matrix.OffsetX, matrix.OffsetY, matrix.OffsetZ);
// backoff translation
matrix.Translate(new System.Windows.Media.Media3D.Vector3D(-translation.X, -translation.Y, -translation.Z));
Vector3D localZ = matrix.Transform(new System.Windows.Media.Media3D.Vector3D(0, 0, 1));
// backoff existing spin
matrix.Rotate(new System.Windows.Media.Media3D.Quaternion(localZ, matrix.GetSpin() * -1.0));
matrix.Rotate(new System.Windows.Media.Media3D.Quaternion(localZ, spin));
// add back translation
matrix.Translate(translation);
return(matrix);
}
static void SetupMatrices()
{
mx1 = new Matrix3D();
mx1.Translate(new Vector3D(0, -2.4, 0));
mx2 = new Matrix3D();
mx3 = new Matrix3D();
mx3.Translate(new Vector3D(0, 2.4, 0));
mx4 = new Matrix3D();
mx4.Translate(new Vector3D(0, 0, -1));
}
public static Matrix3D GetLocalToParent(IDictionary dictionary)
{
var matrix3D = new Matrix3D();
if (dictionary != null)
{
var rotations = Helpers.IDictionaryHelper.GetRotationsXYZ(dictionary);
matrix3D = Helpers.Matrix3DHelper.RotateXYZ(new Matrix3D(), Helpers.IDictionaryHelper.GetRotationsXYZ(dictionary));
matrix3D.Translate(Helpers.IDictionaryHelper.GetTranslation(dictionary));
}
return matrix3D;
}
private static Transform3D GetTransform3D(IDictionary source, IFactory factory)
{
// transform should NOT include Scale
ITranslation itranslation = null;
IRotations irotations = null;
if (factory != null)
{
itranslation = factory.Create<ITranslation>(source,null);
irotations = factory.Create<IRotations>(source,null);
var matrix3D = new Matrix3D();
if (irotations != null) matrix3D = IMatrix3DHelper.RotateXYZ(new Matrix3D(), irotations.RotationsXYZ);
if (itranslation != null) matrix3D.Translate(itranslation.Translation);
if (!matrix3D.IsIdentity)
{
return new MatrixTransform3D { Matrix = matrix3D };
}
}
return null;
}
public static IMatrix3D FromIDictionary(IDictionary source, IFactory factory_in)
{
IFactory factory = factory_in;
if (factory == null) factory = Node.Net.Factories.Deprecated.Factory.Default;
var matrix3D = new Matrix3D();
var irotations = factory.Create<IRotations>(source,null);
if (irotations == null) irotations = Node.Net.Factories.Deprecated.Factory.Default.Create<IRotations>(source,null);
var iscale = factory.Create<IScale>(source,null);
//if (iscale == null) iscale = Node.Net.Factories.Factory.Default.Create<IScale>(source);
var itranslation = factory.Create<ITranslation>(source,null);
if (itranslation == null) itranslation = factory.Create<ITranslation>(source,null);
if (irotations != null)
{
matrix3D = RotateXYZ(new Matrix3D(), irotations.RotationsXYZ);
}
if (iscale != null) matrix3D.Scale(iscale.Scale);
if (itranslation != null) matrix3D.Translate(itranslation.Translation);
if (!matrix3D.IsIdentity)
{
return new ConcreteMatrix3D { Matrix3D = matrix3D };
}
return new ConcreteMatrix3D();
}
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);
}
internal override void Append(ref Matrix3D matrix)
{
matrix.Translate(new Vector3D(_cachedOffsetXValue, _cachedOffsetYValue, _cachedOffsetZValue));
}
/// <summary>
/// Translate the point cloud by a given value
/// </summary>
/// <param name="tx">Up to three co-ords</param>
public void translate(double[] tx)
{
if (tx.Length == 3) {
//turn the transformation vector into and object
Console.WriteLine("Translating");
TranslateTransform3D translation = new TranslateTransform3D(tx[0], tx[1], tx[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) {
//perform the new translation which does appear to work.
Matrix3D mtx = new Matrix3D();
mtx.Translate(new Vector3D(tx[0], tx[1], tx[2]));
//complete translation
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
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));
//perform the old translation method which doesn't appear to work.
//point.point.Offset(tx[0], tx[1], tx[2]);
//double[] key = new double[3]{point.point.X, point.point.Y, point.point.Z};
//newPoints.insert(key, point);
}
//replace the old kd tree with the new one
this.points = newPoints;
}
else {
//probably want to throw an exception here
}
}
public void UpdateOrCreateJoint(Joint joint)
{
int segments = 5;
double jointRadius = 0.07;
double boneRadius = 0.03;
int jointId = (int)joint.ID;
int connectedToJoint = SkeletonMetadata.BoneConnectionMapping[jointId];
_jointPositions[jointId] = new Vector3D(joint.Position.X, joint.Position.Y, joint.Position.Z);
// Create new 3d cube for the joint if not yet existing
GeometryModel3D model;
if (_joints[jointId] != null)
{
model = _joints[jointId];
}
else
{
model = Model3DFactory.CreateNormalizedSphere(defaultMaterial, segments);
_environment.Children.Add(model);
_joints[jointId] = model;
if (connectedToJoint >= 0)
{
GeometryModel3D cylinder = Model3DFactory.CreateNormalizedCylinder(defaultMaterial, segments);
_environment.Children.Add(cylinder);
_bones[jointId] = cylinder;
}
}
// Performance improvement: not using a transformation group, but multiply
// matrices first and use a single MatrixTransform3D
var matrix = new Matrix3D();
matrix.Scale(new Vector3D(jointRadius, jointRadius, jointRadius));
matrix.Translate(new Vector3D(joint.Position.X, joint.Position.Y, joint.Position.Z));
// Update position and the material/color (based on current tracking state), color right shoulder blue
if (joint.ID == JointID.ShoulderRight)
model.Material = rightShoulderIndicatorMaterial;
else
model.Material = _trackingStateMaterials[joint.TrackingState];
model.Transform = new MatrixTransform3D(matrix);
if (connectedToJoint >= 0)
{
GeometryModel3D bone = _bones[jointId];
Vector3D boneStart = _jointPositions[jointId];
Vector3D boneEnd = _jointPositions[connectedToJoint];
Vector3D boneCenter = (boneStart + boneEnd) / 2;
Vector3D boneVector = boneEnd - boneStart;
// Again, compute a single transformation matrix and apply it to each bone
var boneMatrix = new Matrix3D();
boneMatrix.Scale(new Vector3D(boneRadius, boneRadius, boneVector.Length));
boneMatrix.Rotate(GetQuaternionFromVectors(new Vector3D(0, 0, 1), boneVector));
boneMatrix.Translate(boneCenter);
bone.Material = _trackingStateMaterials[joint.TrackingState];
bone.Transform = new MatrixTransform3D(boneMatrix); ;
}
}
private void UpdateCameraMatrix()
{
// right-handed coordinates
#if false
// front view
var eye = new Vector3D(0.0, 0.0, -CameraDistance);
var at = new Vector3D(0.0, 0.0, 0.0);
var up = new Vector3D(0.0, 1.0, 0.0);
var lookAt = WWMatrixUtil.CalculateLookAt(eye, at, up);
#else
// virtual trackball
var cameraRot = mVirtualTrackball.RotationMatrix();
var cameraTranslate = new Matrix3D();
cameraTranslate.Translate(new Vector3D(0.0, 0.0, -mCameraDistanceCurrent));
var cameraMat = cameraTranslate * cameraRot;
var lookAt = cameraMat;
lookAt.Invert();
#endif
var viewProjectionMatrix = WWMatrixUtil.CreatePerspectiveProjectionMatrix(mCameraNear, mCameraDistanceCurrent * 2.0, CameraFovHDegree, 1.0);
mWorldProjectionMatrix = lookAt * viewProjectionMatrix;
}
/// <summary>
/// パーティクルの描画設定をまとめて行います。
/// </summary>
public override void RenderParticles(IEnumerable<Particle> particles)
{
base.RenderParticles(particles);
foreach (var particle in particles)
{
if (particle.Brush != null)
{
var opacity = (double)((particle.Color >> 24) & 0xFF) / 255.0;
particle.Brush.Opacity = EffectObject.InheritedOpacity * opacity;
}
if (particle.Material != null)
{
var em = particle.Material as EmissiveMaterial;
if (em != null)
{
em.Color = ToColor(particle.Color);
}
else
{
var dm = particle.Material as DiffuseMaterial;
if (dm != null)
{
dm.Color = ToColor(particle.Color);
}
}
}
// 行列変換
var m = new Matrix3D();
if (particle.Scale != 1.0)
{
m.Scale(new Vector3D(particle.Scale, particle.Scale, 1.0));
}
if (particle.Rotation != 0.0)
{
double rot = MathEx.ToDeg(particle.Rotation);
m.Rotate(new Quaternion(new Vector3D(0, 0, 1), rot));
}
m.Translate(new Vector3D(particle.X, particle.Y, -15.0));
particle.Model.Transform = new MatrixTransform3D(m);
}
}
internal override void Append(ref Matrix3D matrix)
{
matrix.Translate(new Vector3D(_cachedOffsetXValue, _cachedOffsetYValue, _cachedOffsetZValue));
}
private Matrix3D calculateTranslationMatrix(double x, double y, double z)
{
Matrix3D matrix = new Matrix3D();
matrix.Translate(new Vector3D(x, y, z));
return matrix;
}
/// <summary>
/// Gets a transform matrix representing the transform of the robot model
/// </summary>
/// <returns></returns>
public Matrix3D GetTransformMatrix()
{
Matrix3D mat = new Matrix3D();
Vector3D offset = new Vector3D(-OriginX, -OriginY, -OriginZ);
mat.Translate(offset);
Vector3D xAxis = new Vector3D(1, 0, 0);
Quaternion roll = new Quaternion(xAxis, OriginR * 180 / Math.PI);
Vector3D yAxis = new Vector3D(0, 1, 0);
Quaternion pitch = new Quaternion(yAxis, OriginP * 180 / Math.PI);
Vector3D zAxis = new Vector3D(0, 0, 1);
Quaternion yaw = new Quaternion(zAxis, OriginW * 180 / Math.PI);
mat.Rotate(roll);
mat.Rotate(pitch);
mat.Rotate(yaw);
RobotInfo.WriteToLogFile("Transform Matrix Created");
return mat;
}
public static MatrixTransform3D Offset(Transform3D transform, double x, double y, double z)
{
//Model3DGroup rlt = new Model3DGroup();
Matrix3D m = new Matrix3D();
MatrixTransform3D origin = transform as MatrixTransform3D;
TranslateTransform3D tns = new TranslateTransform3D(x, y, z);
m.Translate(new Vector3D(x, y, z));
return new MatrixTransform3D(Matrix3D.Multiply(origin.Matrix, m));
//Transform3DGroup grp = new Transform3DGroup();
//grp.Children.Add(origin);
//grp.Children.Add(new TranslateTransform3D(x, y, z));
//return grp;
}
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();
}
private void ComposeMatrix()
{
if (_decomposeDirty)
{
_matrix = Matrix3D.Identity;
_matrix.Translate(_translation);
_matrix.Prepend(Math3D.CreateYawPitchRollMatrix(_rotation));
_decomposeDirty = false;
}
}
private void RedrawRoom()
{
DrawModel(mRoom.RoomModel, Matrix3D.Identity, new SolidColorBrush(Colors.White));
Matrix3D listenerMatrix = new Matrix3D();
listenerMatrix.Translate((Vector3D)mRoom.ListenerPos);
DrawModel(mRoom.ListenerModel, listenerMatrix, new SolidColorBrush(Colors.Gray));
for (int i = 0; i < WWRoom.NUM_OF_SPEAKERS; ++i) {
var pos = mRoom.SpeakerPos(i);
var dir = mRoom.SpeakerDir(i);
Vector3D posV = new Vector3D(pos.X, pos.Y, pos.Z);
Vector3D at = new Vector3D(pos.X +dir.X, pos.Y + dir.Y, pos.Z + dir.Z);
Vector3D up = new Vector3D(0.0, 1.0, 0.0);
Matrix3D speakerMatrixInv = WWMatrixUtil.CalculateLookAt(posV, at, up);
var speakerMatrix = speakerMatrixInv;
speakerMatrix.Invert();
DrawModel(mRoom.SpeakerModel, speakerMatrix, new SolidColorBrush(Colors.Gray));
}
}
private Matrix3D SetTransformMatrix(double offsetX, double offsetY, double offsetZ, int rotateAroundX)
{
Matrix3D resultMatrix = new Matrix3D();
resultMatrix.Rotate(new Quaternion(new Vector3D(10, 0, 0), -rotateAroundX));
resultMatrix.Translate(new Vector3D(offsetX, offsetY, -offsetZ));
return resultMatrix;
}
protected virtual void NewTransform()
{
if (lockCount != 0)
return;
Matrix3D m = new Matrix3D();
m.Scale(new Vector3D(ScaleX, ScaleY, ScaleZ));
m.Rotate(Rotation1);
m.Rotate(Rotation2);
m.Translate(new Vector3D(Position.X, Position.Y, Position.Z));
m.Rotate(Rotation3);
Transform = new MatrixTransform3D(m);
}
/// <summary>
/// Calculate data of transformator by using source and target points.
/// </summary>
/// <param name="sourcePoints">Source points.</param>
/// <param name="targetPoints">Target points.</param>
public void Fill(Point3D[] sourcePoints, Point3D[] targetPoints)
{
Debug.Assert(sourcePoints.Length == targetPoints.Length);
int length = Math.Min(sourcePoints.Length, targetPoints.Length);
if (length == 0) {
this.SetIdentity();
return;
}
#region Find base.
int baseIndex = 1;
Vector3D[] sourceBbase = new Vector3D[dimExt];
Vector3D[] targetBbase = new Vector3D[dimExt];
for (int i = 0; i < length; ++i) {
sourceBbase[0] += (Vector3D)sourcePoints[i];
targetBbase[0] += (Vector3D)targetPoints[i];
}
sourceBbase[0] /= length; targetBbase[0] /= length;
Vector3D sourceVector = new Vector3D();
Vector3D targetVector = new Vector3D();
for (int i = 0; i < length && baseIndex < dimExt; ++i) {
sourceVector = (Vector3D)sourcePoints[i] - sourceBbase[0];
targetVector = (Vector3D)targetPoints[i] - targetBbase[0];
for (int j = 1; j < baseIndex; ++j) {
sourceVector -= Vector3D.DotProduct(sourceVector, sourceBbase[j]) * sourceBbase[j];
targetVector -= Vector3D.DotProduct(targetVector, targetBbase[j]) * targetBbase[j];
}
if (sourceVector.LengthSquared > sqrEps && targetVector.LengthSquared > sqrEps) {
sourceVector.Normalize();
targetVector.Normalize();
sourceBbase[baseIndex] = sourceVector;
targetBbase[baseIndex] = targetVector;
++baseIndex;
}
}
#endregion Find base.
if (baseIndex == 1) {
this.SetIdentity();
this.matrix[0, dim] = targetBbase[0].X - sourceBbase[0].X;
this.matrix[1, dim] = targetBbase[0].Y - sourceBbase[0].Y;
this.matrix[2, dim] = targetBbase[0].Z - sourceBbase[0].Z;
return;
}
if (baseIndex == 2) {
// TODO: (R) Bad code. :-(
Matrix3D matrix3D = new Matrix3D();
matrix3D.Translate(targetBbase[0] - sourceBbase[0]);
matrix3D.Rotate(new Quaternion(Vector3D.CrossProduct(sourceBbase[1], targetBbase[1]), Vector3D.AngleBetween(sourceBbase[1], targetBbase[1])));
this.MatrixCopy = new double[dim * dimExt]
{
matrix3D.M11, matrix3D.M21, matrix3D.M31, matrix3D.OffsetX,
matrix3D.M12, matrix3D.M22, matrix3D.M32, matrix3D.OffsetY,
matrix3D.M13, matrix3D.M23, matrix3D.M33, matrix3D.OffsetZ
};
return;
}
if (baseIndex == 3) {
sourceBbase[3] = Vector3D.CrossProduct(sourceBbase[1], sourceBbase[2]);
targetBbase[3] = Vector3D.CrossProduct(targetBbase[1], targetBbase[2]);
}
int fictiveLength = baseIndex == 3 ? 1 : 0;
double[,] sourcePointsCopy = new double[length + fictiveLength, dimExt];
double[,] targetPointsCopy = new double[length + fictiveLength, dimExt];
for (int i = 0; i < length; ++i) {
sourcePointsCopy[i, 0] = sourcePoints[i].X;
sourcePointsCopy[i, 1] = sourcePoints[i].Y;
sourcePointsCopy[i, 2] = sourcePoints[i].Z;
sourcePointsCopy[i, 3] = 1;
targetPointsCopy[i, 0] = targetPoints[i].X;
targetPointsCopy[i, 1] = targetPoints[i].Y;
targetPointsCopy[i, 2] = targetPoints[i].Z;
targetPointsCopy[i, 3] = 1;
}
if (fictiveLength > 0) {
sourcePointsCopy[length, 0] = sourceBbase[0].X + sourceBbase[3].X;
sourcePointsCopy[length, 1] = sourceBbase[0].Y + sourceBbase[3].Y;
sourcePointsCopy[length, 2] = sourceBbase[0].Z + sourceBbase[3].Z;
sourcePointsCopy[length, 3] = 1;
targetPointsCopy[length, 0] = targetBbase[0].X + targetBbase[3].X;
targetPointsCopy[length, 1] = targetBbase[0].Y + targetBbase[3].Y;
targetPointsCopy[length, 2] = targetBbase[0].Z + targetBbase[3].Z;
targetPointsCopy[length, 3] = 1;
}
this.Fill(sourcePointsCopy, targetPointsCopy);
}
