private void CalculateCubeTensor()
{
var factor = cubeMass * CubeSize * cubeSize;
var d = 2.0 / 3.0 * factor;
var nd = -1.0 / 4.0 * factor;
cubeTensor = new Matrix3D(d, nd, nd, 0, nd, d, nd, 0, nd, nd, d, 0, 0, 0, 0, 1);
invertCubeTensor = new Matrix3D(d, nd, nd, 0, nd, d, nd, 0, nd, nd, d, 0, 0, 0, 0, 1);
invertCubeTensor.Invert();
}
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);
}
internal override RayHitTestParameters RayFromViewportPoint(Point p, Size viewSize, Rect3D boundingRect, out double distanceAdjustment)
{
// The camera may be animating. Take a snapshot of the current value
// and get the property values we need. (Window OS #992662)
Point3D position = Position;
Vector3D lookDirection = LookDirection;
Vector3D upDirection = UpDirection;
Transform3D transform = Transform;
double zn = NearPlaneDistance;
double zf = FarPlaneDistance;
double fov = M3DUtil.DegreesToRadians(FieldOfView);
//
// Compute rayParameters
//
// Find the point on the projection plane in post-projective space where
// the viewport maps to a 2x2 square from (-1,1)-(1,-1).
Point np = M3DUtil.GetNormalizedPoint(p, viewSize);
// Note: h and w are 1/2 of the inverse of the width/height ratios:
//
// h = 1/(heightDepthRatio) * (1/2)
// w = 1/(widthDepthRatio) * (1/2)
//
// Computation for h is a bit different than what you will find in
// D3DXMatrixPerspectiveFovRH because we have a horizontal rather
// than vertical FoV.
double aspectRatio = M3DUtil.GetAspectRatio(viewSize);
double halfWidthDepthRatio = Math.Tan(fov / 2);
double h = aspectRatio / halfWidthDepthRatio;
double w = 1 / halfWidthDepthRatio;
// To get from projective space to camera space we apply the
// width/height ratios to find our normalized point at 1 unit
// in front of the camera. (1 is convenient, but has no other
// special significance.) See note above about the construction
// of w and h.
Vector3D rayDirection = new Vector3D(np.X / w, np.Y / h, -1);
// Apply the inverse of the view matrix to our rayDirection vector
// to convert it from camera to world space.
//
// NOTE: Because our construction of the ray assumes that the
// viewMatrix translates the position to the origin we pass
// null for the Camera.Transform below and account for it
// later.
Matrix3D viewMatrix = CreateViewMatrix(/* trasform = */ null, ref position, ref lookDirection, ref upDirection);
Matrix3D invView = viewMatrix;
invView.Invert();
invView.MultiplyVector(ref rayDirection);
// The we have the ray direction, now we need the origin. The camera's
// position would work except that we would intersect geometry between
// the camera plane and the near plane so instead we must find the
// point on the project plane where the ray (position, rayDirection)
// intersect (Windows OS #1005064):
//
// | _.> p = camera position
// rd _+" ld = camera look direction
// .-" |ro pp = projection plane
// _.-" | rd = ray direction
// p +"--------+---> ro = desired ray origin on pp
// ld |
// pp
//
// Above we constructed the direction such that it's length projects to
// 1 unit on the lookDirection vector.
//
//
// rd _.>
// .-" rd = unnormalized rayDirection
// _.-" ld = normalized lookDirection (length = 1)
// -"--------->
// ld
//
// So to find the desired rayOrigin on the projection plane we simply do:
Point3D rayOrigin = position + zn * rayDirection;
rayDirection.Normalize();
// Account for the Camera.Transform we ignored during ray construction above.
if (transform != null && transform != Transform3D.Identity)
{
Matrix3D m = transform.Value;
m.MultiplyPoint(ref rayOrigin);
m.MultiplyVector(ref rayDirection);
PrependInverseTransform(m, ref viewMatrix);
}
RayHitTestParameters rayParameters = new RayHitTestParameters(rayOrigin, rayDirection);
//
// Compute HitTestProjectionMatrix
//
Matrix3D projectionMatrix = GetProjectionMatrix(aspectRatio, zn, zf);
// The projectionMatrix takes camera-space 3D points into normalized clip
// space.
// The viewportMatrix will take normalized clip space into
// viewport coordinates, with an additional 2D translation
// to put the ray at the rayOrigin.
Matrix3D viewportMatrix = new Matrix3D();
viewportMatrix.TranslatePrepend(new Vector3D(-p.X, viewSize.Height - p.Y, 0));
viewportMatrix.ScalePrepend(new Vector3D(viewSize.Width / 2, -viewSize.Height / 2, 1));
viewportMatrix.TranslatePrepend(new Vector3D(1, 1, 0));
// `First world-to-camera, then camera's projection, then normalized clip space to viewport.
rayParameters.HitTestProjectionMatrix =
viewMatrix *
projectionMatrix *
viewportMatrix;
//
// Perspective camera doesn't allow negative NearPlanes, so there's
// not much point in adjusting the ray origin. Hence, the
// distanceAdjustment remains 0.
//
distanceAdjustment = 0.0;
return(rayParameters);
}
private bool UpdateTransforms()
{
Viewport3DVisual viewport;
bool success;
Matrix3D visualToScreen = MathUtils.TryTransformTo2DAncestor(this, out viewport, out success);
if (!success || !visualToScreen.HasInverse)
return false;
if (visualToScreen == _visualToScreen)
return false;
_visualToScreen = _screenToVisual = visualToScreen;
_screenToVisual.Invert();
return true;
}
private Vector3D QuaternionVectorMultiply(Quaternion q, Vector3D v)
{
var xx = q.X * q.X;
var xy = q.X * q.Y;
var xz = q.X * q.Z;
var xw = q.X * q.W;
var yy = q.Y * q.Y;
var yz = q.Y * q.Z;
var yw = q.Y * q.W;
var zz = q.Z * q.Z;
var zw = q.Z * q.W;
var m00 = 1 - 2 * (yy + zz);
var m01 = 2 * (xy - zw);
var m02 = 2 * (xz + yw);
var m10 = 2 * (xy + zw);
var m11 = 1 - 2 * (xx + zz);
var m12 = 2 * (yz - xw);
var m20 = 2 * (xz - yw);
var m21 = 2 * (yz + xw);
var m22 = 1 - 2 * (xx + yy);
var matrix = new Matrix3D(m00, m10, m20, 0, m01, m11, m21, 0, m02, m12, m22, 0, 0, 0, 0, 1);
matrix.Invert();
return MatrixVectorMultiply(matrix, v);
}
// 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);
}
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;
}
}
public virtual void ReadFromXMLNode(XmlNode node)
{
// Read the origin node
XmlNode originNode = node.SelectSingleNode(ModelXMLDefinition.pipeUCSOrigin);
CPoint3D origin = CPoint3DSerializer.ReadPoint(originNode);
// Read the x axis
XmlNode xAxisNode = node.SelectSingleNode(ModelXMLDefinition.pipeUCSXAxis);
CPoint3D xAxis = CPoint3DSerializer.ReadPoint(xAxisNode);
// Read the x axis
XmlNode yAxisNode = node.SelectSingleNode(ModelXMLDefinition.pipeUCSYAxis);
CPoint3D yAxis = CPoint3DSerializer.ReadPoint(yAxisNode); ;
// Calculate the UCS
Vector3D vecX = new Vector3D(xAxis.X, xAxis.Y, xAxis.Z);
vecX.Normalize();
Vector3D vecY = new Vector3D(yAxis.X, yAxis.Y, yAxis.Z);
vecY.Normalize();
// Get the z axis
Vector3D vecZ = Vector3D.CrossProduct(vecX, vecY);
// Recalculate the y axis since yaxis may not be preh to xaxis.
vecY = Vector3D.CrossProduct(vecZ, vecX);
// Create matrix3d representation
m_matrix3d = new Matrix3D(vecX.X, vecX.Y, vecX.Z, 0,
vecY.X, vecY.Y, vecY.Z, 0,
vecZ.X, vecZ.Y, vecZ.Z, 0,
origin.X, origin.Y, origin.Z, 1.0);
m_matrix3dInvert = new Matrix3D(vecX.X, vecX.Y, vecX.Z, 0,
vecY.X, vecY.Y, vecY.Z, 0,
vecZ.X, vecZ.Y, vecZ.Z, 0,
origin.X, origin.Y, origin.Z, 1.0);
m_matrix3dInvert.Invert();
// Set transform
m_tranform = m_matrix3d.ToVTKTransformation();
m_tranformInvert.DeepCopy(m_tranform);
m_tranformInvert.Inverse();
}
private Dictionary<JointType, Vector3D> MapJointsToVectors(IReadOnlyDictionary<JointType, Joint> joints)
{
//Vectors are added in the same order as the bodytree depthfirst search
Dictionary<JointType, Vector3D> jointVectors = new Dictionary<JointType, Vector3D>();
jointVectors.Add(JointType.SpineMid, SubtractJoints(joints[JointType.SpineMid], joints[JointType.SpineBase]));
jointVectors.Add(JointType.SpineShoulder, SubtractJoints(joints[JointType.SpineShoulder], joints[JointType.SpineMid]));
jointVectors.Add(JointType.ShoulderLeft, SubtractJoints(joints[JointType.ShoulderLeft], joints[JointType.SpineShoulder]));
jointVectors.Add(JointType.ElbowLeft, SubtractJoints(joints[JointType.ElbowLeft], joints[JointType.ShoulderLeft]));
jointVectors.Add(JointType.WristLeft, SubtractJoints(joints[JointType.WristLeft], joints[JointType.ElbowLeft]));
jointVectors.Add(JointType.HandLeft, SubtractJoints(joints[JointType.HandLeft], joints[JointType.WristLeft]));
jointVectors.Add(JointType.HandTipLeft, SubtractJoints(joints[JointType.HandTipLeft], joints[JointType.HandLeft]));
jointVectors.Add(JointType.ThumbLeft, SubtractJoints(joints[JointType.ThumbLeft], joints[JointType.HandLeft]));
jointVectors.Add(JointType.Neck, SubtractJoints(joints[JointType.Neck], joints[JointType.SpineShoulder]));
jointVectors.Add(JointType.Head, SubtractJoints(joints[JointType.Head], joints[JointType.Neck]));
jointVectors.Add(JointType.ShoulderRight, SubtractJoints(joints[JointType.ShoulderRight], joints[JointType.SpineShoulder]));
jointVectors.Add(JointType.ElbowRight, SubtractJoints(joints[JointType.ElbowRight], joints[JointType.ShoulderRight]));
jointVectors.Add(JointType.WristRight, SubtractJoints(joints[JointType.WristRight], joints[JointType.ElbowRight]));
jointVectors.Add(JointType.HandRight, SubtractJoints(joints[JointType.HandRight], joints[JointType.WristRight]));
jointVectors.Add(JointType.HandTipRight, SubtractJoints(joints[JointType.HandTipRight], joints[JointType.HandRight]));
jointVectors.Add(JointType.ThumbRight, SubtractJoints(joints[JointType.ThumbRight], joints[JointType.HandRight]));
jointVectors.Add(JointType.SpineBase, SubtractJoints(joints[JointType.SpineBase], new Joint()));
jointVectors.Add(JointType.HipLeft, SubtractJoints(joints[JointType.HipLeft], joints[JointType.SpineBase]));
jointVectors.Add(JointType.KneeLeft, SubtractJoints(joints[JointType.KneeLeft], joints[JointType.HipLeft]));
jointVectors.Add(JointType.AnkleLeft, SubtractJoints(joints[JointType.AnkleLeft], joints[JointType.KneeLeft]));
jointVectors.Add(JointType.FootLeft, SubtractJoints(joints[JointType.FootLeft], joints[JointType.AnkleLeft]));
jointVectors.Add(JointType.HipRight, SubtractJoints(joints[JointType.HipRight], joints[JointType.SpineBase]));
jointVectors.Add(JointType.KneeRight, SubtractJoints(joints[JointType.KneeRight], joints[JointType.HipRight]));
jointVectors.Add(JointType.AnkleRight, SubtractJoints(joints[JointType.AnkleRight], joints[JointType.KneeRight]));
jointVectors.Add(JointType.FootRight, SubtractJoints(joints[JointType.FootRight], joints[JointType.AnkleRight]));
Matrix3D rotationMatrix = new Matrix3D();
{
Vector3D j = new Vector3D(0.0, 1.0, 0.0);
Vector3D i = SubtractJoints(joints[JointType.HipRight], joints[JointType.HipLeft]);
i.Normalize();
Vector3D k = Vector3D.CrossProduct(i, j);
k.Normalize();
rotationMatrix.M11 = i.X;
rotationMatrix.M12 = i.Y;
rotationMatrix.M13 = i.Z;
rotationMatrix.M21 = j.X;
rotationMatrix.M22 = j.Y;
rotationMatrix.M23 = j.Z;
rotationMatrix.M31 = k.X;
rotationMatrix.M32 = k.Y;
rotationMatrix.M33 = k.Z;
}
rotationMatrix.Invert();
for (int i = 0; i < jointVectors.Count; i++)
{
jointVectors[(JointType)i] = jointVectors[(JointType)i] * rotationMatrix;
Vector3D normalized = jointVectors[(JointType)i];
normalized.Normalize();
jointVectors[(JointType)i] = normalized;
}
return jointVectors;
}
internal override RayHitTestParameters RayFromViewportPoint(Point p, Size viewSize, Rect3D boundingRect, out double distanceAdjustment)
{
// The camera may be animating. Take a snapshot of the current value
// and get the property values we need. (Window OS #992662)
Point3D position = Position;
Vector3D lookDirection = LookDirection;
Vector3D upDirection = UpDirection;
double zn = NearPlaneDistance;
double zf = FarPlaneDistance;
double width = Width;
//
// Compute rayParameters
//
// Find the point on the projection plane in post-projective space where
// the viewport maps to a 2x2 square from (-1,1)-(1,-1).
Point np = M3DUtil.GetNormalizedPoint(p, viewSize);
double aspectRatio = M3DUtil.GetAspectRatio(viewSize);
double w = width;
double h = w / aspectRatio;
// Direction is always perpendicular to the viewing surface.
Vector3D direction = new Vector3D(0, 0, -1);
// Apply the inverse of the view matrix to our ray.
Matrix3D viewMatrix = CreateViewMatrix(Transform, ref position, ref lookDirection, ref upDirection);
Matrix3D invView = viewMatrix;
invView.Invert();
// We construct our ray such that the origin resides on the near
// plane. If our near plane is too far from our the bounding box
// of our scene then the results will be inaccurate. (e.g.,
// OrthographicCameras permit negative near planes, so the near
// plane could be at -Inf.)
//
// However, it is permissable to move the near plane nearer to
// the scene bounds without changing what the ray intersects.
// If the near plane is sufficiently far from the scene bounds
// we make this adjustment below to increase precision.
Rect3D transformedBoundingBox =
M3DUtil.ComputeTransformedAxisAlignedBoundingBox(
ref boundingRect,
ref viewMatrix);
// DANGER: The NearPlaneDistance property is specified as a
// distance from the camera position along the
// LookDirection with (Near < Far).
//
// However, when we transform our scene bounds so that
// the camera is aligned with the negative Z-axis the
// relationship inverts (Near > Far) as illustrated
// below:
//
// NearPlane Y FarPlane
// | ^ |
// | | |
// | | (rect.Z + rect.SizeZ) |
// | | o____ |
// | | | | |
// | | | | |
// | | ____o |
// | | (rect.Z) |
// | Camera -> |
// +Z <----------+----------------------------> -Z
// | 0 |
//
// It is surprising, but its the "far" side of the
// transformed scene bounds that determines the near
// plane distance.
double zn2 = -AddEpsilon(transformedBoundingBox.Z + transformedBoundingBox.SizeZ);
if (zn2 > zn)
{
//
// Our near plane is far from our children. Construct a new
// near plane that's closer. Note that this will modify our
// distance computations, so we have to be sure to adjust our
// distances appropriately.
//
distanceAdjustment = zn2 - zn;
zn = zn2;
}
else
{
//
// Our near plane is either close to or in front of our
// children, so let's keep it -- no distance adjustment needed.
//
distanceAdjustment = 0.0;
}
// Our origin is the point normalized to the front of our viewing volume.
// To find our origin's x/y we just need to scale the normalize point by our
// width/height. In camera space we are looking down the negative Z axis
// so we just set Z to be -zn which puts us on the projection plane
// (Windows OS #1005064).
Point3D origin = new Point3D(np.X * (w / 2), np.Y * (h / 2), -zn);
invView.MultiplyPoint(ref origin);
invView.MultiplyVector(ref direction);
RayHitTestParameters rayParameters = new RayHitTestParameters(origin, direction);
//
// Compute HitTestProjectionMatrix
//
Matrix3D projectionMatrix = GetProjectionMatrix(aspectRatio, zn, zf);
// The projectionMatrix takes camera-space 3D points into normalized clip
// space.
// The viewportMatrix will take normalized clip space into
// viewport coordinates, with an additional 2D translation
// to put the ray at the origin.
Matrix3D viewportMatrix = new Matrix3D();
viewportMatrix.TranslatePrepend(new Vector3D(-p.X, viewSize.Height - p.Y, 0));
viewportMatrix.ScalePrepend(new Vector3D(viewSize.Width / 2, -viewSize.Height / 2, 1));
viewportMatrix.TranslatePrepend(new Vector3D(1, 1, 0));
// `First world-to-camera, then camera's projection, then normalized clip space to viewport.
rayParameters.HitTestProjectionMatrix =
viewMatrix *
projectionMatrix *
viewportMatrix;
return(rayParameters);
}
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;
}
}
/// <summary>
/// This function converts Kinect joints to a Z3 body.
/// It also changes the basis of body coordinates to make it
/// invariant to body position in relation to the sensor.
///
/// The origin of the new coordinate system is user hips.
/// </summary>
/// <param name="kinectJoints"></param>
/// <returns></returns>
public static Z3Body CreateZ3Body(
IReadOnlyDictionary<Microsoft.Kinect.JointType, Microsoft.Kinect.Joint>
kinectJoints)
{
var jointVectors = new Dictionary<Microsoft.Kinect.JointType, Vector3D>();
jointVectors.Add(Microsoft.Kinect.JointType.SpineMid, SubtractJoints(kinectJoints[Microsoft.Kinect.JointType.SpineMid], kinectJoints[Microsoft.Kinect.JointType.SpineBase]));
jointVectors.Add(Microsoft.Kinect.JointType.SpineShoulder, SubtractJoints(kinectJoints[Microsoft.Kinect.JointType.SpineShoulder], kinectJoints[Microsoft.Kinect.JointType.SpineMid]));
jointVectors.Add(Microsoft.Kinect.JointType.ShoulderLeft, SubtractJoints(kinectJoints[Microsoft.Kinect.JointType.ShoulderLeft], kinectJoints[Microsoft.Kinect.JointType.SpineShoulder]));
jointVectors.Add(Microsoft.Kinect.JointType.ElbowLeft, SubtractJoints(kinectJoints[Microsoft.Kinect.JointType.ElbowLeft], kinectJoints[Microsoft.Kinect.JointType.ShoulderLeft]));
jointVectors.Add(Microsoft.Kinect.JointType.WristLeft, SubtractJoints(kinectJoints[Microsoft.Kinect.JointType.WristLeft], kinectJoints[Microsoft.Kinect.JointType.ElbowLeft]));
jointVectors.Add(Microsoft.Kinect.JointType.HandLeft, SubtractJoints(kinectJoints[Microsoft.Kinect.JointType.HandLeft], kinectJoints[Microsoft.Kinect.JointType.WristLeft]));
jointVectors.Add(Microsoft.Kinect.JointType.HandTipLeft, SubtractJoints(kinectJoints[Microsoft.Kinect.JointType.HandTipLeft], kinectJoints[Microsoft.Kinect.JointType.HandLeft]));
jointVectors.Add(Microsoft.Kinect.JointType.ThumbLeft, SubtractJoints(kinectJoints[Microsoft.Kinect.JointType.ThumbLeft], kinectJoints[Microsoft.Kinect.JointType.HandLeft]));
jointVectors.Add(Microsoft.Kinect.JointType.Neck, SubtractJoints(kinectJoints[Microsoft.Kinect.JointType.Neck], kinectJoints[Microsoft.Kinect.JointType.SpineShoulder]));
jointVectors.Add(Microsoft.Kinect.JointType.Head, SubtractJoints(kinectJoints[Microsoft.Kinect.JointType.Head], kinectJoints[Microsoft.Kinect.JointType.Neck]));
jointVectors.Add(Microsoft.Kinect.JointType.ShoulderRight, SubtractJoints(kinectJoints[Microsoft.Kinect.JointType.ShoulderRight], kinectJoints[Microsoft.Kinect.JointType.SpineShoulder]));
jointVectors.Add(Microsoft.Kinect.JointType.ElbowRight, SubtractJoints(kinectJoints[Microsoft.Kinect.JointType.ElbowRight], kinectJoints[Microsoft.Kinect.JointType.ShoulderRight]));
jointVectors.Add(Microsoft.Kinect.JointType.WristRight, SubtractJoints(kinectJoints[Microsoft.Kinect.JointType.WristRight], kinectJoints[Microsoft.Kinect.JointType.ElbowRight]));
jointVectors.Add(Microsoft.Kinect.JointType.HandRight, SubtractJoints(kinectJoints[Microsoft.Kinect.JointType.HandRight], kinectJoints[Microsoft.Kinect.JointType.WristRight]));
jointVectors.Add(Microsoft.Kinect.JointType.HandTipRight, SubtractJoints(kinectJoints[Microsoft.Kinect.JointType.HandTipRight], kinectJoints[Microsoft.Kinect.JointType.HandRight]));
jointVectors.Add(Microsoft.Kinect.JointType.ThumbRight, SubtractJoints(kinectJoints[Microsoft.Kinect.JointType.ThumbRight], kinectJoints[Microsoft.Kinect.JointType.HandRight]));
// Spine base is the root of the system, as it has no direction to store, it stores its own position
jointVectors.Add(Microsoft.Kinect.JointType.SpineBase, SubtractJoints(kinectJoints[Microsoft.Kinect.JointType.SpineBase], new Joint()));
jointVectors.Add(Microsoft.Kinect.JointType.HipLeft, SubtractJoints(kinectJoints[Microsoft.Kinect.JointType.HipLeft], kinectJoints[Microsoft.Kinect.JointType.SpineBase]));
jointVectors.Add(Microsoft.Kinect.JointType.KneeLeft, SubtractJoints(kinectJoints[Microsoft.Kinect.JointType.KneeLeft], kinectJoints[Microsoft.Kinect.JointType.HipLeft]));
jointVectors.Add(Microsoft.Kinect.JointType.AnkleLeft, SubtractJoints(kinectJoints[Microsoft.Kinect.JointType.AnkleLeft], kinectJoints[Microsoft.Kinect.JointType.KneeLeft]));
jointVectors.Add(Microsoft.Kinect.JointType.FootLeft, SubtractJoints(kinectJoints[Microsoft.Kinect.JointType.FootLeft], kinectJoints[Microsoft.Kinect.JointType.AnkleLeft]));
jointVectors.Add(Microsoft.Kinect.JointType.HipRight, SubtractJoints(kinectJoints[Microsoft.Kinect.JointType.HipRight], kinectJoints[Microsoft.Kinect.JointType.SpineBase]));
jointVectors.Add(Microsoft.Kinect.JointType.KneeRight, SubtractJoints(kinectJoints[Microsoft.Kinect.JointType.KneeRight], kinectJoints[Microsoft.Kinect.JointType.HipRight]));
jointVectors.Add(Microsoft.Kinect.JointType.AnkleRight, SubtractJoints(kinectJoints[Microsoft.Kinect.JointType.AnkleRight], kinectJoints[Microsoft.Kinect.JointType.KneeRight]));
jointVectors.Add(Microsoft.Kinect.JointType.FootRight, SubtractJoints(kinectJoints[Microsoft.Kinect.JointType.FootRight], kinectJoints[Microsoft.Kinect.JointType.AnkleRight]));
var rotationMatrix = new Matrix3D();
InitMatrix(out rotationMatrix, kinectJoints);
rotationMatrix.Invert();
var joints = new Dictionary<PreposeGestures.JointType, Z3Point3D>();
var norms = new Dictionary<PreposeGestures.JointType, ArithExpr>();
norms.Add(PreposeGestures.JointType.SpineBase, Z3Math.Real(jointVectors[Microsoft.Kinect.JointType.SpineBase].Length));
joints.Add(PreposeGestures.JointType.SpineBase,
new Z3Point3D(
jointVectors[Microsoft.Kinect.JointType.SpineBase].X,
jointVectors[Microsoft.Kinect.JointType.SpineBase].Y,
jointVectors[Microsoft.Kinect.JointType.SpineBase].Z));
var jointTypes = EnumUtil.GetValues<Microsoft.Kinect.JointType>();
foreach (var jointType in jointTypes)
{
if (jointType != Microsoft.Kinect.JointType.SpineBase)
{
jointVectors[jointType] = jointVectors[jointType] * rotationMatrix;
var z3Joint = Convert(jointType);
norms.Add(z3Joint, Z3Math.Real(jointVectors[jointType].Length));
var temp = jointVectors[jointType];
temp.Normalize();
joints.Add(
z3Joint,
new Z3Point3D(
temp.X,
temp.Y,
-temp.Z));
}
}
return new Z3Body(joints, norms);
}
/// <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;
}
internal static void PrependInverseTransform( Matrix3D matrix, ref Matrix3D viewMatrix )
{
matrix.Invert();
viewMatrix.Prepend( matrix );
}
internal override RayHitTestParameters RayFromViewportPoint(Point p, Size viewSize, Rect3D boundingRect, out double distanceAdjustment)
{
//
// Compute rayParameters
//
// Find the point on the projection plane in post-projective space where
// the viewport maps to a 2x2 square from (-1,1)-(1,-1).
Point np = M3DUtil.GetNormalizedPoint(p, viewSize);
// This ray is consistent with a left-handed camera
// MatrixCamera should be right-handed and should be updated to be so.
// So (conceptually) the user clicked on the point (np.X,
// np.Y, 0) in post-projection clipping space and the ray
// extends in the direction (0, 0, 1) because our ray
// after projection looks down the positive z axis. We
// need to convert this ray and direction back to world
// space.
Matrix3D worldToCamera = GetViewMatrix() * ProjectionMatrix;
Matrix3D cameraToWorld = worldToCamera;
if (!cameraToWorld.HasInverse)
{
// When the following issue is addressed we should
// investigate if the custom code paths in Orthographic and PerspectiveCamera
// are worth keeping. They may not be buying us anything aside from handling
// singular matrices.
// Need to handle singular matrix cameras
throw new NotSupportedException(SR.Get(SRID.HitTest_Singular));
}
cameraToWorld.Invert();
Point4D origin4D = new Point4D(np.X, np.Y, 0, 1) * cameraToWorld;
Point3D origin = new Point3D(origin4D.X / origin4D.W,
origin4D.Y / origin4D.W,
origin4D.Z / origin4D.W);
// To transform the direction we use the Jacobian of
// cameraToWorld at the point np.X,np.Y,0 that we just
// transformed.
//
// The Jacobian of the homogeneous matrix M is a 3x3 matrix.
//
// Let x be the point we are computing the Jacobian at, and y be the
// result of transforming x by M, i.e.
// (wy w) = (x 1) M
// Where (wy w) is the homogeneous point representing y with w as its homogeneous coordinate
// And (x 1) is the homogeneous point representing x with 1 as its homogeneous coordinate
//
// Then the i,j component of the Jacobian (at x) is
// (M_ij - M_i4 y_j) / w
//
// Since we're only concerned with the direction of the
// transformed vector and not its magnitude, we can scale
// this matrix by a POSITIVE factor. The computation
// below computes the Jacobian scaled by 1/w and then
// after we normalize the final vector we flip it around
// if w is negative.
//
// To transform a vector we just right multiply it by this Jacobian matrix.
//
// Compute the Jacobian at np.X,np.Y,0 ignoring the constant factor of w.
// Here's the pattern
//
// double Jij = cameraToWorld.Mij - cameraToWorld.Mi4 * origin.j
//
// but we only need J31,J32,&J33 because we're only
// transforming the vector 0,0,1
double J31 = cameraToWorld.M31 - cameraToWorld.M34 * origin.X;
double J32 = cameraToWorld.M32 - cameraToWorld.M34 * origin.Y;
double J33 = cameraToWorld.M33 - cameraToWorld.M34 * origin.Z;
// Then multiply that matrix by (0, 0, 1) which is
// the direction of the ray in post-projection space.
Vector3D direction = new Vector3D(J31, J32, J33);
direction.Normalize();
// We multiplied by the Jacobian times W, so we need to
// account for whether that flipped our result or not.
if (origin4D.W < 0)
{
direction = -direction;
}
RayHitTestParameters rayParameters = new RayHitTestParameters(origin, direction);
//
// Compute HitTestProjectionMatrix
//
// The viewportMatrix will take normalized clip space into
// viewport coordinates, with an additional 2D translation
// to put the ray at the origin.
Matrix3D viewportMatrix = new Matrix3D();
viewportMatrix.TranslatePrepend(new Vector3D(-p.X, viewSize.Height - p.Y, 0));
viewportMatrix.ScalePrepend(new Vector3D(viewSize.Width / 2, -viewSize.Height / 2, 1));
viewportMatrix.TranslatePrepend(new Vector3D(1, 1, 0));
// `First world-to-camera, then camera's projection, then normalized clip space to viewport.
rayParameters.HitTestProjectionMatrix =
worldToCamera *
viewportMatrix;
//
// MatrixCamera does not allow for Near/Far plane adjustment, so
// the distanceAdjustment remains 0.
//
distanceAdjustment = 0.0;
return(rayParameters);
}
void OnRendering()
{
if (LineCollection.Count == 0)
{
return;
}
Matrix3D matx = VisualInfo.GetTotalTransform(this);
if (matx == VisualInfo.ZeroMatrix)
{
return;
}
// How can this happen????
if (matx.IsIdentity)
{
return;
}
if (matx != matxVisualToScreen)
{
matxVisualToScreen = matx;
matxScreenToVisual = matx;
if (matxScreenToVisual.HasInverse)
{
matxScreenToVisual.Invert(); // might not be possible !!!!!!
needRecalculation = true;
}
else
throw new ApplicationException("Here's where the problem is");
}
if (needRecalculation)
{
Recalculate();
needRecalculation = false;
}
}
/// <summary>
/// This function converts Kinect joints to different coordinate system
/// invariant to body position in relation to the sensor.
/// The origin of the new coordinate system is middel point between the user hips.
///
/// </summary>
/// <param name="kinectJoints"></param>
/// <returns></returns>
public static Dictionary<Microsoft.Kinect.JointType, Vector3D> KinectToHipsSpineCoordinateSystem(
IReadOnlyDictionary<Microsoft.Kinect.JointType, Microsoft.Kinect.Joint>
kinectJoints)
{
// positions are updated considering the spine base as center and a new coordinate system
// with hips (left to right) as the X axis
// (0,1,0) based on the senor as the Y axis
// and the cross product between X and Y as the Z axis pointing towards the user front
var jointPositionsRotated = new Dictionary<Microsoft.Kinect.JointType, Vector3D>();
jointPositionsRotated.Add(Microsoft.Kinect.JointType.SpineMid, SubtractJoints(kinectJoints[Microsoft.Kinect.JointType.SpineMid], kinectJoints[Microsoft.Kinect.JointType.SpineBase]));
jointPositionsRotated.Add(Microsoft.Kinect.JointType.SpineShoulder, SubtractJoints(kinectJoints[Microsoft.Kinect.JointType.SpineShoulder], kinectJoints[Microsoft.Kinect.JointType.SpineBase]));
jointPositionsRotated.Add(Microsoft.Kinect.JointType.ShoulderLeft, SubtractJoints(kinectJoints[Microsoft.Kinect.JointType.ShoulderLeft], kinectJoints[Microsoft.Kinect.JointType.SpineBase]));
jointPositionsRotated.Add(Microsoft.Kinect.JointType.ElbowLeft, SubtractJoints(kinectJoints[Microsoft.Kinect.JointType.ElbowLeft], kinectJoints[Microsoft.Kinect.JointType.SpineBase]));
jointPositionsRotated.Add(Microsoft.Kinect.JointType.WristLeft, SubtractJoints(kinectJoints[Microsoft.Kinect.JointType.WristLeft], kinectJoints[Microsoft.Kinect.JointType.SpineBase]));
jointPositionsRotated.Add(Microsoft.Kinect.JointType.HandLeft, SubtractJoints(kinectJoints[Microsoft.Kinect.JointType.HandLeft], kinectJoints[Microsoft.Kinect.JointType.SpineBase]));
jointPositionsRotated.Add(Microsoft.Kinect.JointType.HandTipLeft, SubtractJoints(kinectJoints[Microsoft.Kinect.JointType.HandTipLeft], kinectJoints[Microsoft.Kinect.JointType.SpineBase]));
jointPositionsRotated.Add(Microsoft.Kinect.JointType.ThumbLeft, SubtractJoints(kinectJoints[Microsoft.Kinect.JointType.ThumbLeft], kinectJoints[Microsoft.Kinect.JointType.SpineBase]));
jointPositionsRotated.Add(Microsoft.Kinect.JointType.Neck, SubtractJoints(kinectJoints[Microsoft.Kinect.JointType.Neck], kinectJoints[Microsoft.Kinect.JointType.SpineBase]));
jointPositionsRotated.Add(Microsoft.Kinect.JointType.Head, SubtractJoints(kinectJoints[Microsoft.Kinect.JointType.Head], kinectJoints[Microsoft.Kinect.JointType.SpineBase]));
jointPositionsRotated.Add(Microsoft.Kinect.JointType.ShoulderRight, SubtractJoints(kinectJoints[Microsoft.Kinect.JointType.ShoulderRight], kinectJoints[Microsoft.Kinect.JointType.SpineBase]));
jointPositionsRotated.Add(Microsoft.Kinect.JointType.ElbowRight, SubtractJoints(kinectJoints[Microsoft.Kinect.JointType.ElbowRight], kinectJoints[Microsoft.Kinect.JointType.SpineBase]));
jointPositionsRotated.Add(Microsoft.Kinect.JointType.WristRight, SubtractJoints(kinectJoints[Microsoft.Kinect.JointType.WristRight], kinectJoints[Microsoft.Kinect.JointType.SpineBase]));
jointPositionsRotated.Add(Microsoft.Kinect.JointType.HandRight, SubtractJoints(kinectJoints[Microsoft.Kinect.JointType.HandRight], kinectJoints[Microsoft.Kinect.JointType.SpineBase]));
jointPositionsRotated.Add(Microsoft.Kinect.JointType.HandTipRight, SubtractJoints(kinectJoints[Microsoft.Kinect.JointType.HandTipRight], kinectJoints[Microsoft.Kinect.JointType.SpineBase]));
jointPositionsRotated.Add(Microsoft.Kinect.JointType.ThumbRight, SubtractJoints(kinectJoints[Microsoft.Kinect.JointType.ThumbRight], kinectJoints[Microsoft.Kinect.JointType.SpineBase]));
// Spine base is the root of the system, as it has no direction to store, it stores its own position
jointPositionsRotated.Add(Microsoft.Kinect.JointType.SpineBase, SubtractJoints(kinectJoints[Microsoft.Kinect.JointType.SpineBase], new Joint()));
jointPositionsRotated.Add(Microsoft.Kinect.JointType.HipLeft, SubtractJoints(kinectJoints[Microsoft.Kinect.JointType.HipLeft], kinectJoints[Microsoft.Kinect.JointType.SpineBase]));
jointPositionsRotated.Add(Microsoft.Kinect.JointType.KneeLeft, SubtractJoints(kinectJoints[Microsoft.Kinect.JointType.KneeLeft], kinectJoints[Microsoft.Kinect.JointType.SpineBase]));
jointPositionsRotated.Add(Microsoft.Kinect.JointType.AnkleLeft, SubtractJoints(kinectJoints[Microsoft.Kinect.JointType.AnkleLeft], kinectJoints[Microsoft.Kinect.JointType.SpineBase]));
jointPositionsRotated.Add(Microsoft.Kinect.JointType.FootLeft, SubtractJoints(kinectJoints[Microsoft.Kinect.JointType.FootLeft], kinectJoints[Microsoft.Kinect.JointType.SpineBase]));
jointPositionsRotated.Add(Microsoft.Kinect.JointType.HipRight, SubtractJoints(kinectJoints[Microsoft.Kinect.JointType.HipRight], kinectJoints[Microsoft.Kinect.JointType.SpineBase]));
jointPositionsRotated.Add(Microsoft.Kinect.JointType.KneeRight, SubtractJoints(kinectJoints[Microsoft.Kinect.JointType.KneeRight], kinectJoints[Microsoft.Kinect.JointType.SpineBase]));
jointPositionsRotated.Add(Microsoft.Kinect.JointType.AnkleRight, SubtractJoints(kinectJoints[Microsoft.Kinect.JointType.AnkleRight], kinectJoints[Microsoft.Kinect.JointType.SpineBase]));
jointPositionsRotated.Add(Microsoft.Kinect.JointType.FootRight, SubtractJoints(kinectJoints[Microsoft.Kinect.JointType.FootRight], kinectJoints[Microsoft.Kinect.JointType.SpineBase]));
var rotationMatrix = new Matrix3D();
InitMatrix(out rotationMatrix, kinectJoints);
rotationMatrix.Invert();
var jointTypes = EnumUtil.GetValues<Microsoft.Kinect.JointType>();
foreach (var jointType in jointTypes)
{
if (jointType != Microsoft.Kinect.JointType.SpineBase)
{
var current = jointPositionsRotated[jointType];
var rotated = current * rotationMatrix;
jointPositionsRotated[jointType] = rotated;
}
}
return jointPositionsRotated;
}
