public FixTrans2(
Fix m11, Fix m12, Fix m13,
Fix m21, Fix m22, Fix m23
)
{
_m11 = m11; _m12 = m12; _m13 = m13;
_m21 = m21; _m22 = m22; _m23 = m23;
}
public FixTrans2(FixVec2 position, FixVec2 scale, Fix rotation)
{
Fix cos = FixMath.Cos(rotation);
Fix sin = FixMath.Sin(rotation);
_m11 = cos * scale.X; _m12 = -sin * scale.X; _m13 = position.X;
_m21 = sin * scale.Y; _m22 = cos * scale.Y; _m23 = position.Y;
}
public FixTrans3(
Fix m11, Fix m12, Fix m13, Fix m14,
Fix m21, Fix m22, Fix m23, Fix m24,
Fix m31, Fix m32, Fix m33, Fix m34
)
{
_m11 = m11; _m12 = m12; _m13 = m13; _m14 = m14;
_m21 = m11; _m22 = m12; _m23 = m13; _m24 = m14;
_m31 = m11; _m32 = m12; _m33 = m13; _m34 = m14;
}
static FixMath()
{
if (_quarterSineResPower >= Fix.FRACTIONAL_BITS)
throw new Exception("_quarterSineResPower must be less than Fix.FractionalBits.");
if (_quarterSineConsts.Length != 90 * (1 << _quarterSineResPower) + 1)
throw new Exception("_quarterSineConst.Length must be 90 * 2^(_quarterSineResPower) + 1.");
PI = _piConst;
E = _eConst;
_log2_E = _log2_EConst;
_log2_10 = _log2_10Const;
_ln2 = _ln2Const;
_log10_2 = _log10_2Const;
_quarterSine = Array.ConvertAll(_quarterSineConsts, c => (Fix)c);
_cordicAngles = Array.ConvertAll(_cordicAngleConsts, c => (Fix)c);
_cordicGains = Array.ConvertAll(_cordicGainConsts, c => (Fix)c);
}
public static Fix Max(Fix v1, Fix v2)
{
return(v1 > v2 ? v1 : v2);
}
public static Fix Log10(Fix value)
{
return Log2(value) * _log10_2;
}
FixVec3 ScalarAdd(Fix value)
{
return(new FixVec3(_x + value, _y + value, _z + value));
}
public FixTrans3 RotateX(Fix degrees)
{
return(MakeRotationX(degrees) * this);
}
public static Fix Log10(Fix value)
{
return(Log2(value) * _log10_2);
}
public static Fix Atan(Fix value)
{
return(Atan2(value, 1));
}
public static Fix Truncate(Fix value)
{
if (value < 0)
return new Fix((value.Raw + Fix.FRACTION_RANGE) & Fix.INTEGER_MASK);
else
return new Fix(value.Raw & Fix.INTEGER_MASK);
}
public static Fix Tan(Fix degrees)
{
return Sin(degrees) / Cos(degrees);
}
public static Fix Sqrt(Fix value)
{
if (value.Raw < 0)
throw new ArgumentOutOfRangeException("value", "Value must be non-negative.");
if (value.Raw == 0)
return 0;
return new Fix((int)(SqrtULong((ulong)value.Raw << (Fix.FRACTIONAL_BITS + 2)) + 1) >> 1);
}
public static Fix Sin(Fix degrees)
{
return CosRaw(degrees.Raw - (90 << Fix.FRACTIONAL_BITS));
}
public static Fix Sign(Fix value)
{
if (value < 0)
return -1;
else if (value > 0)
return 1;
else
return 0;
}
public static Fix Round(Fix value)
{
return new Fix((value.Raw + (Fix.FRACTION_RANGE >> 1)) & ~Fix.FRACTION_MASK);
}
public static Fix Pow(Fix b, Fix exp)
{
if (b == 1 || exp == 0)
return 1;
int intPow;
Fix intFactor;
if ((exp.Raw & Fix.FRACTION_MASK) == 0) {
intPow = (int)((exp.Raw + (Fix.FRACTION_RANGE >> 1)) >> Fix.FRACTIONAL_BITS);
Fix t;
int p;
if (intPow < 0) {
t = 1 / b;
p = -intPow;
} else {
t = b;
p = intPow;
}
intFactor = 1;
while (p > 0) {
if ((p & 1) != 0)
intFactor *= t;
t *= t;
p >>= 1;
}
return intFactor;
}
exp *= Log(b, 2);
b = 2;
intPow = (int)((exp.Raw + (Fix.FRACTION_RANGE >> 1)) >> Fix.FRACTIONAL_BITS);
intFactor = intPow < 0 ? Fix.One >> -intPow : Fix.One << intPow;
long x = (
((exp.Raw - (intPow << Fix.FRACTIONAL_BITS)) * _ln2Const.Raw)
+ (Fix.FRACTION_RANGE >> 1)
) >> Fix.FRACTIONAL_BITS;
if (x == 0)
return intFactor;
long fracFactor = x;
long xa = x;
for (int i = 2; i < _invFactConsts.Length; i++) {
if (xa == 0)
break;
xa *= x;
xa += (1L << (32 - 1));
xa >>= 32;
long p = xa * _invFactConsts[i].Raw;
p += (1L << (32 - 1));
p >>= 32;
fracFactor += p;
}
return new Fix((int)((((long)intFactor.Raw * fracFactor + (1L << (32 - 1))) >> 32) + intFactor.Raw));
}
public static Fix Cos(Fix degrees)
{
return(CosRaw(degrees.raw));
}
public static Fix Asin(Fix value)
{
return(Atan2(value, Sqrt((1 + value) * (1 - value))));
}
static Fix Log2(Fix value)
{
if (value <= 0)
throw new ArgumentOutOfRangeException("value", "Value must be positive.");
uint x = (uint)value.Raw;
uint b = 1U << (Fix.FRACTIONAL_BITS - 1);
uint y = 0;
while (x < 1U << Fix.FRACTIONAL_BITS) {
x <<= 1;
y -= 1U << Fix.FRACTIONAL_BITS;
}
while (x >= 2U << Fix.FRACTIONAL_BITS) {
x >>= 1;
y += 1U << Fix.FRACTIONAL_BITS;
}
ulong z = x;
for (int i = 0; i < Fix.FRACTIONAL_BITS; i++) {
z = z * z >> Fix.FRACTIONAL_BITS;
if (z >= 2U << Fix.FRACTIONAL_BITS) {
z >>= 1;
y += b;
}
b >>= 1;
}
return new Fix((int)y);
}
public static Fix Pow(Fix b, Fix exp)
{
if (b == 1 || exp == 0)
{
return(1);
}
int intPow;
Fix intFactor;
if ((exp.raw & Fix.FRACTION_MASK) == 0)
{
intPow = (int)((exp.raw + (Fix.FRACTION_RANGE >> 1)) >> Fix.FRACTIONAL_BITS);
Fix t;
int p;
if (intPow < 0)
{
t = 1 / b;
p = -intPow;
}
else
{
t = b;
p = intPow;
}
intFactor = 1;
while (p > 0)
{
if ((p & 1) != 0)
{
intFactor *= t;
}
t *= t;
p >>= 1;
}
return(intFactor);
}
exp *= Log(b, 2);
b = 2;
intPow = (int)((exp.raw + (Fix.FRACTION_RANGE >> 1)) >> Fix.FRACTIONAL_BITS);
intFactor = intPow < 0 ? Fix.one >> -intPow : Fix.one << intPow;
long x = (
((exp.raw - (intPow << Fix.FRACTIONAL_BITS)) * _ln2Const.raw)
+ (Fix.FRACTION_RANGE >> 1)
) >> Fix.FRACTIONAL_BITS;
if (x == 0)
{
return(intFactor);
}
long fracFactor = x;
long xa = x;
for (int i = 2; i < _invFactConsts.Length; i++)
{
if (xa == 0)
{
break;
}
xa *= x;
xa += (1L << (32 - 1));
xa >>= 32;
long p = xa * _invFactConsts[i].raw;
p += (1L << (32 - 1));
p >>= 32;
fracFactor += p;
}
return(new Fix((int)((((long)intFactor.raw * fracFactor + (1L << (32 - 1))) >> 32) + intFactor.raw)));
}
public static Fix Abs(Fix value)
{
return value.Raw < 0 ? new Fix(-value.Raw) : value;
}
public static Fix Min(Fix v1, Fix v2)
{
return v1 < v2 ? v1 : v2;
}
public static Fix Acos(Fix value)
{
return Atan2(Sqrt((1 + value) * (1 - value)), value);
}
FixVec3 ScalarAdd(Fix value)
{
return(new FixVec3(x + value, y + value, z + value));
}
public static Fix Asin(Fix value)
{
return Atan2(value, Sqrt((1 + value) * (1 - value)));
}
public static FixTrans3 MakeRotationZ(Fix degrees)
{
Fix cos = FixMath.Cos(degrees);
Fix sin = FixMath.Sin(degrees);
return new FixTrans3(
cos, -sin, 0, 0,
sin, cos, 0, 0,
0, 0, 1, 0
);
}
public static Fix Atan(Fix value)
{
return Atan2(value, 1);
}
FixVec2 ScalarMultiply(Fix value)
{
return new FixVec2(_x * value, _y * value);
}
public static Fix Atan2(Fix y, Fix x)
{
if (x == 0 && y == 0)
throw new ArgumentOutOfRangeException("y and x cannot both be 0.");
Fix angle = 0;
Fix xNew, yNew;
if (x < 0) {
if (y < 0) {
xNew = -y;
yNew = x;
angle = -90;
} else if (y > 0) {
xNew = y;
yNew = -x;
angle = 90;
} else {
xNew = x;
yNew = y;
angle = 180;
}
x = xNew;
y = yNew;
}
for (int i = 0; i < Fix.FRACTIONAL_BITS + 2; i++) {
if (y > 0) {
xNew = x + (y >> i);
yNew = y - (x >> i);
angle += _cordicAngles[i];
} else if (y < 0) {
xNew = x - (y >> i);
yNew = y + (x >> i);
angle -= _cordicAngles[i];
} else
break;
x = xNew;
y = yNew;
}
return angle;
}
public static Fix Min(Fix v1, Fix v2)
{
return(v1 < v2 ? v1 : v2);
}
FixVec3 ScalarAdd(Fix value)
{
return new FixVec3(_x + value, _y + value, _z + value);
}
public static Fix Sin(Fix degrees)
{
return(CosRaw(degrees.raw - (90 << Fix.FRACTIONAL_BITS)));
}
FixVec3 ScalarMultiply(Fix value)
{
return new FixVec3(_x * value, _y * value, _z * value);
}
public static Fix Tan(Fix degrees)
{
return(Sin(degrees) / Cos(degrees));
}
public FixVec3(Fix x, Fix y, Fix z)
{
_x = x;
_y = y;
_z = z;
}
public static Fix Acos(Fix value)
{
return(Atan2(Sqrt((1 + value) * (1 - value)), value));
}
public FixTrans2 Rotate(Fix degrees)
{
return MakeRotation(degrees) * this;
}
public static Fix Exp(Fix value)
{
return(Pow(E, value));
}
public static Fix DegreesToRadians(Fix d)
{
return(d * FixMath.PI / 180);
}
public static Fix Log(Fix value)
{
return(Log2(value) * _ln2);
}
public static Fix Ceiling(Fix value)
{
return(new Fix((value.raw + Fix.FRACTION_MASK) & Fix.INTEGER_MASK));
}
public static Fix RadianToDegrees(Fix r)
{
return(r * 180 / FixMath.PI);
}
public static Fix Max(Fix v1, Fix v2)
{
return v1 > v2 ? v1 : v2;
}
public static Fix Abs(Fix value)
{
return(value.raw < 0 ? new Fix(-value.raw) : value);
}
public static void Multiply(ref FixQuaternion quaternion, ref Fix scale, out FixQuaternion result)
{
result = new FixQuaternion(quaternion.X * scale, quaternion.Y * scale, quaternion.Z * scale, quaternion.W * scale);
}
public static Fix Floor(Fix value)
{
return(new Fix(value.raw & Fix.INTEGER_MASK));
}
/// <summary>
/// Multiplies an instance by a scalar.
/// </summary>
/// <param name="quaternion">The instance.</param>
/// <param name="scale">The scalar.</param>
/// <returns>A new instance containing the result of the calculation.</returns>
public static FixQuaternion Multiply(FixQuaternion quaternion, Fix scale)
{
return(new FixQuaternion(quaternion.X * scale, quaternion.Y * scale, quaternion.Z * scale, quaternion.W * scale));
}
public FixVec3(Fix x, Fix y, Fix z)
{
this.x = x;
this.y = y;
this.z = z;
}
/// <summary>
/// Scale the given quaternion to unit length
/// </summary>
/// <param name="q">The quaternion to normalize</param>
/// <param name="result">The normalized quaternion</param>
public static void Normalize(ref FixQuaternion q, out FixQuaternion result)
{
Fix scale = Fix.One / q.Length;
result = new FixQuaternion(q.Xyz * scale, q.W * scale);
}
FixVec3 ScalarMultiply(Fix value)
{
return(new FixVec3(x * value, y * value, z * value));
}
/// <summary>
/// Construct a new Quaternion from vector and w components
/// </summary>
/// <param name="v">The vector part</param>
/// <param name="w">The w part</param>
public FixQuaternion(FixVec3 v, Fix w)
{
this.xyz = v;
this.w = w;
}
FixVec3 ScalarMultiply(Fix value)
{
return(new FixVec3(_x * value, _y * value, _z * value));
}
/// <summary>
/// Do Spherical linear interpolation between two quaternions
/// </summary>
/// <param name="q1">The first quaternion</param>
/// <param name="q2">The second quaternion</param>
/// <param name="blend">The blend factor</param>
/// <returns>A smooth blend between the given quaternions</returns>
public static FixQuaternion Slerp(FixQuaternion q1, FixQuaternion q2, Fix blend)
{
// if either input is zero, return the other.
if (q1.LengthSquared == Fix.Zero)
{
if (q2.LengthSquared == Fix.Zero)
{
return(Identity);
}
return(q2);
}
else if (q2.LengthSquared == Fix.Zero)
{
return(q1);
}
Fix cosHalfAngle = q1.W * q2.W + q1.Xyz.Dot(q2.Xyz);
if (cosHalfAngle >= Fix.One || cosHalfAngle <= -Fix.One)
{
// angle = 0.0f, so just return one input.
return(q1);
}
else if (cosHalfAngle < Fix.Zero)
{
q2.Xyz = -q2.Xyz;
q2.W = -q2.W;
cosHalfAngle = -cosHalfAngle;
}
Fix blendA;
Fix blendB;
if (cosHalfAngle < (Fix.One / 100) * 99)
{
// do proper slerp for big angles
Fix halfAngle = FixMath.Acos(cosHalfAngle);
Fix sinHalfAngle = FixMath.Sin(halfAngle);
Fix oneOverSinHalfAngle = Fix.One / sinHalfAngle;
blendA = FixMath.Sin(halfAngle * (Fix.One - blend)) * oneOverSinHalfAngle;
blendB = FixMath.Sin(halfAngle * blend) * oneOverSinHalfAngle;
}
else
{
// do lerp if angle is really small.
blendA = Fix.One - blend;
blendB = blend;
}
FixQuaternion result = new FixQuaternion(blendA * q1.Xyz + blendB * q2.Xyz, blendA * q1.W + blendB * q2.W);
if (result.LengthSquared > Fix.Zero)
{
return(Normalize(result));
}
else
{
return(Identity);
}
}
public FixTrans3 RotateZ(Fix degrees)
{
return MakeRotationZ(degrees) * this;
}
/// <summary>
/// Construct a new Quaternion
/// </summary>
/// <param name="x">The x component</param>
/// <param name="y">The y component</param>
/// <param name="z">The z component</param>
/// <param name="w">The w component</param>
public FixQuaternion(Fix x, Fix y, Fix z, Fix w)
: this(new FixVec3(x, y, z), w)
{
}
FixVec2 ScalarAdd(Fix value)
{
return new FixVec2(_x + value, _y + value);
}
public static Fix Round(Fix value)
{
return(new Fix((value.raw + (Fix.FRACTION_RANGE >> 1)) & ~Fix.FRACTION_MASK));
}
public FixVec2(Fix x, Fix y)
{
_x = x;
_y = y;
}
public static Fix Log(Fix value, Fix b)
{
if (b == 2)
return Log2(value);
else if (b == E)
return Log(value);
else if (b == 10)
return Log10(value);
else
return Log2(value) / Log2(b);
}
