public Matrix33(Quat q)
{
var v2 = q.V + q.V;
var xx = 1 - v2.X * q.V.X;
var yy = v2.Y * q.V.Y;
var xw = v2.X * q.W;
var xy = v2.Y * q.V.X;
var yz = v2.Z * q.V.Y;
var yw = v2.Y * q.W;
var xz = v2.Z * q.V.X;
var zz = v2.Z * q.V.Z;
var zw = v2.Z * q.W;
M00 = 1 - yy - zz;
M01 = xy - zw;
M02 = xz + yw;
M10 = xy + zw;
M11 = xx - zz;
M12 = yz - xw;
M20 = xz - yw;
M21 = yz + xw;
M22 = xx - yy;
}
public Vec3(Quat q)
{
Y = (float)Math.Asin(Math.Max(-1.0f, Math.Min(1.0f, -(q.V.X * q.V.Z - q.W * q.V.Y) * 2)));
if (Math.Abs(Math.Abs(Y) - (Math.PI * 0.5f)) < 0.01f)
{
X = 0;
Z = (float)Math.Atan2(-2 * (q.V.X * q.V.Y - q.W * q.V.Z), 1 - (q.V.X * q.V.X + q.V.Z * q.V.Z) * 2);
}
else
{
X = (float)Math.Atan2((q.V.Y * q.V.Z + q.W * q.V.X) * 2, 1 - (q.V.X * q.V.X + q.V.Y * q.V.Y) * 2);
Z = (float)Math.Atan2((q.V.X * q.V.Y + q.W * q.V.Z) * 2, 1 - (q.V.Z * q.V.Z + q.V.Y * q.V.Y) * 2);
}
}
public bool IsEquivalent(Quat q, float epsilon = 0.05f)
{
var p = -q;
bool t0 = (Math.Abs(V.X - q.V.X) <= epsilon) && (Math.Abs(V.Y - q.V.Y) <= epsilon) && (Math.Abs(V.Z - q.V.Z) <= epsilon) && (Math.Abs(W - q.W) <= epsilon);
bool t1 = (Math.Abs(V.X - p.V.X) <= epsilon) && (Math.Abs(V.Y - p.V.Y) <= epsilon) && (Math.Abs(V.Z - p.V.Z) <= epsilon) && (Math.Abs(W - p.W) <= epsilon);
t0 |= t1;
return t0;
}
public void ExpSlerp(Quat start, Quat end, float amount)
{
var q = end;
if ((start | q) < 0) { q = -q; }
this = start * MathHelpers.Exp(MathHelpers.Log(!start * q) * amount);
}
/// <summary>
/// Links to another entity, becoming the parent. Any change to the parent object is propagated to all child (linked) objects.
/// </summary>
/// <param name="linkName">Name of the link</param>
/// <param name="otherEntityId">Id of the entity we wish to be linked to</param>
/// <param name="relativeRot"></param>
/// <param name="relativePos"></param>
/// <returns>true if successful, otherwise false.</returns>
public bool Link(string linkName, EntityId otherEntityId, Quat relativeRot, Vec3 relativePos)
{
return NativeEntityMethods.AddEntityLink(this.GetEntityHandle(), linkName, otherEntityId, relativeRot, relativePos);
}
public QuatT(Vec3 t, Quat q)
{
Q = q;
T = t;
}
public void Set(Vec3 s, Quat q, Vec3 t = default(Vec3))
{
float vxvx = q.V.X * q.V.X; float vzvz = q.V.Z * q.V.Z; float vyvy = q.V.Y * q.V.Y;
float vxvy = q.V.X * q.V.Y; float vxvz = q.V.X * q.V.Z; float vyvz = q.V.Y * q.V.Z;
float svx = q.W * q.V.X; float svy = q.W * q.V.Y; float svz = q.W * q.V.Z;
M00 = (1 - (vyvy + vzvz) * 2) * s.X; M01 = (vxvy - svz) * 2 * s.Y; M02 = (vxvz + svy) * 2 * s.Z; M03 = t.X;
M10 = (vxvy + svz) * 2 * s.X; M11 = (1 - (vxvx + vzvz) * 2) * s.Y; M12 = (vyvz - svx) * 2 * s.Z; M13 = t.Y;
M20 = (vxvz - svy) * 2 * s.X; M21 = (vyvz + svx) * 2 * s.Y; M22 = (1 - (vxvx + vyvy) * 2) * s.Z; M23 = t.Z;
}
public void Slerp(Quat start, Quat end, float amount)
{
var p = start;
var q = end;
var q2 = new Quat();
var cosine = (p | q);
if (cosine < 0.0f) { cosine = -cosine; q = -q; } // take shortest arc
if (cosine > 0.9999f)
{
Nlerp(p, q, amount);
return;
}
// from now on, a division by 0 is not possible any more
q2.W = q.W - p.W * cosine;
q2.V.X = q.V.X - p.V.X * cosine;
q2.V.Y = q.V.Y - p.V.Y * cosine;
q2.V.Z = q.V.Z - p.V.Z * cosine;
var sine = Math.Sqrt(q2 | q2);
double s, c;
MathHelpers.SinCos(Math.Atan2(sine, cosine) * amount, out s, out c);
W = (float)(p.W * c + q2.W * s / sine);
V.X = (float)(p.V.X * c + q2.V.X * s / sine);
V.Y = (float)(p.V.Y * c + q2.V.Y * s / sine);
V.Z = (float)(p.V.Z * c + q2.V.Z * s / sine);
}
public static Quat CreateRotationAA(float rad, Vec3 axis)
{
var q = new Quat();
q.SetRotationAA(rad, axis);
return q;
}
public QuatT(Matrix34 m)
{
Q = new Quat(m);
T = m.Translation;
}
public static EntityLink Create(EntityBase parent, EntityBase slave, string linkName, Vec3? relativePos = null, Quat? relativeRot = null)
{
return new EntityLink(NativeEntityMethods.AddEntityLink(parent.GetIEntity(), linkName, slave.Id, relativeRot ?? Quat.Identity, relativePos ?? Vec3.Zero), parent);
}
public QuatT(Vec3 t, Quat q)
{
Q = q;
T = t;
}
public void SaveLast()
{
if (ViewIDLast != 0xff)
{
PositionLast = Position;
RotationLast = Rotation;
FOVLast = FieldOfView;
}
else
ViewIDLast = 0xfe;
}
public void Nlerp(Quat start, Quat end, float amount)
{
var q = end;
if ((start | q) < 0) { q = -q; }
var vDiff = q.V - start.V;
V = start.V + (vDiff * amount);
W = start.W + ((q.W - start.W) * amount);
Normalize();
}
public static Quat CreateRotationAA(float cosha, float sinha, Vec3 axis)
{
var q = new Quat();
q.SetRotationAA(cosha, sinha, axis);
return q;
}
public void Nlerp2(Quat start, Quat end, float amount)
{
var q = end;
var cosine = (start | q);
if (cosine < 0) q = -q;
var k = (1 - Math.Abs(cosine)) * 0.4669269f;
var s = 2 * k * amount * amount * amount - 3 * k * amount * amount + (1 + k) * amount;
V.X = start.V.X * (1.0f - s) + q.V.X * s;
V.Y = start.V.Y * (1.0f - s) + q.V.Y * s;
V.Z = start.V.Z * (1.0f - s) + q.V.Z * s;
W = start.W * (1.0f - s) + q.W * s;
Normalize();
}
public static Quat CreateRotationVDir(Vec3 vDir, float r)
{
var q = new Quat();
q.SetRotationVDir(vDir, r);
return q;
}
public Matrix34(Vec3 s, Quat q, Vec3 t = default(Vec3))
: this()
{
Set(s, q, t);
}
public static Quat CreateRotationXYZ(Vec3 angle)
{
var q = new Quat();
q.SetRotationXYZ(angle);
return q;
}
public static Matrix34 Create(Vec3 s, Quat q, Vec3 t = default(Vec3))
{
var matrix = new Matrix34();
matrix.Set(s, q, t);
return matrix;
}
public static Quat CreateRotationZ(float r)
{
var q = new Quat();
q.SetRotationZ(r);
return q;
}
public void Invert()
{
T = -T * Q;
Q = !Q;
}
public static Quat CreateSlerp(Quat start, Quat end, float amount)
{
var q = new Quat();
q.Slerp(start, end, amount);
return q;
}
public QuatT(Matrix34 m)
{
Q = new Quat(m);
T = m.GetTranslation();
}
public Matrix34(Vec3 s, Quat q, Vec3 t = default(Vec3))
: this()
{
Set(s, q, t);
}
