internal static Matrix3F CreateRotationMatrix(ref QuaternionF quaternion, ref Point3F center)
{
Matrix3F matrix = s_identity;
matrix.IsDistinguishedIdentity = false; // Will be using direct member access
Float wx, wy, wz, xx, yy, yz, xy, xz, zz, x2, y2, z2;
x2 = quaternion.X + quaternion.X;
y2 = quaternion.Y + quaternion.Y;
z2 = quaternion.Z + quaternion.Z;
xx = quaternion.X * x2;
xy = quaternion.X * y2;
xz = quaternion.X * z2;
yy = quaternion.Y * y2;
yz = quaternion.Y * z2;
zz = quaternion.Z * z2;
wx = quaternion.W * x2;
wy = quaternion.W * y2;
wz = quaternion.W * z2;
matrix._m11 = (Float)1.0 - (yy + zz);
matrix._m12 = xy + wz;
matrix._m13 = xz - wy;
matrix._m21 = xy - wz;
matrix._m22 = (Float)1.0 - (xx + zz);
matrix._m23 = yz + wx;
matrix._m31 = xz + wy;
matrix._m32 = yz - wx;
matrix._m33 = (Float)1.0 - (xx + yy);
if (center.X != 0 || center.Y != 0 || center.Z != 0)
{
matrix._offsetX = -center.X * matrix._m11 - center.Y * matrix._m21 - center.Z * matrix._m31 + center.X;
matrix._offsetY = -center.X * matrix._m12 - center.Y * matrix._m22 - center.Z * matrix._m32 + center.Y;
matrix._offsetZ = -center.X * matrix._m13 - center.Y * matrix._m23 - center.Z * matrix._m33 + center.Z;
}
return(matrix);
}
public void ScaleAt(Vector3F scale, Point3F center)
{
if (IsDistinguishedIdentity)
{
SetScaleMatrix(ref scale, ref center);
}
else
{
Float tmp = _m14 * center.X;
_m11 = tmp + scale.X * (_m11 - tmp);
tmp = _m14 * center.Y;
_m12 = tmp + scale.Y * (_m12 - tmp);
tmp = _m14 * center.Z;
_m13 = tmp + scale.Z * (_m13 - tmp);
tmp = _m24 * center.X;
_m21 = tmp + scale.X * (_m21 - tmp);
tmp = _m24 * center.Y;
_m22 = tmp + scale.Y * (_m22 - tmp);
tmp = _m24 * center.Z;
_m23 = tmp + scale.Z * (_m23 - tmp);
tmp = _m34 * center.X;
_m31 = tmp + scale.X * (_m31 - tmp);
tmp = _m34 * center.Y;
_m32 = tmp + scale.Y * (_m32 - tmp);
tmp = _m34 * center.Z;
_m33 = tmp + scale.Z * (_m33 - tmp);
tmp = _m44 * center.X;
_offsetX = tmp + scale.X * (_offsetX - tmp);
tmp = _m44 * center.Y;
_offsetY = tmp + scale.Y * (_offsetY - tmp);
tmp = _m44 * center.Z;
_offsetZ = tmp + scale.Z * (_offsetZ - tmp);
}
}
internal void MultiplyPoint(ref Point3F point)
{
if (IsDistinguishedIdentity)
{
return;
}
Float x = point.X;
Float y = point.Y;
Float z = point.Z;
point.X = x * _m11 + y * _m21 + z * _m31 + _offsetX;
point.Y = x * _m12 + y * _m22 + z * _m32 + _offsetY;
point.Z = x * _m13 + y * _m23 + z * _m33 + _offsetZ;
if (!IsAffine)
{
Float w = x * _m14 + y * _m24 + z * _m34 + _m44;
point.X /= w;
point.Y /= w;
point.Z /= w;
}
}
public void ScaleAtPrepend(Vector3F scale, Point3F center)
{
if (IsDistinguishedIdentity)
{
SetScaleMatrix(ref scale, ref center);
}
else
{
Float csx = center.X - center.X * scale.X;
Float csy = center.Y - center.Y * scale.Y;
Float csz = center.Z - center.Z * scale.Z;
// We have to set the bottom row first because it depends
// on values that will change
_offsetX += _m11 * csx + _m21 * csy + _m31 * csz;
_offsetY += _m12 * csx + _m22 * csy + _m32 * csz;
_offsetZ += _m13 * csx + _m23 * csy + _m33 * csz;
_m44 += _m14 * csx + _m24 * csy + _m34 * csz;
_m11 *= scale.X; _m12 *= scale.X; _m13 *= scale.X; _m14 *= scale.X;
_m21 *= scale.Y; _m22 *= scale.Y; _m23 *= scale.Y; _m24 *= scale.Y;
_m31 *= scale.Z; _m32 *= scale.Z; _m33 *= scale.Z; _m34 *= scale.Z;
}
}
public static Point3F Multiply(Point3F point, Matrix3F matrix)
{
return(matrix.Transform(point));
}
internal static void Subtract(ref Point3F p1, ref Point3F p2, out Vector3F result)
{
result._x = p1._x - p2._x;
result._y = p1._y - p2._y;
result._z = p1._z - p2._z;
}
public static Point3F Subtract(Point3F point, Vector3F vector)
{
return(new Point3F(point._x - vector._x,
point._y - vector._y,
point._z - vector._z));
}
public static Point3F Add(Point3F point, Vector3F vector)
{
return(new Point3F(point._x + vector._x,
point._y + vector._y,
point._z + vector._z));
}
public void RotateAt(QuaternionF quaternion, Point3F center)
{
this *= CreateRotationMatrix(ref quaternion, ref center);
}
public static Point3F Add(Vector3F vector, Point3F point)
{
return(new Point3F(vector._x + point._x,
vector._y + point._y,
vector._z + point._z));
}
public static Rect3F Union(Rect3F rect, Point3F point)
{
rect.Union(new Rect3F(point, point));
return(rect);
}
public Point3F Transform(Point3F point)
{
MultiplyPoint(ref point);
return(point);
}
public void RotateAtPrepend(QuaternionF quaternion, Point3F center)
{
this = CreateRotationMatrix(ref quaternion, ref center) * this;
}
public bool Contains(Point3F point)
{
return(Contains(point._x, point._y, point._z));
}
public void Rotate(QuaternionF quaternion)
{
Point3F center = new Point3F();
this *= CreateRotationMatrix(ref quaternion, ref center);
}
public void Union(Point3F point)
{
Union(new Rect3F(point, point));
}
public static bool Equals(Point3F point1, Point3F point2)
{
return(point1.X.Equals(point2.X) &&
point1.Y.Equals(point2.Y) &&
point1.Z.Equals(point2.Z));
}
internal Rect3F(Point3F point, Vector3F vector) : this(point, point + vector)
{
}
public bool Equals(Point3F value)
{
return(Point3F.Equals(this, value));
}
public static Point3F Subtract(Vector3F vector, Point3F point)
{
return(new Point3F(vector._x - point._x,
vector._y - point._y,
vector._z - point._z));
}
public void RotatePrepend(QuaternionF quaternion)
{
Point3F center = new Point3F();
this = CreateRotationMatrix(ref quaternion, ref center) * this;
}
