/// <summary>
/// Projects the stored 3D points in to 2D.
/// </summary>
/// <param name="objectToViewportTransform">The transformation matrix to use</param>
public void Project(Matrix3D objectToViewportTransform)
{
Point3D projPoint1 = objectToViewportTransform.Transform(_p1);
Point3D projPoint2 = objectToViewportTransform.Transform(_p2);
_p1Transformed = new Point(projPoint1.X, projPoint1.Y);
_p2Transformed = new Point(projPoint2.X, projPoint2.Y);
}
public RotateEventHandler(MVMIH.IRotationHandle rotationHandle)
{
_rotationHandle = rotationHandle;
_matrix = GetTranslateTransformation(rotationHandle);
_eleRotMatrix = Converters.StaticTransformationConverter.Convert((rotationHandle as IMachineElement).Parent.Parent.Transformation);
_rotationDirection = _eleRotMatrix.Transform(GetRotationDirection(rotationHandle));
_rotationCenter = _eleRotMatrix.Transform(GetRotationCenter((rotationHandle as IMachineElement).Parent.Parent));
_rotationHandle.StartRotate();
}
public static Matrix3D RotateXYZ(Matrix3D matrix, Vector3D rotationsXYZ)
{
matrix.Rotate(new Quaternion(new Vector3D(0, 0, 1), rotationsXYZ.Z));
var localY = matrix.Transform(new Vector3D(0, 1, 0));
matrix.Rotate(new Quaternion(localY, rotationsXYZ.Y));
var localX = matrix.Transform(new Vector3D(1, 0, 0));
matrix.Rotate(new Quaternion(localX, rotationsXYZ.X));
return matrix;
}
public static Matrix3D RotateXYZ(Matrix3D matrix, double xRotationDegrees, double yRotationDegrees, double zRotationDegrees)
{
matrix.Rotate(new Quaternion(new Vector3D(1, 0, 0), xRotationDegrees));
var localY = matrix.Transform(new Vector3D(0, 1, 0));
matrix.Rotate(new Quaternion(localY, yRotationDegrees));
var localZ = matrix.Transform(new Vector3D(0, 0, 1));
matrix.Rotate(new Quaternion(localZ, zRotationDegrees));
return matrix;
}
/// <summary>
/// Create a transform matrix that translates from skeleton space
/// into a space whose origin is the center of the screen and has the
/// same units as skeleton space.
/// </summary>
/// <param name="sensorOffset">vector, in meters, from the center of the display to the center of the Kinect sensor</param>
/// <param name="sensorElevationAngle">elevation angle of the sensor in degrees</param>
/// <returns>transform matrix</returns>
public static Matrix3D SensorToScreenPositionTransform(Vector3D sensorOffset, double sensorElevationAngle)
{
var rotateIntoSensorSpace = new Matrix3D();
rotateIntoSensorSpace.Rotate(new Quaternion(new Vector3D(1.0, 0.0, 0.0), sensorElevationAngle));
var eye = -sensorOffset;
eye = rotateIntoSensorSpace.Transform(eye);
var normalFromEye = new Vector3D(0.0, 0.0, 1.0);
normalFromEye = rotateIntoSensorSpace.Transform(normalFromEye);
var up = new Vector3D(0.0, 1.0, 0.0);
return LookAt(eye, normalFromEye, up);
}
public void Rotate(SWMM.Point3D newPosition, SWMM.Point3D oldPosition)
{
var me = _rotationHandle as IMachineElement;
var m = new SWMM.Matrix3D(_matrix.M11, _matrix.M12, _matrix.M13, _matrix.M14, _matrix.M21, _matrix.M22, _matrix.M23, _matrix.M24, _matrix.M31, _matrix.M32, _matrix.M33, _matrix.M34, _matrix.OffsetX, _matrix.OffsetY, _matrix.OffsetZ, _matrix.M44);
m.Invert();
var p1 = m.Transform(oldPosition);
var p2 = m.Transform(newPosition);
var v1 = GetOrtoComponent(p1 - _rotationCenter, _rotationDirection);
var v2 = GetOrtoComponent(p2 - _rotationCenter, _rotationDirection);
var a = SWMM.Vector3D.AngleBetween(v1, v2);
_rotationHandle.Rotate(a);
}
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);
}
//// http://en.wikipedia.org/wiki/Polygon_triangulation
//// http://en.wikipedia.org/wiki/Monotone_polygon
//// http://www.codeproject.com/KB/recipes/hgrd.aspx LGPL
//// http://www.springerlink.com/content/g805787811vr1v9v/
/// <summary>
/// Flattens this polygon.
/// </summary>
/// <returns>
/// The 2D polygon.
/// </returns>
public Polygon Flatten()
{
// http://forums.xna.com/forums/p/16529/86802.aspx
// http://stackoverflow.com/questions/1023948/rotate-normal-vector-onto-axis-plane
var up = this.GetNormal();
up.Normalize();
var right = Vector3D.CrossProduct(
up, Math.Abs(up.X) > Math.Abs(up.Z) ? new Vector3D(0, 0, 1) : new Vector3D(1, 0, 0));
var backward = Vector3D.CrossProduct(right, up);
var m = new Matrix3D(
backward.X, right.X, up.X, 0, backward.Y, right.Y, up.Y, 0, backward.Z, right.Z, up.Z, 0, 0, 0, 0, 1);
// make first point origin
var offs = m.Transform(this.Points[0]);
m.OffsetX = -offs.X;
m.OffsetY = -offs.Y;
var polygon = new Polygon { Points = new PointCollection(this.Points.Count) };
foreach (var p in this.Points)
{
var pp = m.Transform(p);
polygon.Points.Add(new Point(pp.X, pp.Y));
}
return polygon;
}
public void Move(SWMM.Vector3D step)
{
SWMM.Vector3D v = GetMoveDirection(_positionHandle);
var v2 = _matrix.Transform(v);
var dp = SWMM.Vector3D.DotProduct(step, v2);
var v3 = v2 * dp;
_positionHandle.Move(v3.X, v3.Y, v3.Z);
}
public static Point3D[] Transform(Matrix3D matrix, Point3D[] points)
{
var transformed_points = new List<Point3D>();
foreach (var point in points)
{
transformed_points.Add(matrix.Transform(point));
}
return transformed_points.ToArray();
}
public static IList<Point3D> GetGeometryPoints(MeshGeometry3D mesh, Matrix3D transformMatrix)
{
if (mesh != null)
{
bool applyMatrix = (transformMatrix != Matrix3D.Identity);
List<Point3D> points;
if ((mesh.TriangleIndices != null) && (mesh.TriangleIndices.Count > 0))
{
points = new List<Point3D>(mesh.TriangleIndices.Count);
for (int i = 0, count = mesh.TriangleIndices.Count; i < count; i++)
{
int positionIndex = mesh.TriangleIndices[i];
if ((positionIndex < 0) || (positionIndex >= mesh.Positions.Count))
break;
if (applyMatrix)
points.Add(transformMatrix.Transform(mesh.Positions[positionIndex]));
else
points.Add(mesh.Positions[positionIndex]);
}
}
else
{
points = new List<Point3D>(mesh.Positions.Count);
if (applyMatrix)
{
foreach (Point3D p in mesh.Positions)
points.Add(transformMatrix.Transform(p));
}
else
points.AddRange(mesh.Positions);
}
if (points.Count > 0)
return points;
else
return null;
}
else
return null;
}
public static double GetOrientation(this System.Windows.Media.Media3D.Matrix3D matrix)
{
Vector3D localX = matrix.Transform(new System.Windows.Media.Media3D.Vector3D(1, 0, 0));
Vector3D zcross = Vector3D.CrossProduct(new Vector3D(1, 0, 0), localX);
double multiplier = 1.0;
if (zcross.Z < 0)
{
multiplier = -1.0;
}
// rotation about the Z axis = angle between localX and parent X
return(System.Windows.Media.Media3D.Vector3D.AngleBetween(localX, new System.Windows.Media.Media3D.Vector3D(1, 0, 0)) * multiplier);
}
public static double GetTilt(this System.Windows.Media.Media3D.Matrix3D matrix)
{
// get the rotation about the X` local axis
Vector3D localX = matrix.Transform(new System.Windows.Media.Media3D.Vector3D(1, 0, 0));
Vector3D localY = matrix.Transform(new System.Windows.Media.Media3D.Vector3D(0, 1, 0));
Matrix3D orientMatrix = new System.Windows.Media.Media3D.Matrix3D().SetOrientation(matrix.GetOrientation());
Vector3D orientedY = orientMatrix.Transform(new System.Windows.Media.Media3D.Vector3D(0, 1, 0));
double multiplier = 1.0;
Vector3D xcross = Vector3D.CrossProduct(orientedY, localY);
double xangle = Vector3D.AngleBetween(localX, xcross);
if (xangle > 90.0)
{
multiplier = -1.0;
}
//if(xcross.X < 0.0) { multiplier = -1.0; }
// rotation about the X' axis = angle between localY and orientedY
double angle = System.Windows.Media.Media3D.Vector3D.AngleBetween(localY, orientedY) * multiplier;
return(angle);
}
private static void PopulateProfile(Bin[, ,] dProfile, Matrix3D navigationMatrix)
{
DataTable dataFile = new DataTable();
try
{
// add the csv bin file
using (GenericParserAdapter parser = new GenericParserAdapter(PATH_TO_BIN_PROFILE))
{
System.Data.DataSet dsResult = parser.GetDataSet();
dataFile = dsResult.Tables[0];
}
}
catch
{ }
for (int i = 1; i < dataFile.Rows.Count; i++)
{
//lab vale as got form profile index
Point3D labBin = new Point3D();
labBin.X = Convert.ToDouble(dataFile.Rows[i][0].ToString());
labBin.Y = Convert.ToDouble(dataFile.Rows[i][1].ToString());
labBin.Z = Convert.ToDouble(dataFile.Rows[i][2].ToString());
//trasfered points
Point3D labCoordinate = navigationMatrix.Transform(labBin);
//gets the bin to fill up
Bin actualBin = GetProfileBin(dProfile, labCoordinate);
//bin RGB Value
actualBin.binRGB.X = Convert.ToByte(dataFile.Rows[i][9].ToString());
actualBin.binRGB.Y = Convert.ToByte(dataFile.Rows[i][10].ToString());
actualBin.binRGB.Z = Convert.ToByte(dataFile.Rows[i][11].ToString());
//Measure Lab Values
actualBin.measuredLAB.X = Convert.ToDouble(dataFile.Rows[i][3].ToString());
actualBin.measuredLAB.Y = Convert.ToDouble(dataFile.Rows[i][4].ToString());
actualBin.measuredLAB.Z = Convert.ToDouble(dataFile.Rows[i][5].ToString());
//measured XYZ Values
actualBin.measuredXYZ.X = Convert.ToDouble(dataFile.Rows[i][6].ToString());
actualBin.measuredXYZ.Y = Convert.ToDouble(dataFile.Rows[i][7].ToString());
actualBin.measuredXYZ.Z = Convert.ToDouble(dataFile.Rows[i][8].ToString());
//is empty check
actualBin.isEmpty = false;
}
}
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);
}
public static double GetSpin(this System.Windows.Media.Media3D.Matrix3D matrix)
{
// get the rotatoin about the Z`` axis: angle between localX and tiltedX
Vector3D localX = matrix.Transform(new System.Windows.Media.Media3D.Vector3D(1, 0, 0));
Matrix3D tiltMatrix = new System.Windows.Media.Media3D.Matrix3D()
.SetOrientation(matrix.GetOrientation())
.SetTilt(matrix.GetTilt());
Vector3D tiltedX = tiltMatrix.Transform(new System.Windows.Media.Media3D.Vector3D(1, 0, 0));
double multiplier = 1.0;
Vector3D zcross = Vector3D.CrossProduct(tiltedX, localX);
if (zcross.Z < 0)
{
multiplier = -1.0;
}
return(System.Windows.Media.Media3D.Vector3D.AngleBetween(localX, tiltedX) * multiplier);
}
/// <summary>
/// Rotate camera using 'Turntable' rotation.
/// </summary>
/// <param name="delta">
/// The relative change in position.
/// </param>
/// <param name="rotateAround">
/// The point to rotate around.
/// </param>
public void RotateTurntable(Vector delta, Point3D rotateAround)
{
Vector3D relativeTarget = rotateAround - this.CameraTarget;
Vector3D relativePosition = rotateAround - this.CameraPosition;
Vector3D up = this.ModelUpDirection;
Vector3D dir = this.CameraLookDirection;
dir.Normalize();
Vector3D right = Vector3D.CrossProduct(dir, this.CameraUpDirection);
right.Normalize();
double d = -0.5;
if (this.CameraMode != CameraMode.Inspect)
{
d *= -0.2;
}
d *= this.RotationSensitivity;
var q1 = new Quaternion(up, d * delta.X);
var q2 = new Quaternion(right, d * delta.Y);
Quaternion q = q1 * q2;
var m = new Matrix3D();
m.Rotate(q);
Vector3D newUpDirection = m.Transform(this.CameraUpDirection);
Vector3D newRelativeTarget = m.Transform(relativeTarget);
Vector3D newRelativePosition = m.Transform(relativePosition);
Point3D newTarget = rotateAround - newRelativeTarget;
Point3D newPosition = rotateAround - newRelativePosition;
this.CameraLookDirection = newTarget - newPosition;
if (CameraMode == CameraMode.Inspect)
{
this.CameraPosition = newPosition;
}
this.CameraUpDirection = newUpDirection;
}
/// <summary>
/// Rotate around three axes.
/// </summary>
/// <param name="p1">
/// The previous mouse position.
/// </param>
/// <param name="p2">
/// The current mouse position.
/// </param>
/// <param name="rotateAround">
/// The point to rotate around.
/// </param>
public void RotateTurnball(Point p1, Point p2, Point3D rotateAround)
{
this.InitTurnballRotationAxes(p1);
Vector delta = p2 - p1;
Vector3D relativeTarget = rotateAround - this.CameraTarget;
Vector3D relativePosition = rotateAround - this.CameraPosition;
double d = -1;
if (this.CameraMode != CameraMode.Inspect)
{
d = 0.2;
}
d *= this.RotationSensitivity;
var q1 = new Quaternion(this.rotationAxisX, d * delta.X);
var q2 = new Quaternion(this.rotationAxisY, d * delta.Y);
Quaternion q = q1 * q2;
var m = new Matrix3D();
m.Rotate(q);
Vector3D newLookDir = m.Transform(this.CameraLookDirection);
Vector3D newUpDirection = m.Transform(this.CameraUpDirection);
Vector3D newRelativeTarget = m.Transform(relativeTarget);
Vector3D newRelativePosition = m.Transform(relativePosition);
Vector3D newRightVector = Vector3D.CrossProduct(newLookDir, newUpDirection);
newRightVector.Normalize();
Vector3D modUpDir = Vector3D.CrossProduct(newRightVector, newLookDir);
modUpDir.Normalize();
if ((newUpDirection - modUpDir).Length > 1e-8)
{
newUpDirection = modUpDir;
}
Point3D newTarget = rotateAround - newRelativeTarget;
Point3D newPosition = rotateAround - newRelativePosition;
Vector3D newLookDirection = newTarget - newPosition;
this.CameraLookDirection = newLookDirection;
if (CameraMode == CameraMode.Inspect)
{
this.CameraPosition = newPosition;
}
this.CameraUpDirection = newUpDirection;
}
private void RotateCameraSpherical(double dx, double dy)
{
// Figure out how to rotate the screen into world coords
Quaternion quatScreenToWorld = GetRotationForDelta();
// Turn the mouse movement into 3D rotation
Quaternion quat = GetSphericalMovement(dx, dy);
// Rotate that
quat = new Quaternion(quatScreenToWorld.GetRotatedVector(quat.Axis), quat.Angle * .1d); // also, make it less twitchy
//quat = new Quaternion(quat.Axis, quat.Angle * .1d);
// Create a matrix that will perform the rotation
Matrix3D matrix = new Matrix3D();
matrix.Rotate(quat);
// Store the camera's stats
Vector3D[] vectors = new Vector3D[2];
vectors[0] = _camera1.LookDirection;
vectors[1] = _camera1.UpDirection;
// Rotate those stats
matrix.Transform(vectors);
// Apply the new stats
_camera1.LookDirection = vectors[0];
_camera1.UpDirection = vectors[1];
}
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;
}
/// <summary>
/// Get flat (square) points facing camera
/// </summary>
/// <param name="cameraPos">Camera position</param>
/// <param name="displayMatrix">View + projection + display matrix</param>
/// <returns>Points</returns>
protected Point3D[] GetFlatPoints( Point3D cameraPos, Matrix3D displayMatrix )
{
Vector3D perpendicular = Vector3D.CrossProduct( (Vector3D)position, (Vector3D)cameraPos );
perpendicular.Normalize();
Vector3D distance = bounds.Positions[ 0 ] - position;
double radius = Math.Max( Math.Max( Math.Abs( distance.X ), Math.Abs( distance.Y ) ), Math.Abs( distance.Z ) );
Point3D[] points = new Point3D[] { position, position + ( perpendicular * radius ) };
displayMatrix.Transform( points );
Point3D center = points[ 0 ];
double half = ( points[ 1 ] - points[ 0 ] ).Length;
points = new Point3D[] {
new Point3D( center.X - half, center.Y - half, center.Z ),
new Point3D( center.X + half, center.Y - half, center.Z ),
new Point3D( center.X + half, center.Y + half, center.Z ),
new Point3D( center.X - half, center.Y + half, center.Z ) };
displayMatrix.Invert();
displayMatrix.Transform( points );
return points;
}
/// <summary>
/// Determines whether the back of the plane is showing, given a rotation.
/// Credit to Joel Pryde.
/// </summary>
/// <param name="x">The x rotation.</param>
/// <param name="y">The y rotation.</param>
/// <param name="z">The z rotation.</param>
/// <returns>
/// <c>true</c> if the back of the plane is showing, given a rotation; otherwise, <c>false</c>.
/// </returns>
private static bool IsBackShowingRotation(double x, double y, double z)
{
Matrix3D rotMatrix = new Matrix3D();
rotMatrix.Rotate(new Quaternion(new Vector3D(1, 0, 0), x));
rotMatrix.Rotate(new Quaternion(new Vector3D(0, 1, 0) * rotMatrix, y));
rotMatrix.Rotate(new Quaternion(new Vector3D(0, 0, 1) * rotMatrix, z));
Vector3D transformZ = rotMatrix.Transform(new Vector3D(0, 0, 1));
return Vector3D.DotProduct(new Vector3D(0, 0, 1), transformZ) < 0;
}
/// <summary>
/// Vector3D * Matrix3D multiplication
/// </summary>
/// <param name="vector">Vector being tranformed.</param>
/// <param name="matrix">Transformation matrix applied to the vector.</param>
/// <returns>Result of multiplication.</returns>
public static Vector3D Multiply(Vector3D vector, Matrix3D matrix)
{
return(matrix.Transform(vector));
}
/// <summary>
/// Point4D * Matrix3D multiplication.
/// </summary>
/// <param name="point">Point being transformed.</param>
/// <param name="matrix">Transformation matrix applied to the point.</param>
/// <returns>Result of the transformation matrix applied to the point.</returns>
public static Point4D Multiply(Point4D point, Matrix3D matrix)
{
return matrix.Transform(point);
}
// updates the matrices and plane equation and sends it to the pixel shader.</summary>
private void _updateMatricesAndShaderValues() {
// Creating the worldViewProj matrix corresponding to the current plane transformation
_worldViewProj = PlaneTransformation * _viewProjMatrix;
if (RecenterViewport) {
Point3D transformedCenter = _worldViewProj.Transform(new Point3D(0, 0, 0));
ViewportOffset = new Point(transformedCenter.X, transformedCenter.Y);
_hitTestingTransform.ViewportOffset = ViewportOffset;
}
_worldViewProjI = _worldViewProj;
// and its inversed matrix
_worldViewProjI.Invert();
_hitTestingTransform.PlaneTransformation = _worldViewProj;
_hitTestingTransform.InverseTransformation = _worldViewProjI;
// Calculate the 3 coordinates of the plane after transformation
var rawTopLeft = _worldViewProj.Transform(new Point3D(-1, 1, 0));
var rawTopRight = _worldViewProj.Transform(new Point3D(1, 1, 0));
var rawBottomLeft = _worldViewProj.Transform(new Point3D(-1, -1, 0));
// compute the normal of the plane
var planeNormal = Vector3D.CrossProduct((rawTopRight - rawTopLeft), (rawBottomLeft - rawTopLeft));
planeNormal.Normalize();
PlaneNormal = planeNormal;
_hitTestingTransform.PlaneNormal = planeNormal;
// and its distance from the origin
this.PlaneDistance = -Vector3D.DotProduct(planeNormal, new Vector3D(rawTopLeft.X, rawTopLeft.Y, rawTopLeft.Z));
_hitTestingTransform.PlaneDistance = this.PlaneDistance;
// Pass it to the pixel shader
M1 = new Point4D(_worldViewProjI.M11, _worldViewProjI.M12, _worldViewProjI.M13, _worldViewProjI.M14);
M2 = new Point4D(_worldViewProjI.M21, _worldViewProjI.M22, _worldViewProjI.M23, _worldViewProjI.M24);
M3 = new Point4D(_worldViewProjI.M31, _worldViewProjI.M32, _worldViewProjI.M33, _worldViewProjI.M34);
M4 = new Point4D(_worldViewProjI.OffsetX, _worldViewProjI.OffsetY, _worldViewProjI.OffsetZ, _worldViewProjI.M44);
}
/// <summary>
/// Vector3D * Matrix3D multiplication
/// </summary>
/// <param name="vector">Vector being tranformed.</param>
/// <param name="matrix">Transformation matrix applied to the vector.</param>
/// <returns>Result of multiplication.</returns>
public static Vector3D Multiply(Vector3D vector, Matrix3D matrix)
{
return matrix.Transform(vector);
}
private void CaptureMouseMove(object sender, MouseEventArgs e)
{
Point currentMousePoint = e.GetPosition(this);
if (this.pdbViewer.ActionType == PdbActionType.Rotate)
{
Vector3D currentMouseVector3D = this.ProjectToTrackball(currentMousePoint);
Vector3D axis = Vector3D.CrossProduct(
this.previousMouseVector, currentMouseVector3D);
double angle = 2 * Vector3D.AngleBetween(
this.previousMouseVector, currentMouseVector3D);
if (axis.LengthSquared > 0)
{
QuaternionRotation3D rotation =
this.rotateTransform.Rotation as QuaternionRotation3D;
if (rotation != null)
{
rotation.Quaternion = new Quaternion(axis, angle) *
rotation.Quaternion;
}
}
this.previousMouseVector = currentMouseVector3D;
}
else if (this.pdbViewer.ActionType == PdbActionType.Select ||
this.pdbViewer.ActionType == PdbActionType.Deselect &&
currentMousePoint != this.previousMousePoint)
{
double left = Math.Min(this.previousMousePoint.X, currentMousePoint.X);
double top = Math.Min(this.previousMousePoint.Y, currentMousePoint.Y);
double right = this.ActualWidth -
Math.Max(this.previousMousePoint.X, currentMousePoint.X);
double bottom = this.ActualHeight -
Math.Max(this.previousMousePoint.Y, currentMousePoint.Y);
left = Math.Max(0, left);
top = Math.Max(this.CaptureElement.TranslatePoint(new Point(), this).Y, top);
right = Math.Max(0, right);
bottom = Math.Max(0, bottom);
this.selectionRectangle.Margin = new Thickness(left, top, right, bottom);
this.selectionRectangle.Visibility = Visibility.Visible;
Rect selectionRect = new Rect(this.previousMousePoint, currentMousePoint);
foreach (Atom atom in this.Molecule.Atoms)
{
Point? atomPoint = this.GetViewportCoordinatePoint3Ds(atom);
bool contained = atomPoint != null && selectionRect.Contains(atomPoint.Value);
if (contained)
{
if (this.pdbViewer.ActionType == PdbActionType.Select)
atom.ShowAsSelected = true;
else if (this.pdbViewer.ActionType == PdbActionType.Deselect)
atom.ShowAsSelected = false;
}
else
{
atom.ShowAsSelected = atom.IsSelected;
}
}
}
else if (this.pdbViewer.ActionType == PdbActionType.Translate)
{
double multiplier = 2 * Math.Tan(Math.PI / 8) * cameraOffset /
this.scaleTransform.ScaleX;
double deltaX = (currentMousePoint.X - this.previousMousePoint.X) /
this.viewport.ActualWidth * multiplier;
double deltaY = -(currentMousePoint.Y - this.previousMousePoint.Y) /
this.viewport.ActualHeight * multiplier;
Vector3D deltaVector = new Vector3D(deltaX, deltaY, 0);
QuaternionRotation3D rotation =
(QuaternionRotation3D)this.rotateTransform.Rotation;
Matrix3D matrix = new Matrix3D();
matrix.Rotate(rotation.Quaternion);
matrix.Invert();
deltaVector = matrix.Transform(deltaVector);
this.translateTransform.OffsetX += deltaVector.X;
this.translateTransform.OffsetY += deltaVector.Y;
this.translateTransform.OffsetZ += deltaVector.Z;
this.previousMousePoint = currentMousePoint;
}
}
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 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;
}
public void RotateTwoAxes(double dx, double dy)
{
Vector3D up = ModelUpDirection; // new Vector3D(0, 0, 1);
Vector3D dir = LookDirection;
dir.Normalize();
Vector3D right = Vector3D.CrossProduct(dir, UpDirection);
right.Normalize();
double d = -0.5;
if (CameraMode == CameraMode.WalkAround)
d = 0.1;
d *= RotationSensitivity;
var q1 = new Quaternion(up, d*dx);
var q2 = new Quaternion(right, d*dy);
Quaternion q = q1*q2;
var m = new Matrix3D();
m.Rotate(q);
Vector3D newLookDir = m.Transform(LookDirection);
Vector3D newUpDir = m.Transform(UpDirection);
right = Vector3D.CrossProduct(newLookDir, newUpDir);
right.Normalize();
Vector3D modUpDir = Vector3D.CrossProduct(right, newLookDir);
modUpDir.Normalize();
if ((newUpDir - modUpDir).Length > 1e-8)
newUpDir = modUpDir;
LookDirection = newLookDir;
UpDirection = newUpDir;
}
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>
/// Transforms the given mesh
/// </summary>
/// <param name="mesh">This mesh</param>
/// <param name="transform">Transform</param>
public static void Transform(this MeshGeometry3D mesh, Matrix3D transform)
{
Matrix3D rotationOnly = transform;
rotationOnly.OffsetX = rotationOnly.OffsetY = rotationOnly.OffsetZ = 0;
// Transform positions
for(int i = 0; i < mesh.Positions.Count; i++)
mesh.Positions[i] = transform.Transform(mesh.Positions[i]);
// Transform normals
for (int i = 0; i < mesh.Normals.Count; i++)
mesh.Normals[i] = rotationOnly.Transform(mesh.Normals[i]);
}
private void RotateCameraAroundLookDir(double curX, double curY, double dx, double dy)
{
const double ROTATESPEED = .005d;
// Calculate angle
//double radians = ROTATESPEED * Math.Sqrt(Math.Pow(dx, 2) + Math.Pow(dy, 2));
double radians = ROTATESPEED * Math.Sqrt((dx * dx) + (dy * dy));
double degrees = Math1D.RadiansToDegrees(radians);
if (radians < 0)
{
MessageBox.Show("ya");
}
// See if I should negate the angle
//NOTE: This logic is flawed. I fixed this later by taking curXY cross prevXY
if (curX >= 0 && curY >= 0)
{
// Q1
if (dx > 0 || dy < 0)
{
degrees *= -1;
}
}
else if (curX <= 0 && curY >= 0)
{
// Q2
if (dx > 0 || dy > 0)
{
degrees *= -1;
}
}
else if (curX <= 0 && curY <= 0)
{
// Q3
if (dx < 0 || dy > 0)
{
degrees *= -1;
}
}
else if (curX >= 0 && curY <= 0)
{
// Q4
if (dx < 0 || dy < 0)
{
degrees *= -1;
}
}
// Create a matrix that will perform the rotation
Matrix3D matrix = new Matrix3D();
matrix.Rotate(new Quaternion(_camera1.LookDirection, degrees));
// Rotate the camera
_camera1.UpDirection = matrix.Transform(_camera1.UpDirection);
}
/// <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 ;)
}
}
/// <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
}
}
/// <summary>
/// Helper function to transform a Kinect SkeletonPoint using a
/// transform matrix.
/// </summary>
/// <param name="transform">The transform to use</param>
/// <param name="skeletonPoint">The skeleton point to be transformed</param>
/// <returns>the transformed skeleton point</returns>
public static SkeletonPoint TransformSkeletonPoint(Matrix3D transform, SkeletonPoint skeletonPoint)
{
var point = new Point4D(skeletonPoint.X, skeletonPoint.Y, skeletonPoint.Z, 1.0);
point = transform.Transform(point);
return new SkeletonPoint { X = (float)point.X, Y = (float)point.Y, Z = (float)point.Z };
}
void WorkShop_MouseMove(object sender, MouseEventArgs e)
{
if (ActionType == ActionType.Rotate)
{
var currentMousePoint = e.GetPosition(this);
var currentMouseVector = ProjectToTrackBall(currentMousePoint);
Vector3D axis = Vector3D.CrossProduct(PreviousMouseVector, currentMouseVector);
double angle = Vector3D.AngleBetween(PreviousMouseVector, currentMouseVector);
if (axis.LengthSquared > 0)
{
var rotation = RotateTransform.Rotation as QuaternionRotation3D;
if (rotation != null)
{
rotation.Quaternion = new Quaternion(axis, angle) * rotation.Quaternion;
PreviousMouseVector = currentMouseVector;
}
}
}
else if (ActionType == ActionType.Translate)
{
var currentMousePoint = e.GetPosition(this);
double multiplier = 2 * (Math.Tan(Math.PI / 8) / ScaleTransform.ScaleX) * 4;
double deltaX = ((currentMousePoint.X - PreviousMousePoint.X) / Space.ActualWidth) * multiplier;
double deltaY = -((currentMousePoint.Y - PreviousMousePoint.Y) / Space.ActualHeight) * multiplier;
var deltaVector = new Vector3D(deltaX, deltaY, 0);
var rotation = (QuaternionRotation3D)RotateTransform.Rotation;
var matrix = new Matrix3D();
matrix.Rotate(rotation.Quaternion);
matrix.Invert();
deltaVector = matrix.Transform(deltaVector);
TranslateTransform.OffsetX += deltaVector.X;
TranslateTransform.OffsetY += deltaVector.Y;
TranslateTransform.OffsetZ += deltaVector.Z;
PreviousMousePoint = currentMousePoint;
}
}
/// <summary>
/// Transforms the axis-aligned bounding box 'bounds' by
/// 'transform'
/// </summary>
/// <param name="bounds">The AABB to transform</param>
/// <returns>Transformed AABB</returns>
public static Rect3D TransformBounds(Rect3D bounds, Matrix3D transform)
{
double x1 = bounds.X;
double y1 = bounds.Y;
double z1 = bounds.Z;
double x2 = bounds.X + bounds.SizeX;
double y2 = bounds.Y + bounds.SizeY;
double z2 = bounds.Z + bounds.SizeZ;
Point3D[] points = new Point3D[] {
new Point3D(x1, y1, z1),
new Point3D(x1, y1, z2),
new Point3D(x1, y2, z1),
new Point3D(x1, y2, z2),
new Point3D(x2, y1, z1),
new Point3D(x2, y1, z2),
new Point3D(x2, y2, z1),
new Point3D(x2, y2, z2),
};
transform.Transform(points);
// reuse the 1 and 2 variables to stand for smallest and largest
Point3D p = points[0];
x1 = x2 = p.X;
y1 = y2 = p.Y;
z1 = z2 = p.Z;
for (int i = 1; i < points.Length; i++)
{
p = points[i];
x1 = Math.Min(x1, p.X); y1 = Math.Min(y1, p.Y); z1 = Math.Min(z1, p.Z);
x2 = Math.Max(x2, p.X); y2 = Math.Max(y2, p.Y); z2 = Math.Max(z2, p.Z);
}
return new Rect3D(x1, y1, z1, x2 - x1, y2 - y1, z2 - z1);
}
// CTAABB for non-affine transformations
internal static Rect3D ComputeTransformedAxisAlignedBoundingBoxNonAffine(/* IN */ ref Rect3D originalBox, /* IN */ ref Matrix3D matrix)
{
Debug.Assert(!matrix.IsAffine);
double x1 = originalBox.X;
double y1 = originalBox.Y;
double z1 = originalBox.Z;
double x2 = originalBox.X + originalBox.SizeX;
double y2 = originalBox.Y + originalBox.SizeY;
double z2 = originalBox.Z + originalBox.SizeZ;
Point3D[] points = new Point3D[] {
new Point3D(x1, y1, z1),
new Point3D(x1, y1, z2),
new Point3D(x1, y2, z1),
new Point3D(x1, y2, z2),
new Point3D(x2, y1, z1),
new Point3D(x2, y1, z2),
new Point3D(x2, y2, z1),
new Point3D(x2, y2, z2),
};
matrix.Transform(points);
Point3D p = points[0];
Rect3D newBounds = new Rect3D(p.X, p.Y, p.Z, 0, 0, 0);
// Traverse the entire mesh and compute bounding box.
for(int i = 1; i < points.Length; i++)
{
p = points[i];
AddPointToBounds(ref p, ref newBounds);
}
return newBounds;
}
/// <summary>
/// Point3D * Matrix3D multiplication.
/// </summary>
/// <param name="point">Point being transformed.</param>
/// <param name="matrix">Transformation matrix applied to the point.</param>
/// <returns>Result of the transformation matrix applied to the point.</returns>
public static Point3D Multiply(Point3D point, Matrix3D matrix)
{
return(matrix.Transform(point));
}