public Vector3P Transform(Vector3P xyz)
{
var vQ = new Quaternion(xyz.X, xyz.Y, xyz.Z, 0);
var vQnew = this * vQ * Inverse;
return(new Vector3P(vQnew.x, vQnew.y, vQnew.z));
}
public static Vector3P CrossProduct(Vector3P a, Vector3P b)
{
var x = a.Y * b.Z - a.Z * b.Y;
var y = a.Z * b.X - a.X * b.Z;
var z = a.X * b.Y - a.Y * b.X;
return(new Vector3P(x, y, z));
}
private Matrix3P(Vector3P aside, Vector3P up, Vector3P dir)
{
columns = new Vector3P[3]
{
aside,
up,
dir
};
}
public void SetTilda(Vector3P a)
{
Aside.Y = -a.Z;
Aside.Z = a.Y;
Up.X = a.Z;
Up.Z = -a.X;
Dir.X = -a.Y;
Dir.Y = a.X;
}
public static TransformP Read(BinaryReaderEx input)
{
var quaternion = QuaternionP.ReadCompressed(input);
var x = new ShortFloat(input.ReadUInt16());
var y = new ShortFloat(input.ReadUInt16());
var z = new ShortFloat(input.ReadUInt16());
var vector = new Vector3P((float)x.DoubleValue, (float)y.DoubleValue, (float)z.DoubleValue);
return(new TransformP(quaternion, vector));
}
public override bool Equals(object obj)
{
Vector3P p = obj as Vector3P;
if (p == null)
{
return(false);
}
return(base.Equals(obj) && Equals(p));
}
public float Distance(Vector3P v)
{
var d = this - v;
return((float)System.Math.Sqrt(d.X * d.X + d.Y * d.Y + d.Z * d.Z));
}
public bool Equals(Vector3P other)
{
Func <float, float, bool> nearlyEqual = (f1, f2) => System.Math.Abs(f1 - f2) < 0.05;
return(nearlyEqual(X, other.X) && nearlyEqual(Y, other.Y) && nearlyEqual(Z, other.Z));
}
public TransformP(QuaternionP quaternion, Vector3P vector)
{
this.quaternion = quaternion;
this.vector = vector;
}
public Vector3P Transform(Vector3P xyz)
{
return(new Vector3P(System.Numerics.Vector3.Transform(xyz.Vector3, quaternion)));
}
private Matrix4P(Matrix3P orientation, Vector3P position)
{
Orientation = orientation;
Position = position;
}
public static Vector3P CrossProduct(Vector3P a, Vector3P b)
{
return(new Vector3P(Vector3.Cross(a.xyz, b.xyz)));
}
public float Distance(Vector3P v)
{
return(Vector3.Distance(xyz, v.xyz));
}