public static void CalcTMatrix(Vector3D value)
{
Tmatrix = new Matrix3D(1, 0, 0, -value.X,
0, 1, 0, -value.Y,
0, 0, 1, -value.Z,
0, 0, 0, 1);
}
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();
}
internal static void PrependInverseTransform(Transform3D transform, ref Matrix3D viewMatrix)
{
if (transform != null && transform != Transform3D.Identity)
{
PrependInverseTransform(transform.Value, ref viewMatrix);
}
}
public PhotoAlbum()
{
InitializeComponent();
dt.Interval = TimeSpan.FromSeconds(2);
dt.Tick += new EventHandler(dt_Tick);
for (int i = 0; i < 16; i++)
{
ImageBrush ib = new ImageBrush(new BitmapImage(new Uri("pack://application:,,,/images/albums/im" + i + ".jpg")));
ib.Stretch = Stretch.Uniform;
ModelVisual3D mv = new ModelVisual3D();
Material mat = new DiffuseMaterial(ib);
GeometryModel3D plane = new GeometryModel3D(planeFactory.Mesh, mat);
mv.Content = plane;
mvs[i] = mv;
myViewPort3D.Children.Add(mv);
Matrix3D trix = new Matrix3D();
double x = ran.NextDouble() * 50 - 50;
double y = ran.NextDouble() * 2 - 2;
double z = -i * 10;
p3s[i] = new Point3D(x, y, z);
trix.Append(new TranslateTransform3D(x, y, z).Value);
mv.Transform = new MatrixTransform3D(trix);
}
pa = new Point3DAnimation(p3s[0], TimeSpan.FromMilliseconds(300));
pa.AccelerationRatio = 0.3;
pa.DecelerationRatio = 0.3;
pa.Completed += new EventHandler(pa_Completed);
cam.BeginAnimation(PerspectiveCamera.PositionProperty, pa);
}
private static void WriteTransformMatrix(Matrix3D m, XmlWriter bw)
{
bw.WriteStartElement("T");
if (m.IsIdentity)
bw.WriteAttributeString("value", "Identity");
else
{
bw.WriteAttributeString("M11", m.M11.ToString());
bw.WriteAttributeString("M12", m.M12.ToString());
bw.WriteAttributeString("M13", m.M13.ToString());
bw.WriteAttributeString("M14", m.M14.ToString());
bw.WriteAttributeString("M21", m.M21.ToString());
bw.WriteAttributeString("M22", m.M22.ToString());
bw.WriteAttributeString("M23", m.M23.ToString());
bw.WriteAttributeString("M24", m.M24.ToString());
bw.WriteAttributeString("M31", m.M31.ToString());
bw.WriteAttributeString("M32", m.M32.ToString());
bw.WriteAttributeString("M33", m.M33.ToString());
bw.WriteAttributeString("M34", m.M34.ToString());
bw.WriteAttributeString("M41", m.OffsetX.ToString());
bw.WriteAttributeString("M42", m.OffsetY.ToString());
bw.WriteAttributeString("M43", m.OffsetZ.ToString());
bw.WriteAttributeString("M44", m.M44.ToString());
}
bw.WriteEndElement();
}
protected override CollisionMask OnInitialise(Matrix3D initialMatrix)
{
if (Visual.Content != null)
{
TerrianCollisionMask3D terrianCollisionMask3D = null;
CollisionMask collisionMask = _GetCollisionMask(this.Visual);
if (collisionMask != null)
{
if (!(collisionMask is TerrianCollisionMask3D))
throw new InvalidOperationException("Currently another type of CollisionMaskMask is attached to this visual.");
else
terrianCollisionMask3D = (TerrianCollisionMask3D)collisionMask;
}
else
{
terrianCollisionMask3D = new TerrianCollisionMask3D();
_SetCollisionMask(this.Visual, terrianCollisionMask3D);
}
terrianCollisionMask3D.Initialise(this.World, initialMatrix);
return terrianCollisionMask3D;
}
else
return null;
}
public Transform3DGroup CreateTransformGroup(Halo3.ObjectChunk placedObject)
{
var transformGroup = new Transform3DGroup();
float yaw, pitch, roll;
Core.Helpers.VectorMath.Convert.ToYawPitchRoll(
placedObject.SpawnPosition.Right,
placedObject.SpawnPosition.Forward,
placedObject.SpawnPosition.Up,
out yaw,
out pitch,
out roll);
// For some reason you have to swag the roll and yaw.
var swag = Microsoft.Xna.Framework.Quaternion.CreateFromYawPitchRoll(roll, pitch, yaw);
// Apply 3D Matrix
var matrix = new Matrix3D();
matrix.Rotate(new Quaternion(swag.X, swag.Y, swag.Z, swag.W));
matrix.OffsetX = placedObject.SpawnCoordinates.X;
matrix.OffsetY = placedObject.SpawnCoordinates.Y;
matrix.OffsetZ = placedObject.SpawnCoordinates.Z;
// TODO: F**K THIS VALUE
// TODO: AND F**K BUNGIE
//matrix.Prepend(new Matrix3D
// {
// OffsetX = 0,
// OffsetY = 0,
// OffsetZ = 0
// });
transformGroup.Children.Add(new MatrixTransform3D(matrix));
return transformGroup;
}
public static Matrix3D CalcRotationMatrix(double x, double y, double z, Point3D center, Vector3D up, Vector3D look, Transform3D transform, RotationType type)
{
//Transform3DGroup trm = new Transform3DGroup();
//trm.Children.Add(transform);
Vector3D realup = transform.Transform(up);
if (type != RotationType.LockAxisY)
{
up = realup;
}
if (type != RotationType.LockAxisZ)
{
look = transform.Transform(look);
}
center = transform.Transform(center);
Vector3D axisX = Vector3D.CrossProduct(up, look);
Matrix3D matrix = new Matrix3D();
//Quaternion q = new Quaternion();
//q.
double ang = AngleBetween(realup, YAxis) + x;
if (ang >= 90)
{
x = 90 - ang;
}
matrix.RotateAt(new Quaternion(axisX, x), center);
matrix.RotateAt(new Quaternion(up, y), center);
matrix.RotateAt(new Quaternion(look, z), center);
return matrix;
}
//// 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;
}
/// <summary>
/// 当窗口大小发生改变时，更新视口投影矩阵
/// </summary>
public void UpdateProjViewMat(Double height, double width)
{
double aspectRatio = width / height;
double FoV = MathUtils.DegreesToRadians(45);
double zn = 0.125;
double xScale = 1 / Math.Tan(FoV / 2);
double yScale = aspectRatio * xScale;
//double yScale = 1 / Math.Tan(FoV / 2);
//double xScale = yScale / aspectRatio;
double m33 = -1;
double m43 = zn * m33;
projViewMat = new Matrix3D(
xScale, 0, 0, 0,
0, yScale, 0, 0,
0, 0, m33, -1,
0, 0, m43, 0);
double scaleX = width / 2;
double scaleY = height / 2;
projViewMat.Append(new Matrix3D(
scaleX, 0, 0, 0,
0, -scaleY, 0, 0,
0, 0, 1, 0,
scaleX, scaleY, 0, 1));
UpdateTo2DMat();
}
/// <summary>
/// Creates a new detection result
/// </summary>
/// <param name="marker">A reference to the found marker.</param>
/// <param name="confidence">The confidence / quality of the result.</param>
/// <param name="transformation">The transformation matrix for the marker.</param>
/// <param name="square">The pixel coordinates where the square marker was found. </param>
public DetectionResult(Marker marker, double confidence, Matrix3D transformation, Square square)
{
this.Marker = marker;
this.Confidence = confidence;
this.Transformation = transformation;
this.Square = square;
}
public static Matrix3D ToMatrix3D(this Matrix matrix)
{
Matrix3D matrix3D = new Matrix3D()
{
M11 = matrix.M11,
M12 = matrix.M12,
M13 = matrix.M13,
M14 = matrix.M14,
M21 = matrix.M21,
M22 = matrix.M22,
M23 = matrix.M23,
M24 = matrix.M24,
M31 = matrix.M31,
M32 = matrix.M32,
M33 = matrix.M33,
M34 = matrix.M34,
OffsetX = matrix.M41,
OffsetY = matrix.M42,
OffsetZ = matrix.M43,
M44 = matrix.M44
};
return matrix3D;
}
/// <summary>
/// Create a look at matrix using U, V, and N vectors
/// </summary>
/// <param name="eye">Location of the eye</param>
/// <param name="lookDirection">Direction to look</param>
/// <param name="up">The up vector</param>
/// <returns>Transform matrix from world space to camera space.</returns>
public static Matrix3D LookAt(Vector3D eye, Vector3D lookDirection, Vector3D up)
{
var n = lookDirection;
n.Normalize();
var v = up - (Vector3D.DotProduct(up, n) * n);
v.Normalize();
// The "-" below makes this be a right-handed uvn space so we aren't
// negating the x component in SensorToScreenCoordinatesTransform.
// It also makes SensorToScreenPositionTransform give an immediately
// usable transform without having to flip the X.
var u = -Vector3D.CrossProduct(n, v);
var lookAt = new Matrix3D(
u.X,
v.X,
n.X,
0,
u.Y,
v.Y,
n.Y,
0,
u.Z,
v.Z,
n.Z,
0,
-Vector3D.DotProduct(u, eye),
-Vector3D.DotProduct(v, eye),
-Vector3D.DotProduct(n, eye),
1);
return lookAt;
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="matrix">Combined matrix</param>
/// <param name="normalize">Normalize?</param>
public Frustum( Matrix3D matrix, bool normalize )
{
// Left clipping plane
planes[ 0 ].Normal = new Vector3D( matrix.M14 + matrix.M11, matrix.M24 + matrix.M21, matrix.M34 + matrix.M31 );
planes[ 0 ].D = matrix.M44 + matrix.OffsetX;
// Right clipping plane
planes[ 1 ].Normal = new Vector3D( matrix.M14 - matrix.M11, matrix.M24 - matrix.M21, matrix.M34 - matrix.M31 );
planes[ 1 ].D = matrix.M44 - matrix.OffsetX;
// Top clipping plane
planes[ 2 ].Normal = new Vector3D( matrix.M14 - matrix.M12, matrix.M24 - matrix.M22, matrix.M34 - matrix.M32 );
planes[ 2 ].D = matrix.M44 - matrix.OffsetY;
// Bottom clipping plane
planes[ 3 ].Normal = new Vector3D( matrix.M14 + matrix.M12, matrix.M24 + matrix.M22, matrix.M34 + matrix.M32 );
planes[ 3 ].D = matrix.M44 + matrix.OffsetY;
// Near clipping plane
planes[ 4 ].Normal = new Vector3D( matrix.M13, matrix.M23, matrix.M33 );
planes[ 4 ].D = matrix.OffsetZ;
// Far clipping plane
planes[ 5 ].Normal = new Vector3D( matrix.M14 - matrix.M13, matrix.M24 - matrix.M23, matrix.M34 - matrix.M33 );
planes[ 5 ].D = matrix.M44 - matrix.OffsetZ;
// Normalize the plane equations, if requested
if ( normalize )
{
for ( int index = 0; index < planes.Length; ++index ) planes[ index ].Normalize();
}
}
public MoveEventsHandler(MVMIH.IPositionHandle positionHandle)
{
_positionHandle = positionHandle;
_matrix = GetTranslateTransformation(positionHandle);
_positionHandle.StartMove();
}
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);
}
private static Matrix3D GetViewMatrix(ProjectionCamera camera)
{
if (camera == null) throw new ArgumentNullException("camera");
// This math is identical to what you find documented for
// D3DXMatrixLookAtRH with the exception that WPF uses a
// LookDirection vector rather than a LookAt point.
Vector3D zAxis = -camera.LookDirection;
zAxis.Normalize();
Vector3D xAxis = Vector3D.CrossProduct(camera.UpDirection, zAxis);
xAxis.Normalize();
Vector3D yAxis = Vector3D.CrossProduct(zAxis, xAxis);
Vector3D position = (Vector3D)camera.Position;
double offsetX = -Vector3D.DotProduct(xAxis, position);
double offsetY = -Vector3D.DotProduct(yAxis, position);
double offsetZ = -Vector3D.DotProduct(zAxis, position);
Matrix3D m = new Matrix3D(
xAxis.X, yAxis.X, zAxis.X, 0,
xAxis.Y, yAxis.Y, zAxis.Y, 0,
xAxis.Z, yAxis.Z, zAxis.Z, 0,
offsetX, offsetY, offsetZ, 1);
return m;
}
/// <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 Vector3D MatrixToEulerAngle(Matrix3D pMatrix)
{
NewtonVector3 aNewtonVector3 = new NewtonVector3( new Vector3D() );
Newton.NewtonGetEulerAngle(new NewtonMatrix(pMatrix).NWMatrix,
aNewtonVector3.NWVector3);
return aNewtonVector3.ToDirectX();
}
public static Matrix3D ToMatrix3D( this Matrix4x4F source )
{
Matrix3D destination = new Matrix3D( );
destination.M11 = (float)source.M11;
destination.M12 = (float)source.M12;
destination.M13 = (float)source.M13;
destination.M14 = (float)source.M14;
destination.M21 = (float)source.M21;
destination.M22 = (float)source.M22;
destination.M23 = (float)source.M23;
destination.M24 = (float)source.M24;
destination.M31 = (float)source.M31;
destination.M32 = (float)source.M32;
destination.M33 = (float)source.M33;
destination.M34 = (float)source.M34;
destination.OffsetX = (float)source.M41;
destination.OffsetY = (float)source.M42;
destination.OffsetZ = (float)source.M43;
destination.M44 = (float)source.M44;
return destination;
}
public void CreateSphere(Vector3D pRadius, Matrix3D pOffsetMatrix)
{
NewtonMatrix aMatrix = new NewtonMatrix(pOffsetMatrix);
m_Handle = Newton.NewtonCreateSphere(m_World.Handle, (float)pRadius.X, (float)pRadius.Y, (float)pRadius.Z, aMatrix.NWMatrix);
CHashTables.Collision.Add(m_Handle, this);
}
/// <summary>
/// Updates the transforms.
/// </summary>
/// <returns>
/// True if the transform was changed.
/// </returns>
public bool UpdateTransforms()
{
var newTransform = this.visual.GetViewportTransform();
if (double.IsNaN(newTransform.M11))
{
return false;
}
if (!newTransform.HasInverse)
{
return false;
}
if (newTransform == this.visualToScreen)
{
return false;
}
this.visualToScreen = newTransform;
this.screenToVisual = newTransform.Inverse();
if (this.viewport == null)
{
this.viewport = this.visual.GetViewport3D();
}
this.projectionToScreen = this.viewport.GetProjectionMatrix() * this.viewport.GetViewportTransform();
this.visualToProjection = this.visualToScreen * this.projectionToScreen.Inverse();
return true;
}
public CTire VehicleAddTire(Matrix3D pLocalMatrix,
Vector3D pPin,
float pMass,
float pWidth,
float pRadius,
float pSuspesionShock,
float pSuspesionSpring,
float pSuspesionLength,
object pUserData,
int pCollisionID)
{
IntPtr aTireHandle = Newton.NewtonVehicleAddTire(m_Handle,
new NewtonMatrix(pLocalMatrix).NWMatrix,
new NewtonVector3(pPin).NWVector3,
pMass,
pWidth,
pRadius,
pSuspesionShock,
pSuspesionSpring,
pSuspesionLength,
(IntPtr)0, //pUserData.GetHashCode(),
pCollisionID);
CTire aTire = new CTire(this, aTireHandle);
aTire.UserData = pUserData;
return aTire;
}
private int AddModelFaces(Matrix3D parentMatrix, Model3D model)
{
if (model.Transform != null)
parentMatrix = model.Transform.Value * parentMatrix;
int result = 0;
Model3DGroup models = (model as Model3DGroup);
if (models != null)
{
// This is a group. Recurse through the children
foreach (Model3D m in models.Children)
{
result += AddModelFaces(parentMatrix, m);
}
}
else
{
if (!(model is GeometryModel3D))
throw new InvalidOperationException("Current only GeometryModel3D models supported for TerrianCollisionMask3D.");
Geometry3D geometry = ((GeometryModel3D)model).Geometry;
IList<Point3D> meshPoints = GeometryHelper.GetGeometryPoints((MeshGeometry3D)geometry, parentMatrix);
if (meshPoints != null)
{
AddFaces(meshPoints, 3, false);
result = meshPoints.Count;
}
}
return result;
}
public void initialize(Matrix3D M_initial, Matrix3D M_target)
{
this.M_initial = M_initial;
this.M_target = M_target;
get_p0();
get_p2();
get_p1();
}
public static void PrintMatrix(Matrix3D R)
{
Console.WriteLine("rotation matrix 1 : ");
Console.WriteLine(R.M11 + "," + R.M12 + "," + R.M13 + "," + R.M14);
Console.WriteLine(R.M21 + "," + R.M22 + "," + R.M23 + "," + R.M24);
Console.WriteLine(R.M31 + "," + R.M32 + "," + R.M33 + "," + R.M34);
Console.WriteLine(R.OffsetX + "," + R.OffsetY + "," + R.OffsetZ + "," + R.M44);
}
/// <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 void CreateBox(Vector3D pSize, Matrix3D pOffsetMatrix)
{
NewtonMatrix aMatrix = new NewtonMatrix(pOffsetMatrix);
m_Handle = Newton.NewtonCreateBox(m_World.Handle,
(float)pSize.X, (float)pSize.Y, (float)pSize.Z, aMatrix.NWMatrix);
CHashTables.Collision.Add(m_Handle, this);
}
public static Vector3D transform(Matrix3D matrix, Vector3D vector)
{
Vector3D output = new Vector3D();
output.X = matrix.M11 * vector.X + matrix.M12 * vector.Y + matrix.M13 * vector.Z + matrix.M14;
output.Y = matrix.M21 * vector.X + matrix.M22 * vector.Y + matrix.M23 * vector.Z + matrix.M24;
output.Z = matrix.M31 * vector.X + matrix.M32 * vector.Y + matrix.M33 * vector.Z + matrix.M34;
return output;
}
public void CreateChamferCylinder(float pRadius, float pHeight, Matrix3D pOffsetMatrix)
{
NewtonMatrix aMatrix = new NewtonMatrix(pOffsetMatrix);
m_Handle = Newton.NewtonCreateChamferCylinder(m_World.Handle,
pRadius, pHeight, aMatrix.NWMatrix);
CHashTables.Collision.Add(m_Handle, this);
}
public ConvextCollisionModifier(ConvexCollisionMask collisionMask, Matrix3D initialMatrix)
{
if (collisionMask == null) throw new ArgumentNullException("collisionMask");
_collisionMask = collisionMask;
Initialise(collisionMask.World);
NewtonCollision.ConvexHullModifierMatrix = initialMatrix;
}
public static System.Windows.Media.Media3D.Matrix3D ToMatrix3D(this Matrix4 mat)
{
System.Windows.Media.Media3D.Matrix3D m = new System.Windows.Media.Media3D.Matrix3D(mat.M11, mat.M12, mat.M13, mat.M14,
mat.M21, mat.M22, mat.M23, mat.M24,
mat.M31, mat.M32, mat.M33, mat.M34,
mat.M41, mat.M42, mat.M43, mat.M44);
return(m);
}
public Matrix3()
{
Identity3();
Matrix3D m3 = new Matrix3D();
m3.SetIdentity();
}
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 double[] ToArray(this System.Windows.Media.Media3D.Matrix3D mat)
{
if (mat == null)
{
throw new ArgumentNullException("matrix");
}
return(new double[] { mat.M11, mat.M12, mat.M13, mat.M14,
mat.M21, mat.M22, mat.M23, mat.M24,
mat.M31, mat.M32, mat.M33, mat.M34,
mat.OffsetX, mat.OffsetY, mat.OffsetZ, mat.M44, });
}
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 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);
}
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 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);
}
public static List <double> GetValues(this System.Windows.Media.Media3D.Matrix3D matrix, int decimals)
{
List <double>?values = new List <double>();
values.Add(Round(matrix.M11, decimals));
values.Add(Round(matrix.M12, decimals));
values.Add(Round(matrix.M13, decimals));
values.Add(Round(matrix.M14, decimals));
values.Add(Round(matrix.M21, decimals));
values.Add(Round(matrix.M22, decimals));
values.Add(Round(matrix.M23, decimals));
values.Add(Round(matrix.M24, decimals));
values.Add(Round(matrix.M31, decimals));
values.Add(Round(matrix.M32, decimals));
values.Add(Round(matrix.M33, decimals));
values.Add(Round(matrix.M34, decimals));
values.Add(Round(matrix.OffsetX, decimals));
values.Add(Round(matrix.OffsetY, decimals));
values.Add(Round(matrix.OffsetZ, decimals));
values.Add(Round(matrix.M44, decimals));
return(values);
}
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);
}
public static Point4D Multiply(Point4D point, Matrix3D matrix)
{
return(matrix.Transform(point));
}
public static bool AlmostEqual(this System.Windows.Media.Media3D.Matrix3D a,
System.Windows.Media.Media3D.Matrix3D b, int decimals = 4)
{
if (Round(a.M11, decimals) != Round(b.M11, decimals))
{
return(false);
}
if (Round(a.M12, decimals) != Round(b.M12, decimals))
{
return(false);
}
if (Round(a.M13, decimals) != Round(b.M13, decimals))
{
return(false);
}
if (Round(a.M14, decimals) != Round(b.M14, decimals))
{
return(false);
}
if (Round(a.M21, decimals) != Round(b.M21, decimals))
{
return(false);
}
if (Round(a.M22, decimals) != Round(b.M22, decimals))
{
return(false);
}
if (Round(a.M23, decimals) != Round(b.M23, decimals))
{
return(false);
}
if (Round(a.M24, decimals) != Round(b.M24, decimals))
{
return(false);
}
if (Round(a.M31, decimals) != Round(b.M31, decimals))
{
return(false);
}
if (Round(a.M32, decimals) != Round(b.M32, decimals))
{
return(false);
}
if (Round(a.M33, decimals) != Round(b.M33, decimals))
{
return(false);
}
if (Round(a.M34, decimals) != Round(b.M34, decimals))
{
return(false);
}
if (Round(a.OffsetX, decimals) != Round(b.OffsetX, decimals))
{
return(false);
}
if (Round(a.OffsetY, decimals) != Round(b.OffsetY, decimals))
{
return(false);
}
if (Round(a.OffsetZ, decimals) != Round(b.OffsetZ, decimals))
{
return(false);
}
if (Round(a.M44, decimals) != Round(b.M44, decimals))
{
return(false);
}
return(true);
}
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 Methods
//
//------------------------------------------------------
#region Private Methods
//
// Processes a ray-triangle intersection to see if it's a valid hit. Unnecessary faces
// have already been culled by the ray-triange intersection routines.
//
// Shares some code with ValidateLineHit
//
private void ValidateRayHit(
RayHitTestParameters rayParams,
ref Point3D origin,
ref Vector3D direction,
double hitTime,
int i0,
int i1,
int i2,
ref Point barycentric
)
{
if (hitTime > 0)
{
Matrix3D worldTransformMatrix = rayParams.HasWorldTransformMatrix ? rayParams.WorldTransformMatrix : Matrix3D.Identity;
Point3D pointHit = origin + hitTime * direction;
Point3D worldPointHit = pointHit;
worldTransformMatrix.MultiplyPoint(ref worldPointHit);
// If we have a HitTestProjectionMatrix than this hit test originated
// at a Viewport3DVisual.
if (rayParams.HasHitTestProjectionMatrix)
{
// To test if we are in front of the far clipping plane what we
// do conceptually is project our hit point in world space into
// homogenous space and verify that it is on the correct side of
// the Z=1 plane.
//
// To save some cycles we only bother computing Z and W of the
// projected point and use a simple Z/W > 1 test to see if we
// are past the far plane.
//
// NOTE: HitTestProjectionMatrix is not just the camera matrices.
// It has an additional translation to move the ray to the
// origin. This extra translation does not effect this test.
Matrix3D m = rayParams.HitTestProjectionMatrix;
// We directly substitute 1 for p.W below:
double pz = worldPointHit.X * m.M13 + worldPointHit.Y * m.M23 + worldPointHit.Z * m.M33 + m.OffsetZ;
double pw = worldPointHit.X * m.M14 + worldPointHit.Y * m.M24 + worldPointHit.Z * m.M34 + m.M44;
// Early exit if pz/pw > 1. The negated logic is to reject NaNs.
if (!(pz / pw <= 1))
{
return;
}
Debug.Assert(!double.IsInfinity(pz / pw) && !double.IsNaN(pz / pw),
"Expected near/far tests to cull -Inf/+Inf and NaN.");
}
double dist = (worldPointHit - rayParams.Origin).Length;
Debug.Assert(dist > 0, "Distance is negative: " + dist);
if (rayParams.HasModelTransformMatrix)
{
rayParams.ModelTransformMatrix.MultiplyPoint(ref pointHit);
}
rayParams.ReportResult(this, pointHit, dist, i0, i1, i2, barycentric);
}
}
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);
}
//
// Processes a ray-line intersection to see if it's a valid hit.
//
// Shares some code with ValidateRayHit
//
private void ValidateLineHit(
RayHitTestParameters rayParams,
FaceType facesToHit,
int i0,
int i1,
int i2,
ref Point3D v0,
ref Point3D v1,
ref Point3D v2,
ref Point barycentric
)
{
Matrix3D worldTransformMatrix = rayParams.HasWorldTransformMatrix ? rayParams.WorldTransformMatrix : Matrix3D.Identity;
// OK, we have an intersection with the LINE but that could be wrong on three
// accounts:
// 1. We could have hit the line on the wrong side of the ray's origin.
// 2. We may need to cull the intersection if it's beyond the far clipping
// plane (only if the hit test originated from a Viewport3DVisual.)
// 3. We could have hit a back-facing triangle
// We will transform the hit point back into world space to check these
// things & compute the correct distance from the origin to the hit point.
// Hit point in model space
Point3D pointHit = M3DUtil.Interpolate(ref v0, ref v1, ref v2, ref barycentric);
Point3D worldPointHit = pointHit;
worldTransformMatrix.MultiplyPoint(ref worldPointHit);
// Vector from origin to hit point
Vector3D hitVector = worldPointHit - rayParams.Origin;
Vector3D originalDirection = rayParams.Direction;
double rayDistanceUnnormalized = Vector3D.DotProduct(originalDirection, hitVector);
if (rayDistanceUnnormalized > 0)
{
// If we have a HitTestProjectionMatrix than this hit test originated
// at a Viewport3DVisual.
if (rayParams.HasHitTestProjectionMatrix)
{
// To test if we are in front of the far clipping plane what we
// do conceptually is project our hit point in world space into
// homogenous space and verify that it is on the correct side of
// the Z=1 plane.
//
// To save some cycles we only bother computing Z and W of the
// projected point and use a simple Z/W > 1 test to see if we
// are past the far plane.
//
// NOTE: HitTestProjectionMatrix is not just the camera matrices.
// It has an additional translation to move the ray to the
// origin. This extra translation does not effect this test.
Matrix3D m = rayParams.HitTestProjectionMatrix;
// We directly substitute 1 for p.W below:
double pz = worldPointHit.X * m.M13 + worldPointHit.Y * m.M23 + worldPointHit.Z * m.M33 + m.OffsetZ;
double pw = worldPointHit.X * m.M14 + worldPointHit.Y * m.M24 + worldPointHit.Z * m.M34 + m.M44;
// Early exit if pz/pw > 1. The negated logic is to reject NaNs.
if (!(pz / pw <= 1))
{
return;
}
Debug.Assert(!double.IsInfinity(pz / pw) && !double.IsNaN(pz / pw),
"Expected near/far tests to cull -Inf/+Inf and NaN.");
}
Point3D a = v0, b = v1, c = v2;
worldTransformMatrix.MultiplyPoint(ref a);
worldTransformMatrix.MultiplyPoint(ref b);
worldTransformMatrix.MultiplyPoint(ref c);
Vector3D normal = Vector3D.CrossProduct(b - a, c - a);
double cullSign = -Vector3D.DotProduct(normal, hitVector);
double det = worldTransformMatrix.Determinant;
bool frontFace = (cullSign > 0) == (det >= 0);
if (((facesToHit & FaceType.Front) == FaceType.Front && frontFace) || ((facesToHit & FaceType.Back) == FaceType.Back && !frontFace))
{
double dist = hitVector.Length;
if (rayParams.HasModelTransformMatrix)
{
rayParams.ModelTransformMatrix.MultiplyPoint(ref pointHit);
}
rayParams.ReportResult(this, pointHit, dist, i0, i1, i2, barycentric);
}
}
}
public static System.Windows.Media.Media3D.Vector3D Multiply(System.Windows.Media.Media3D.Vector3D vector, Matrix3D matrix)
{
return(default(System.Windows.Media.Media3D.Vector3D));
}
internal override void Append(ref Matrix3D matrix)
{
matrix = matrix * Value;
}
internal abstract void Append(ref Matrix3D matrix);
internal GeneralTransform3DTo2D(Matrix3D projectionTransform, GeneralTransform transformBetween2D)
{
_projectionTransform = projectionTransform;
_transformBetween2D = (GeneralTransform)transformBetween2D.GetAsFrozen();
}
