private void RotateTrackball(Point p1, Point p2, Vector3 rotateAround) { // http://viewport3d.com/trackball.htm // http://www.codeplex.com/3DTools/Thread/View.aspx?ThreadId=22310 Vector3 v1, v2; if (ConstrainAxis.HasValue) { // preparing to compute the ratio to the screen var diag = Math.Sqrt( viewport.ActualWidth * viewport.ActualWidth + viewport.ActualHeight * viewport.ActualHeight ); // can we project the constraintAxis onto the view? var t3 = Vector3.TransformCoordinate(ConstrainAxis.Value, camera.CameraInternal.GetViewMatrix()); var dir = new Vector2(t3.X, t3.Y); // axis of Constraint in view coordinates var pp1 = p1.ToVector2(); // computing distance perpendicular to axis in view coordinates var r1 = pp1 - (pp1 * dir); var pp2 = p2.ToVector2(); // computing distance perpendicular to axis in view coordinates var r2 = pp2 - (pp2 * dir); var angle = (r2.Length() - r1.Length()) / diag * 4; // Create the transform currentRotation *= Matrix.RotationAxis(Vector3.Normalize(ConstrainAxis.Value), (float)(angle * this.RotationSensitivity * 5)); UpdateTransform(); } else { v1 = ProjectToTrackball(p1, viewport.ActualWidth, viewport.ActualHeight); v2 = ProjectToTrackball(p2, viewport.ActualWidth, viewport.ActualHeight); // transform the trackball coordinates to view space var viewZ = camera.CameraInternal.LookDirection; var viewX = Vector3.Cross(camera.CameraInternal.UpDirection, viewZ); var viewY = Vector3.Cross(viewX, viewZ); viewX.Normalize(); viewY.Normalize(); viewZ.Normalize(); var u1 = (viewZ * v1.Z) + (viewX * v1.X) + (viewY * v1.Y); var u2 = (viewZ * v2.Z) + (viewX * v2.X) + (viewY * v2.Y); // Could also use the Camera ViewMatrix // var vm = Viewport3DHelper.GetViewMatrix(this.ActualCamera); // vm.Invert(); // var ct = new MatrixTransform3D(vm); // var u1 = ct.Transform(v1); // var u2 = ct.Transform(v2); // Find the rotation axis and angle var axis = Vector3.Cross(u1, u2); if (axis.LengthSquared() < 1e-8) { return; } var angle = u1.AngleBetween(u2); // Create the transform currentRotation *= Matrix.RotationAxis(Vector3.Normalize(axis), (float)(angle * this.RotationSensitivity * 5)); UpdateTransform(); } }