public static Fix32 RoundFast(this Fix32 x)
{
// https://sestevenson.wordpress.com/2009/08/19/rounding-in-fixed-point-number-conversions/
int odd = ((int)x & ONE) >> FRACTIONAL_BITS;
return((Fix32)(((int)x + (ONE / 2 - 1) + odd) & INTEGER_MASK));
}
public static Fix32 RcpFast(this Fix32 x)
{
#if USE_DOUBLES
return((1 / x.ToDouble()).ToFix32());
#endif
return((Fix32)Fixed32.RcpFast((int)x));
}
public static Fix32 Pow2Fast(this Fix32 xx)
{
#if USE_DOUBLES
return(Math.Pow(2, x.ToDouble()).ToFix32());
#endif
return((Fix32)Fixed32.Pow(2, (int)xx));
}
public static Fix32 TanFastest(this Fix32 x)
{
#if USE_DOUBLES
return(Math.Tan(x.ToDouble()).ToFix32());
#endif
return((Fix32)Fixed32.TanFastest((int)x));
}
public static Fix32 RSqrtFastest(this Fix32 x)
{
#if USE_DOUBLES
return((1 / Math.Sqrt(x.ToDouble())).ToFix32());
#endif
return((Fix32)Fixed32.RSqrtFastest((int)x));
}
public static Fix32 Ln(this Fix32 x)
{
#if USE_DOUBLES
return(Math.Log(x.ToDouble()).ToFix32());
#endif
return(Log2(x).Mul(Fix32.Ln2));
}
public void T005_Sub()
{
PrepareStatistics(out var deltas, out var swF, out var swD);
var terms1 = new Fix32[] { Fix32.MinValue, Fix32.MinusOne, Fix32.Zero, Fix32.One, Fix32.MaxValue };
var terms2 = new Fix32[] { Fix32.One, Fix32.MinusTwo, (1.5).ToFix32(), Fix32.Two, Fix32.MinusOne };
var expecteds = new Fix32[] { Fix32.MinValue, Fix32.One, (-1.5).ToFix32(), Fix32.MinusOne, Fix32.MaxValue };
for (int i = 0; i < terms1.Length; ++i)
{
var actual = terms1[i].Sub(terms2[i]);
var expected = expecteds[i];
Assert.AreEqual(expected, actual, terms1[i].ToStringExt() + " - " + terms2[i].ToStringExt());
}
for (int i = 0; i < TestCases.Count; ++i)
{
for (int j = 0; j < TestCases.Count; ++j)
{
var x = (Fix32)TestCases[i];
var y = (Fix32)TestCases[j];
swD.Start();
double expected = Saturate(x.ToDouble() - y.ToDouble());
swD.Stop();
swF.Start();
double actual = x.Sub(y).ToDouble();
swF.Stop();
Assert.AreEqual(expected, actual, x.ToStringExt() + " - " + y.ToStringExt());
}
}
PrintStatistics(deltas, swF, swD);
}
public void T011_AbsFast()
{
Assert.AreEqual(Fix32.MinValue, Fix32.MinValue.AbsFast()); // Wrong result, but it is spected
var sources = new Fix32[] { Fix32.MaxValue.Neg(), Fix32.MinusOne, Fix32.Zero, Fix32.One, Fix32.MaxValue.Sub(Fix32.One), Fix32.MaxValue };
var expecteds = new Fix32[] { Fix32.MaxValue, Fix32.One, Fix32.Zero, Fix32.One, Fix32.MaxValue.Sub(Fix32.One), Fix32.MaxValue };
for (int i = 0; i < sources.Length; ++i)
{
var actual = sources[i].AbsFast();
var expected = expecteds[i];
Assert.AreEqual(expected, actual, sources[i].ToStringExt());
}
for (int i = 0; i < TestCases.Count; ++i)
{
var actual = ((Fix32)TestCases[i]).AbsFast();
var expected = (Fix32)TestCases[i];
if ((int)expected != (int)Fix32.MinValue)
{
expected = Math.Abs(((Fix32)TestCases[i]).ToDouble()).ToFix32();
}
Assert.AreEqual(expected, actual, TestCases[i].ToFix32().ToStringExt());
}
}
/// <summary>
/// Reads a TWAIN value.
/// </summary>
/// <param name="baseAddress">The base address.</param>
/// <param name="offset">The offset.</param>
/// <param name="type">The TWAIN type.</param>
/// <returns></returns>
public static object ReadValue(IntPtr baseAddress, ref int offset, TwainType type)
{
object val = null;
switch (type)
{
case TwainType.Int8:
val = (sbyte)Marshal.ReadByte(baseAddress, offset);
break;
case TwainType.UInt8:
val = Marshal.ReadByte(baseAddress, offset);
break;
case TwainType.Bool:
case TwainType.UInt16:
val = (ushort)Marshal.ReadInt16(baseAddress, offset);
break;
case TwainType.Int16:
val = Marshal.ReadInt16(baseAddress, offset);
break;
case TwainType.UInt32:
val = (uint)Marshal.ReadInt32(baseAddress, offset);
break;
case TwainType.Int32:
val = Marshal.ReadInt32(baseAddress, offset);
break;
case TwainType.Fix32:
Fix32 f32 = new Fix32();
f32.Whole = Marshal.ReadInt16(baseAddress, offset);
f32.Frac = (ushort)Marshal.ReadInt16(baseAddress, offset + 2);
val = f32;
break;
case TwainType.Frame:
Frame frame = new Frame();
frame.Left = (Fix32)ReadValue(baseAddress, ref offset, TwainType.Fix32);
frame.Top = (Fix32)ReadValue(baseAddress, ref offset, TwainType.Fix32);
frame.Right = (Fix32)ReadValue(baseAddress, ref offset, TwainType.Fix32);
frame.Bottom = (Fix32)ReadValue(baseAddress, ref offset, TwainType.Fix32);
return(frame); // no need to update offset again after reading fix32
case TwainType.Str128:
case TwainType.Str255:
case TwainType.Str32:
case TwainType.Str64:
val = Marshal.PtrToStringAnsi(new IntPtr(baseAddress.ToInt64() + offset));
break;
/* case TwainType.Handle:
* val = Marshal.ReadIntPtr(baseAddress, offset);
* break;*/
}
offset += GetItemTypeSize(type);
return(val);
}
/// <summary>
/// Returns a specified number raised to the specified power. Saturates
/// </summary>
/// <exception cref="DivideByZeroException">
/// The base was zero, with a negative exponent
/// </exception>
/// <exception cref="ArgumentOutOfRangeException">
/// The base was negative, with a non-zero exponent
/// </exception>
public static Fix32 Pow(this Fix32 b, Fix32 exp)
{
#if USE_DOUBLES
return(Math.Pow(b.ToDouble(), exp.ToDouble()).ToFix32());
#endif
if ((int)b == (int)Fix32.One)
{
return(Fix32.One);
}
if ((int)exp == 0)
{
return(Fix32.One);
}
if ((int)b == 0)
{
if ((int)exp < 0)
{
throw new DivideByZeroException();
}
return(Fix32.Zero);
}
if (b < 0 && exp != 0)
{
throw new ArgumentOutOfRangeException("Non-positive value passed to Ln", "x");
}
return(exp.Mul(b.Log2()).Pow2());
}
public static Fix32 Sin(this Fix32 x)
{
#if USE_DOUBLES
return((Fix64)Math.Sin((double)x));
#endif
return((Fix32)Fixed32.Sin((int)x));
}
/// <summary>
/// Returns 2 raised to the specified power.
/// </summary>
public static Fix32 Pow2(this Fix32 xx)
{
#if USE_DOUBLES
return(Math.Pow(2, x.ToDouble()).ToFix32());
#endif
Fix32 x = xx;
if ((int)x == 0)
{
return(Fix32.One);
}
// Avoid negative arguments by exploiting that exp(-x) = 1/exp(x).
bool neg = (int)x < 0;
if (neg)
{
x = x.Neg();
}
if ((int)x == (int)Fix32.One)
{
return(neg ? Fix32.One.Div(Fix32.Two) : Fix32.Two); // Can be cached
}
if ((int)x >= (int)Fix32.Log2Max)
{
return(neg ? Fix32.One.Div(Fix32.MaxValue) : Fix32.MaxValue); // Can be cached
}
if ((int)x <= (int)Fix32.Log2Min)
{
return(neg ? Fix32.MaxValue : Fix32.Zero);
}
/*
* The algorithm is based on the power series for exp(x):
* http://en.wikipedia.org/wiki/Exponential_function#Formal_definition
*
* From term n, we get term n+1 by multiplying with x/n.
* When the sum term drops to zero, we can stop summing.
*/
int integerPart = x.Floor().ToInt();
// Take fractional part of exponent
x = (Fix32)((uint)x & FRACTIONAL_MASK);
Fix32 result = Fix32.One;
Fix32 term = Fix32.One;
Fix32 i = Fix32.One;
while ((int)term != 0)
{
term = x.Mul(term).Mul(Fix32.Ln2.Div(i));
result = result.Add(term);
i = i.AddFast(Fix32.One);
}
result = (Fix32)((int)result << integerPart);
if (neg)
{
result = Fix32.One.Div(result);
}
return(result);
}
public static Fix32 Exp(this Fix32 x)
{
#if USE_DOUBLES
return(Math.Exp(x.ToDouble()).ToFix32());
#endif
return((Fix32)Fixed32.Exp((int)x));
}
public static Fix32 Floor(this Fix32 x)
{
#if USE_DOUBLES
return(Math.Floor(x.ToDouble()).ToFix32());
#endif
// Just zero out the fractional part
return((Fix32)((int)x & INTEGER_MASK));
}
public static Fix32 Neg(this Fix32 x)
{
#if USE_DOUBLES
return((-x.ToDouble()).ToFix32());
#endif
//return new Fix64(-x.RawValue);
return((Fix32)((int)x == MIN_VALUE ? MAX_VALUE : -(int)x));
}
public static Fix32 Sub(this Fix32 x, Fix32 y)
{
#if USE_DOUBLES
return((x.ToDouble() - y.ToDouble()).ToFix32());
#endif
long sub = (long)x - (long)y; // TO TEST: Shift and operate to check overflow
return((Fix32)(((int)sub) != sub ? (int)((((uint)x >> NUM_BITS_MINUS_ONE) - 1U) ^ (1U << NUM_BITS_MINUS_ONE)) : (int)sub));
}
public static Fix32 Ceiling(this Fix32 x)
{
#if USE_DOUBLES
return(Math.Floor(x.ToDouble()).ToFix32());
#endif
var hasFractionalPart = ((int)x & FRACTIONAL_MASK) != 0;
return(hasFractionalPart ? ((Fix32)((int)x & INTEGER_MASK)).Add(Fix32.One) : x);
}
public void IntegrateForces(DBody body, Fix32 delta)
{
//Debug.Log("1body.Velocity:" + body.Velocity.ToVector3());
body.Velocity += (DWorld.GRAVITY + body.Force * body.InvMass) * delta;
// Debug.Log("2body.Velocity:" + body.Velocity.ToVector3());
//Debug.Log("delta:" + (float)delta);
//body.Velocity *= (Fix32.One - body.Drag);
}
public static Fix32 Cos(this Fix32 x)
{
#if USE_DOUBLES
return(Math.Cos(x.ToDouble()).ToFix32());
#endif
// Don't use Fixed32.Cos, it gives an error
var rawAngle = (int)x + (x > 0 ? -PI - PI_OVER_2 : PI_OVER_2);
return(((Fix32)rawAngle).Sin());
}
/// <summary>
/// Creates a new collision manifold between the given objects.
/// </summary>
/// <param name="a">first rigid body</param>
/// <param name="b">second rigid body</param>
/// <param name="normal">collision normal</param>
/// <param name="distance">penetration</param>
public Manifold(DBody a, DBody b, Vector3F normal, Fix32 penetration)
{
this.bodyA = a;
this.bodyB = b;
this.normal = normal;
this.penetration = penetration;
this.trigger = false;
GenerateHash();
}
/// <summary>
/// Main physics loop, find collisions, resolve them and move the bodies.
/// </summary>
/// <param name="delta"> amount of time for this simulation step</param>
public void Step(Fix32 delta)
{
if (!simulate)
{
return;
}
contacts.Clear();
Fix32 invDelta = (delta > Fix32.Zero) ? (Fix32)1 / delta : Fix32.Zero;
//integrate forces
Profiler.BeginSample("Integrate forces");
foreach (DBody body in bodies)
{
if (body.IsFixed())
{
continue;
}
integrator.IntegrateForces(body, delta);
}
Profiler.EndSample();
Profiler.BeginSample("Find collisions");
detector.FindCollisions(contacts);
Profiler.EndSample();
//init collision manifolds
foreach (Manifold contact in contacts)
{
contact.Init(invDelta);
}
//resolve collisions
for (uint i = 0; i < ITERATIONS; i++)
{
foreach (Manifold contact in contacts)
{
contact.ApplyImpulse();
}
}
//integrate velocities
foreach (DBody body in bodies)
{
if (body.IsFixed())
{
continue;
}
Profiler.BeginSample("Remove-Integrate-Insert");
detector.Remove(body);
integrator.IntegrateVelocities(body, delta);
detector.Insert(body);
Profiler.EndSample();
}
}
/// <summary>
/// Creates a new circle collider with the given position and radius.
/// </summary>
/// <param name="position">the center of the circle</param>
/// <param name="radius">the radius of the circle</param>
public DCircleCollider(Vector2F position, Fix32 radius, bool isTrigger) : base(ColliderType.Circle, isTrigger)
{
this.center = position;
this.radius = radius;
Vector2F min = center - Vector2F.One * radius;
Vector2F max = center + Vector2F.One * radius;
boundingBox = new DBoxCollider(min, max, isTrigger);
}
/// <summary>
/// Creates a new circle collider with the given position and radius.
/// </summary>
/// <param name="position">the center of the circle</param>
/// <param name="radius">the radius of the circle</param>
public DSphereCollider(Vector3F position, Fix32 radius, bool isTrigger, bool isDebug)
: base(ColliderType.Sphere, isTrigger, isDebug)
{
this.center = position;
this.radius = radius;
Vector3F scale = new Vector3F(radius * (Fix32)2, radius * (Fix32)2, radius * (Fix32)2);
Vector3F euler = new Vector3F(0, 0, 0);
boundingBox = new DBox3DCollider(center, scale, euler, isTrigger, isDebug);
}
public static Fix32 Mod(this Fix32 x, Fix32 y)
{
#if USE_DOUBLES
return((x.ToDouble() % y.ToDouble()).ToFix32());
#endif
return((Fix32)(
(int)x == MIN_VALUE & (int)y == -1 ?
0 :
(int)x % (int)y));
}
/// <summary>
/// Divide. Overflows.
/// </summary>
public static Fix32 DivFast(this Fix32 x, Fix32 y)
{
if ((int)y == 0)
{
return((Fix32)(unchecked ((int)(((((uint)x) >> NUM_BITS_MINUS_ONE) - 1U) ^ (1U << NUM_BITS_MINUS_ONE)))));
return(x >= 0 ? Fix32.MaxValue : Fix32.MinValue); // Branched version of the previous code, for clarity. Slower
}
return((Fix32)(int)(((long)x << FRACTIONAL_BITS) / (int)y));
}
/// <summary>
/// Creates a new rigid body with the given parameters.
/// </summary>
/// <param name="collider">the collider for this object</param>
/// <param name="position">the current position in the space</param>
/// <param name="mass">the object's mass</param>
/// <param name="restitution">the "bounciness"</param>
/// <param name="drag">amount of friction</param>
public DBody(DCollider collider, Vector2F position, Fix32 mass, Fix32 restitution, Fix32 drag)
{
this.collider = collider;
this.position = position;
this.prevPosition = position;
this.mass = mass;
this.invMass = (mass > Fix32.Zero) ? ((Fix32)1 / mass) : (Fix32)0;
this.restitution = restitution;
this.drag = drag;
this.collider.Body = this;
}
/// <summary>
/// Returns the base-2 logarithm of a specified number.
/// </summary>
/// <exception cref="ArgumentOutOfRangeException">
/// The argument was non-positive
/// </exception>
public static Fix32 Log2(this Fix32 x)
{
#if USE_DOUBLES
return(Math.Log(x.ToDouble(), 2).ToFix32());
#endif
if ((int)x <= 0)
{
throw new ArgumentOutOfRangeException("Non-positive value passed to Ln", "x");
}
//return (Fix32) Fixed32.Log2((int) x);
// This implementation is based on Clay. S. Turner's fast binary logarithm
// algorithm (C. S. Turner, "A Fast Binary Logarithm Algorithm", IEEE Signal
// Processing Mag., pp. 124,140, Sep. 2010.)
//https://github.com/dmoulding/log2fix/blob/master/log2fix.c
const int EXTRA_SHIFT = 8;
long xx = (long)x << EXTRA_SHIFT;
const int PRECISION = FRACTIONAL_BITS + EXTRA_SHIFT;
long b = 1 << (PRECISION - 1);
long y = 0;
long rawX = (long)xx;
while (rawX < 1 << PRECISION)
{
rawX <<= 1;
y -= 1 << PRECISION;
}
while (rawX >= 2 << PRECISION)
{
rawX >>= 1;
y += 1 << PRECISION;
}
ulong z = (ulong)rawX;
for (int i = 0; i < PRECISION; i++)
{
z = z * z >> PRECISION;
if (z >= 2 << PRECISION)
{
z >>= 1;
y += b;
}
b >>= 1;
}
return((Fix32)(int)(y >> EXTRA_SHIFT));
}
public static Fix32 Mul(this Fix32 x, Fix32 y)
{
#if USE_DOUBLES
return(Math.Round(x.ToDouble() * y.ToDouble()).ToFix32());
#endif
long multLong = ((long)x * (long)y) >> FRACTIONAL_BITS;
int finalSign = (int)x ^ (int)y;
return((Fix32)(((finalSign ^ multLong) & SIGN_MASK) != 0 && multLong != 0 ?
(int)((((uint)finalSign >> NUM_BITS_MINUS_ONE) - 1U) ^ (1U << NUM_BITS_MINUS_ONE)) :
(int)multLong));
}
public static Fix32 Atan2(this Fix32 y, Fix32 x)
{
#if USE_DOUBLES
return(Math.Atan2(y.ToDouble(), x.ToDouble()).ToFix32());
#endif
var yl = (int)y;
var xl = (int)x;
if (xl == 0)
{
if (yl > 0)
{
return(Fix32.PiOver2);
}
if (yl == 0)
{
return(Fix32.Zero);
}
return(Fix32.PiOver2.Neg());
}
Fix32 atan;
var z = y.Div(x);
// Deal with overflow
if ((int)Fix32.One.Add(C0p28.Mul(z).Mul(z)) == (int)Fix32.MaxValue)
{
return((int)y < 0 ? Fix32.PiOver2.Neg() : Fix32.PiOver2);
}
if ((int)Abs(z) < (int)Fix32.One)
{
atan = z.Div(Fix32.One.Add(C0p28.Mul(z).Mul(z)));
if (xl < 0)
{
if (yl < 0)
{
return(atan.Sub(Fix32.Pi));
}
return(atan.Add(Fix32.Pi));
}
}
else
{
atan = Fix32.PiOver2.Sub(z.Div(z.Mul(z).Add(C0p28)));
if (yl < 0)
{
return(atan.Sub(Fix32.Pi));
}
}
return(atan);
}
public static Fix32 Abs(this Fix32 x)
{
#if USE_DOUBLES
return(Math.Abs(x.ToDouble()).ToFix32());
#endif
if ((int)x == MIN_VALUE)
{
return(Fix32.MaxValue);
}
// branchless implementation, see http://www.strchr.com/optimized_abs_function
var mask = (int)x >> NUM_BITS_MINUS_ONE;
return((Fix32)(((int)x + mask) ^ mask));
}
