public LineVisual3D(Point3D start, Point3D end, SolidColorBrush brush)
{
Start = start;
End = end;
_visualObject.Stroke = brush;
_visualObject.StrokeThickness = 2;
}
public PerspectiveCamera(Point3D position,
Vector3D lookDirection,
Vector3D upDirection,
double fieldOfView, Viewport3D viewport)
{
this.Position = position;
this.LookDirection = lookDirection;
this.UpDirection = upDirection;
this.FieldOfView = fieldOfView;
this.Viewport = viewport;
}
public override Matrix3D GetMatrix(Point3D center)
{
Quaternion q = this.Quaternion;
double n1 = 2 * q.Y * q.Y;
double n2 = 2 * q.Z * q.Z;
double n3 = 2 * q.X * q.X;
double n4 = 2 * q.X * q.Y;
double n5 = 2 * q.W * q.Z;
double n6 = 2 * q.X * q.Z;
double n7 = 2 * q.W * q.Y;
double n8 = 2 * q.Y * q.Z;
double n9 = 2 * q.W * q.X;
Matrix3D result = Matrix3D.Identity;
result.M11 = 1 - n1 - n2;
result.M12 = n4 + n5;
result.M13 = n6 - n7;
result.M21 = n4 - n5;
result.M22 = 1 - n3 - n2;
result.M23 = n8 + n9;
result.M31 = n6 + n7;
result.M32 = n8 - n9;
result.M33 = 1 - n3 - n1;
result.M44 = 1;
//If this is not Center=(0,0,0) then have to take that into consideration
if (center != null) {
if ((center.X != 0) || (center.Y != 0) || (center.Z != 0)) {
result.OffsetX = (((-center.X * result.M11) -
(center.Y * result.M21)) -
(center.Z * result.M31)) + center.X;
result.OffsetY = (((-center.X * result.M12) -
(center.Y * result.M22)) -
(center.Z * result.M32)) + center.Y;
result.OffsetZ = (((-center.X * result.M13) -
(center.Y * result.M23)) -
(center.Z * result.M33)) + center.Z;
}
}
return result;
}
public override Matrix3D GetMatrix(Point3D center)
{
//Calculations from Essential Mathematics for games & interactive applications
//James M. Van Verth & Lars M. Bishop
Vector3D a = NormalizedAxis;
//TODO: Internally use a Quaternion since it is more efficient to calculate
//TODO: Look into caching all this
double angle = AngleRad;
double t = 1.0 - System.Math.Cos(angle);
double c = System.Math.Cos(angle);
double s = System.Math.Sin(angle);
double x = a.X;
double y = a.Y;
double z = a.Z;
//TODO: Optimize, do not multiple matrices, just expand out terms
//Matrix3D m = new Matrix3D(1, 0, 0, 0,
// 0, 1, 0, 0,
// 0, 0, 1, 0,
// -center.X, -center.Y, -center.Z, 1);
//m.Append(
Matrix3D m = new Matrix3D(t * x * x + c, t * x * y + s * z, t * x * z - s * y, 0,
t * x * y - s * z, t * y * y + c, t * y * z + s * x, 0,
t * x * z + s * y, t * y * z - s * x, t * z * z + c, 0,
0, 0, 0, 1);
//m.Append(new Matrix3D(1, 0, 0, 0,
// 0, 1, 0, 0,
// 0, 0, 1, 0,
// center.X, center.Y, center.Z, 1));
return m;
}
public void TransformTo(Point3D p, Point3D result)
{
if (!this.isNotDefaultMatrix) {
result.X = p.X;
result.Y = p.Y;
result.Z = p.Z;
return;
}
result.X = p.X * this.m11 + p.Y * this.m21 + p.Z * this.m31 + this.offsetX;
result.Y = p.X * this.m12 + p.Y * this.m22 + p.Z * this.m32 + this.offsetY;
result.Z = p.X * this.m13 + p.Y * this.m23 + p.Z * this.m33 + this.offsetZ;
if (!this.IsAffine) {
double d = 1.0 / (p.X * this.m14 + p.Y * this.m24 + p.Z * this.m34 + this.m44);
result.X *= d;
result.Y *= d;
result.Z *= d;
}
}
/// <summary>
/// Multiplies the point by the matrix i.e. p * m
/// </summary>
/// <param name="p"></param>
/// <returns></returns>
public Point3D Transform(Point3D p)
{
if (!this.isNotDefaultMatrix) {
return p;
}
Point3D newPoint = new Point3D(0,0,0);
TransformTo(p, newPoint);
return newPoint;
}
public void ScaleAt(Vector3D scale, Point3D center)
{
if (!this.isNotDefaultMatrix) {
//Simple case
this.m11 = scale.X;
this.m22 = scale.Y;
this.m33 = scale.Z;
this.m44 = 1.0;
this.offsetX = center.X - (center.X * scale.X);
this.offsetY = center.Y - (center.Y * scale.Y);
this.offsetZ = center.Z - (center.Z * scale.Z);
this.isNotDefaultMatrix = true;
}
else {
//Need to multiple the current matrix with the scaling matrix
this.m11 = this.m11 * scale.X + this.m14 * center.X - this.m14 * scale.X * center.X;
this.m12 = this.m12 * scale.Y + this.m14 * center.Y - this.m14 * scale.Y * center.Y;
this.m13 = this.m13 * scale.Z + this.m14 * center.Z - this.m14 * scale.Z * center.Z;
this.m21 = this.m21 * scale.X + this.m24 * center.X - this.m24 * scale.X * center.X;
this.m22 = this.m22 * scale.Y + this.m24 * center.Y - this.m24 * scale.Y * center.Y;
this.m23 = this.m23 * scale.Z + this.m24 * center.Z - this.m24 * scale.Z * center.Z;
this.m31 = this.m31 * scale.X + this.m34 * center.X - this.m34 * scale.X * center.X;
this.m32 = this.m32 * scale.Y + this.m34 * center.Y - this.m34 * scale.Y * center.Y;
this.m33 = this.m33 * scale.Z + this.m34 * center.Z - this.m34 * scale.Z * center.Z;
this.offsetX = this.offsetX * scale.X + this.m44 * center.X - this.m44 * scale.X * center.X;
this.offsetY = this.offsetY * scale.Y + this.m44 * center.Y - this.m44 * scale.Y * center.Y;
this.offsetZ = this.offsetZ * scale.Z + this.m44 * center.Z - this.m44 * scale.Z * center.Z;
}
}
/// <summary>
/// Transforms a 3D point using the matrix and returns only the 2d component of it
/// Usable for projections on 2-d space
/// </summary>
/// <param name="p"></param>
/// <returns></returns>
public Point Project(Point3D p)
{
Point newPoint = new Point(p.X * this.m11 + p.Y * this.m21 + p.Z * this.m31 + this.offsetX,
p.X * this.m12 + p.Y * this.m22 + p.Z * this.m32 + this.offsetY);
if (!this.IsAffine) {
double d = 1.0 / (p.X * this.m14 + p.Y * this.m24 + p.Z * this.m34 + this.m44);
newPoint.X *= d;
newPoint.Y *= d;
}
return newPoint;
}
internal Point3D(Point3D p)
{
this.X = p.X;
this.Y = p.Y;
this.Z = p.Z;
}
public bool Equals(Point3D p)
{
return Equals(this, p);
}
public static Point3D Subtract(Vector3D v, Point3D p)
{
return new Point3D(v.X - p.X, v.Y - p.Y, v.Z - p.Z);
}
public static Vector3D Subtract(Point3D p1, Point3D p2)
{
return new Vector3D(p1.X - p2.X, p1.Y - p2.Y, p1.Z - p2.Z);
}
public static bool Equals(Point3D p1, Point3D p2)
{
if (((object)p1 == null) && ((object) p2 == null)) {
return true;
}
if (((object)p1 == null) || ((object) p2 == null)) {
return false;
}
return p1.X.Equals(p2.X) && p1.Y.Equals(p2.Y) && p1.Z.Equals(p2.Z);
}
public static Point3D Add(Point3D point, Vector3D v)
{
return new Point3D(point.X + v.X, point.Y + v.Y, point.Z + v.Z);
}
public void RotateTo(Point3D vector, Point3D result)
{
double a = this.W;
double b = this.X;
double c = this.Y;
double d = this.Z;
double v1 = vector.X;
double v2 = vector.Y;
double v3 = vector.Z;
double t2 = a * b;
double t3 = a * c;
double t4 = a * d;
double t5 = -b * b;
double t6 = b * c;
double t7 = b * d;
double t8 = -c * c;
double t9 = c * d;
double t10 = -d * d;
result.X = 2 * ((t8 + t10) * v1 + (t6 - t4) * v2 + (t3 + t7) * v3) + v1;
result.Y = 2 * ((t4 + t6) * v1 + (t5 + t10) * v2 + (t9 - t2) * v3) + v2;
result.Z = 2 * ((t7 - t3) * v1 + (t2 + t9) * v2 + (t5 + t8) * v3) + v3;
}
public static Point3D Subtract(Point3D p, Vector3D v)
{
return new Point3D(p.X - v.X, p.Y - v.Y, p.Z - v.Z);
}
public abstract Matrix3D GetMatrix(Point3D center);
public static Point3D Add(Vector3D v, Point3D p)
{
return new Point3D(v.X + p.X, v.Y + p.Y, v.Z + p.Z);
}
