public void GeneralShape()
{
var s = new CylinderShape(1, 3);
float m0;
Vector3 com0;
Matrix i0;
MassHelper.GetMass(s, new Vector3(1, -2, -3), 0.7f, true, 0.001f, 4, out m0, out com0, out i0);
var m = s.GetMesh(0.001f, 10);
var s2 = new TriangleMeshShape(m);
float m1;
Vector3 com1;
Matrix i1;
MassHelper.GetMass(s2, new Vector3(1, -2, -3), 0.7f, true, 0.001f, 4, out m1, out com1, out i1);
const float e = 0.01f;
Assert.IsTrue(Numeric.AreEqual(m0, m1, e * (1 + m0)));
Assert.IsTrue(Vector3.AreNumericallyEqual(com0, com1, e * (1 + com0.Length)));
Assert.IsTrue(Matrix.AreNumericallyEqual(i0, i1, e * (1 + i0.Trace)));
// Try with target mass.
float m2;
Vector3 com2;
Matrix i2;
MassHelper.GetMass(s2, new Vector3(1, -2, -3), 23, false, 0.001f, 4, out m2, out com2, out i2);
Assert.IsTrue(Numeric.AreEqual(23, m2, e * (1 + m0)));
Assert.IsTrue(Vector3.AreNumericallyEqual(com0, com2, e * (1 + com0.Length)));
Assert.IsTrue(Matrix.AreNumericallyEqual(i0 * 23 / m0, i2, e * (1 + i0.Trace)));
}
protected override void OnProcess(RenderContext context)
{
var graphicsDevice = GraphicsService.GraphicsDevice;
if (TextureHelper.IsFloatingPointFormat(context.SourceTexture.Format))
{
graphicsDevice.SamplerStates[0] = SamplerState.PointClamp;
}
else
{
graphicsDevice.SamplerStates[0] = SamplerState.LinearClamp;
}
graphicsDevice.SetRenderTarget(context.RenderTarget);
graphicsDevice.Viewport = context.Viewport;
_viewportSizeParameter.SetValue(new Vector2(graphicsDevice.Viewport.Width, graphicsDevice.Viewport.Height));
_sourceTextureParameter.SetValue(context.SourceTexture);
if (Numeric.AreEqual(Strength, 1))
{
_fullSepiaPass.Apply();
}
else
{
_strengthParameter.SetValue(Strength);
_partialSepiaPass.Apply();
}
graphicsDevice.DrawFullScreenQuad();
_sourceTextureParameter.SetValue((Texture2D)null);
}
private void SetSliderPosition(double scale)
{
if (Numeric.IsGreater(scale, ScaleSmall))
{
// When scale > 1.0 then update the slider.
if (Numeric.IsLessOrEqual(scale, ScaleSmall))
{
SliderPosition = SliderPositionSmall;
}
else if (Numeric.IsLess(scale, ScaleMedium))
{
SliderPosition = (scale - 1.25) / 0.25 * 4 + 116;
}
else if (Numeric.AreEqual(scale, ScaleMedium))
{
SliderPosition = SliderPositionMedium;
}
else if (Numeric.IsLess(scale, ScaleLarge))
{
SliderPosition = (scale - 3.5) / 0.5 * 4 + 155;
}
else if (Numeric.AreEqual(scale, ScaleLarge))
{
SliderPosition = SliderPositionLarge;
}
else if (Numeric.IsLess(scale, ScaleExtraLarge))
{
SliderPosition = (scale - 7) * 4 + 184;
}
else
{
SliderPosition = SliderPositionExtraLarge;
}
}
}
/// <summary>
/// Integrates the specified function within the given interval.
/// </summary>
/// <param name="function">The function.</param>
/// <param name="lowerBound">The lower bound.</param>
/// <param name="upperBound">The upper bound.</param>
/// <returns>
/// The integral of the given function over the interval
/// [<paramref name="lowerBound"/>, <paramref name="upperBound"/>].
/// </returns>
/// <exception cref="ArgumentNullException">
/// <paramref name="function"/> is <see langword="null"/>.
/// </exception>
public override float Integrate(Func <float, float> function, float lowerBound, float upperBound)
{
NumberOfIterations = 0;
if (function == null)
{
throw new ArgumentNullException("function");
}
// see Numerical Recipes p. 144
float integral = 0;
float oldTrapezoidalIntegral = 0;
float oldIntegral = 0;
for (int i = 0; i < MaxNumberOfIterations; i++)
{
NumberOfIterations++;
float trapezoidalIntegral = TrapezoidalIntegratorF.Integrate(function, lowerBound, upperBound, oldTrapezoidalIntegral, i + 1);
integral = (4.0f * trapezoidalIntegral - oldTrapezoidalIntegral) / 3.0f;
if (NumberOfIterations >= MinNumberOfIterations) // Avoid spurious early convergence
{
if (Numeric.AreEqual(integral, oldIntegral, Epsilon))
{
return(integral);
}
}
oldIntegral = integral;
oldTrapezoidalIntegral = trapezoidalIntegral;
}
return(integral);
}
private void TestColorConversion(byte r, byte g, byte b, double hHsv, double sHsv, double vHsv, double hHsl, double sHsl, double lHsl)
{
sHsv *= 100;
vHsv *= 100;
sHsl *= 100;
lHsl *= 100;
double tolerance = 1.1; // 1.1 / 255 = ~4% tolerance
Color c = ColorHelper.FromHsv(hHsv, sHsv, vHsv);
Assert.IsTrue(Numeric.AreEqual(r, c.R, tolerance));
Assert.IsTrue(Numeric.AreEqual(g, c.G, tolerance));
Assert.IsTrue(Numeric.AreEqual(b, c.B, tolerance));
Assert.AreEqual(255, c.A);
c = ColorHelper.FromHsl(hHsl, sHsl, lHsl);
Assert.IsTrue(Numeric.AreEqual(r, c.R, tolerance));
Assert.IsTrue(Numeric.AreEqual(g, c.G, tolerance));
Assert.IsTrue(Numeric.AreEqual(b, c.B, tolerance));
Assert.AreEqual(255, c.A);
tolerance = 0.8; // 8% tolerance
double h, s, v, l;
Color.FromArgb(255, r, g, b).ToHsv(out h, out s, out v);
Assert.IsTrue(Numeric.AreEqual(hHsv, h, tolerance));
Assert.IsTrue(Numeric.AreEqual(sHsv, s, tolerance));
Assert.IsTrue(Numeric.AreEqual(vHsv, v, tolerance));
Color.FromArgb(255, r, g, b).ToHsl(out h, out s, out l);
Assert.IsTrue(Numeric.AreEqual(hHsl, h, tolerance));
Assert.IsTrue(Numeric.AreEqual(sHsl, s, tolerance));
Assert.IsTrue(Numeric.AreEqual(lHsl, l, tolerance));
}
public void ComputeCollisionLineOther()
{
CollisionObject line0 = new CollisionObject();
//line0.Name = "line0";
((GeometricObject)line0.GeometricObject).Shape = new LineShape(new Vector3(0, 0, 1), new Vector3(1, 0, 0));
((GeometricObject)line0.GeometricObject).Pose = new Pose(new Vector3(0, 0, 2));
CollisionObject sphere = new CollisionObject();
//sphere.Name = "sphere";
((GeometricObject)sphere.GeometricObject).Shape = new SphereShape(1);
((GeometricObject)sphere.GeometricObject).Pose = Pose.Identity;
LineAlgorithm algo = new LineAlgorithm(new CollisionDetection());
ContactSet set;
set = algo.GetClosestPoints(line0, sphere);
Assert.IsTrue(Numeric.AreEqual(-2, set[0].PenetrationDepth));
Assert.IsTrue(Vector3.AreNumericallyEqual(-Vector3.UnitZ, set[0].Normal));
Assert.IsTrue(Vector3.AreNumericallyEqual(new Vector3(0, 0, 2), set[0].Position, 0.001f));
Assert.IsTrue(Vector3.AreNumericallyEqual(new Vector3(0, 0, 1), set[0].PositionALocal, 0.001f));
Assert.AreEqual(false, algo.HaveContact(line0, sphere));
set = set.Swapped;
((GeometricObject)sphere.GeometricObject).Pose = new Pose(new Vector3(0, 0, 2.1f));
algo.UpdateContacts(set, 0);
Assert.IsTrue(Numeric.AreEqual(0.1f, set[0].PenetrationDepth, 0.001f));
Assert.IsTrue(Vector3.AreNumericallyEqual(Vector3.UnitZ, set[0].Normal, 0.1f)); // Large epsilon because MPR for spheres is not very accurate.
Assert.IsTrue(Vector3.AreNumericallyEqual(new Vector3(0, 0, 3), set[0].Position, 0.1f));
Assert.IsTrue(Vector3.AreNumericallyEqual(new Vector3(0, 0, 1), set[0].PositionALocal, 0.1f));
Assert.IsTrue(Vector3.AreNumericallyEqual(new Vector3(0, 0, 1), set[0].PositionBLocal, 0.1f));
Assert.AreEqual(true, algo.HaveContact(line0, sphere));
}
public void DotProduct()
{
// 0°
Assert.AreEqual(1.0, Vector3D.Dot(Vector3D.UnitX, Vector3D.UnitX));
Assert.AreEqual(1.0, Vector3D.Dot(Vector3D.UnitY, Vector3D.UnitY));
Assert.AreEqual(1.0, Vector3D.Dot(Vector3D.UnitZ, Vector3D.UnitZ));
// 180°
Assert.AreEqual(-1.0, Vector3D.Dot(Vector3D.UnitX, -Vector3D.UnitX));
Assert.AreEqual(-1.0, Vector3D.Dot(Vector3D.UnitY, -Vector3D.UnitY));
Assert.AreEqual(-1.0, Vector3D.Dot(Vector3D.UnitZ, -Vector3D.UnitZ));
// 90°
Assert.AreEqual(0.0, Vector3D.Dot(Vector3D.UnitX, Vector3D.UnitY));
Assert.AreEqual(0.0, Vector3D.Dot(Vector3D.UnitY, Vector3D.UnitZ));
Assert.AreEqual(0.0, Vector3D.Dot(Vector3D.UnitX, Vector3D.UnitZ));
// 45°
double angle = Math.Acos(Vector3D.Dot(new Vector3D(1, 1, 0).Normalized, Vector3D.UnitX));
Assert.IsTrue(Numeric.AreEqual(MathHelper.ToRadians(45.0), angle));
angle = Math.Acos(Vector3D.Dot(new Vector3D(0, 1, 1).Normalized, Vector3D.UnitY));
Assert.IsTrue(Numeric.AreEqual(MathHelper.ToRadians(45.0), angle));
angle = Math.Acos(Vector3D.Dot(new Vector3D(1, 0, 1).Normalized, Vector3D.UnitZ));
Assert.IsTrue(Numeric.AreEqual(MathHelper.ToRadians(45.0), angle));
}
public void GetLength()
{
CatmullRomSegment2F c = new CatmullRomSegment2F
{
Point1 = new Vector2F(1, 2),
Point2 = new Vector2F(10, 3),
Point3 = new Vector2F(7, 8),
Point4 = new Vector2F(10, 2),
};
HermiteSegment2F h = new HermiteSegment2F
{
Point1 = c.Point2,
Tangent1 = (c.Point3 - c.Point1) * 0.5f,
Tangent2 = (c.Point4 - c.Point2) * 0.5f,
Point2 = c.Point3,
};
float length1 = c.GetLength(0, 1, 20, Numeric.EpsilonF);
float length2 = h.GetLength(0, 1, 20, Numeric.EpsilonF);
Assert.IsTrue(Numeric.AreEqual(length1, length2));
float approxLength = 0;
const float step = 0.0001f;
for (float u = 0; u <= 1.0f; u += step)
{
approxLength += (c.GetPoint(u) - c.GetPoint(u + step)).Length;
}
Assert.IsTrue(Numeric.AreEqual(approxLength, length1, 0.01f));
Assert.IsTrue(Numeric.AreEqual(c.GetLength(0, 1, 100, Numeric.EpsilonF), c.GetLength(0, 0.5f, 100, Numeric.EpsilonF) + c.GetLength(0.5f, 1, 100, Numeric.EpsilonF)));
Assert.IsTrue(Numeric.AreEqual(c.GetLength(0, 1, 100, Numeric.EpsilonF), c.GetLength(1, 0, 100, Numeric.EpsilonF)));
}
private void UpdateTriangleSimplex()
{
// The simplex is a triangle.
var info = new ClosestPointInfo();
GetClosestPointInTriangle(Vector3F.Zero, _w[0], _w[1], _w[2], ref info);
ClosestPointOnA = _pointsA[0] * info.U
+ _pointsA[1] * info.V
+ _pointsA[2] * info.W;
ClosestPointOnB = _pointsB[0] * info.U
+ _pointsB[1] * info.V
+ _pointsB[2] * info.W;
ClosestPoint = info.ClosestPoint;
Reduce(ref info);
IsValid = true;
// Following assert can fail, for example, for ray convex test if objects
// are far away from the origin.
//Debug.Assert(Vector3F.AreNumericallyEqual(ClosestPoint, ClosestPointOnA - ClosestPointOnB, Math.Max(1, ClosestPoint.Length) * 0.0001f), "ClosestPoint computed from barycentric coordinates must be equal to ClosestPointOnA - ClosestPointOnB.");
Debug.Assert(Numeric.IsGreaterOrEqual(info.U, 0));
Debug.Assert(Numeric.IsGreaterOrEqual(info.V, 0));
Debug.Assert(Numeric.IsGreaterOrEqual(info.W, 0));
Debug.Assert(Numeric.AreEqual(info.X, 0));
}
public void ComputeCollision()
{
Gjk algo = new Gjk(new CollisionDetection());
CollisionObject a = new CollisionObject
{
GeometricObject = new GeometricObject(new TriangleShape(new Vector3F(0, 0, 0), new Vector3F(0, 1, 0), new Vector3F(0, 0, 1)), Pose.Identity),
};
CollisionObject b = new CollisionObject
{
GeometricObject = new GeometricObject(new SphereShape(1), Pose.Identity),
};
ContactSet set;
set = algo.GetClosestPoints(a, b);
Assert.AreEqual(true, algo.HaveContact(a, b));
Assert.AreEqual(0, set[0].PenetrationDepth);
((GeometricObject)b.GeometricObject).Pose = new Pose(new Vector3F(2, 0.1f, 0.2f));
algo.UpdateClosestPoints(set, 0);
Assert.AreEqual(false, algo.HaveContact(a, b));
Assert.IsTrue(Numeric.AreEqual(-1, set[0].PenetrationDepth, 0.001f));
Assert.IsTrue(Vector3F.AreNumericallyEqual(new Vector3F(0, 0.1f, 0.2f), set[0].PositionAWorld, 0.01f));
Assert.IsTrue(Vector3F.AreNumericallyEqual(new Vector3F(1, 0.1f, 0.2f), set[0].PositionBWorld, 0.01f));
}
public void NoiseIsPeriodicWithUserPeriod()
{
Random random = new Random(1234567);
for (int i = 0; i < 100; i++)
{
var v = random.NextVector4D(-1000, 1000);
var randomPeriod = random.NextVector4D(2, 444);
var px = (int)randomPeriod.X;
var py = (int)randomPeriod.Y;
var pz = (int)randomPeriod.Z;
var pw = (int)randomPeriod.W;
Assert.IsTrue(Numeric.AreEqual(PerlinNoise.Compute(v.X, px), PerlinNoise.Compute(v.X - px, px)));
Assert.IsTrue(Numeric.AreEqual(PerlinNoise.Compute(v.X, px), PerlinNoise.Compute(v.X + px, px)));
Assert.IsTrue(Numeric.AreEqual(PerlinNoise.Compute(v.X, v.Y, px, py), PerlinNoise.Compute(v.X - px, v.Y - py, px, py)));
Assert.IsTrue(Numeric.AreEqual(PerlinNoise.Compute(v.X, v.Y, px, py), PerlinNoise.Compute(v.X + px, v.Y + py, px, py)));
Assert.IsTrue(Numeric.AreEqual(PerlinNoise.Compute(v.X, v.Y, v.Z, px, py, pz), PerlinNoise.Compute(v.X - px, v.Y - py, v.Z - pz, px, py, pz)));
Assert.IsTrue(Numeric.AreEqual(PerlinNoise.Compute(v.X, v.Y, v.Z, px, py, pz), PerlinNoise.Compute(v.X + px, v.Y + py, v.Z + pz, px, py, pz)));
Assert.IsTrue(Numeric.AreEqual(PerlinNoise.Compute(v.X, v.Y, v.Z, v.W, px, py, pz, pw), PerlinNoise.Compute(v.X - px, v.Y - py, v.Z - pz, v.W - pw, px, py, pz, pw)));
Assert.IsTrue(Numeric.AreEqual(PerlinNoise.Compute(v.X, v.Y, v.Z, v.W, px, py, pz, pw), PerlinNoise.Compute(v.X + px, v.Y + py, v.Z + pz, v.W + pw, px, py, pz, pw)));
}
}
public void FindRootForY()
{
Func <float, float> polynomial = x => (x - 1) * (x - 10) * (x - 18);
BisectionMethodF rootFinder = new BisectionMethodF(polynomial);
rootFinder.EpsilonX = Numeric.EpsilonF / 100;
float xRoot = rootFinder.FindRoot(0, 2, 2);
Assert.IsTrue(Numeric.AreEqual(2, polynomial(xRoot)));
Console.WriteLine("NumberOfIterations: {0}", rootFinder.NumberOfIterations);
xRoot = rootFinder.FindRoot(2, 0, 2);
Assert.IsTrue(Numeric.AreEqual(2, polynomial(xRoot)));
Console.WriteLine("NumberOfIterations: {0}", rootFinder.NumberOfIterations);
xRoot = rootFinder.FindRoot(0, 7, 2);
Assert.IsTrue(Numeric.AreEqual(2, polynomial(xRoot)));
Console.WriteLine("NumberOfIterations: {0}", rootFinder.NumberOfIterations);
xRoot = rootFinder.FindRoot(7, 0, 2);
Assert.IsTrue(Numeric.AreEqual(2, polynomial(xRoot)));
Console.WriteLine("NumberOfIterations: {0}", rootFinder.NumberOfIterations);
xRoot = rootFinder.FindRoot(-10, 2, 2);
Assert.IsTrue(Numeric.AreEqual(2, polynomial(xRoot)));
Console.WriteLine("NumberOfIterations: {0}", rootFinder.NumberOfIterations);
}
public void Test()
{
GaussianFunctionF f = new GaussianFunctionF();
Assert.AreEqual(1, f.Coefficient);
Assert.AreEqual(0, f.ExpectedValue);
Assert.AreEqual(1, f.StandardDeviation);
f = new GaussianFunctionF(3, 4, 5);
Assert.AreEqual(3, f.Coefficient);
Assert.AreEqual(4, f.ExpectedValue);
Assert.AreEqual(5, f.StandardDeviation);
f = new GaussianFunctionF {
ExpectedValue = 10, StandardDeviation = 2, Coefficient = 4
};
Assert.AreEqual(4, f.Coefficient);
Assert.AreEqual(10, f.ExpectedValue);
Assert.AreEqual(2, f.StandardDeviation);
Assert.Less(f.Compute(3), f.Compute(4f));
Assert.Less(f.Compute(5), f.Compute(8f));
Assert.Less(f.Compute(8), f.Compute(10f));
Assert.Greater(f.Compute(10), f.Compute(11f));
Assert.Greater(f.Compute(11), f.Compute(12f));
Assert.Greater(f.Compute(14), f.Compute(16f));
Assert.IsTrue(Numeric.AreEqual(f.Compute(8), f.Compute(12)));
}
public void Test3()
{
Func <double, double> f = delegate(double x) { return(x * x * x * x * Math.Log(x + Math.Sqrt(x * x + 1))); };
RombergIntegratorD integrator = new RombergIntegratorD();
integrator.Epsilon = 0.000001;
double result = integrator.Integrate(f, 0, 2);
// Compare number of iterations with trapezoidal integrator.
TrapezoidalIntegratorD trap = new TrapezoidalIntegratorD();
trap.Epsilon = integrator.Epsilon;
double result2 = trap.Integrate(f, 0, 2);
Assert.IsTrue(Numeric.AreEqual(result, result2, 0.000002));
Assert.Greater(trap.NumberOfIterations, integrator.NumberOfIterations);
// Compare number of iterations with simpson integrator.
SimpsonIntegratorD simpson = new SimpsonIntegratorD();
simpson.Epsilon = integrator.Epsilon;
result2 = simpson.Integrate(f, 0, 2);
Assert.IsTrue(Numeric.AreEqual(result, result2, 0.000002));
Assert.Greater(simpson.NumberOfIterations, integrator.NumberOfIterations);
}
public void TestSeparated()
{
PlaneConvexAlgorithm algo = new PlaneConvexAlgorithm(new CollisionDetection());
CollisionObject a = new CollisionObject(new GeometricObject
{
Shape = new BoxShape(1, 2, 3),
Pose = new Pose(new Vector3F(0, 2, 0)),
});
CollisionObject b = new CollisionObject(new GeometricObject
{
Shape = new PlaneShape(new Vector3F(0, 1, 0), 0),
});
Assert.AreEqual(false, algo.HaveContact(a, b));
Assert.AreEqual(1, algo.GetClosestPoints(a, b).Count);
Assert.AreEqual(new Vector3F(0, -1, 0), algo.GetClosestPoints(a, b)[0].Normal);
Assert.AreEqual(new Vector3F(0.5f, 0.5f, 1.5f), algo.GetClosestPoints(a, b)[0].Position);
Assert.IsTrue(Numeric.AreEqual(-1, algo.GetClosestPoints(a, b)[0].PenetrationDepth));
// Test swapped.
Assert.AreEqual(false, algo.HaveContact(b, a));
Assert.AreEqual(1, algo.GetClosestPoints(b, a).Count);
Assert.AreEqual(new Vector3F(0, 1, 0), algo.GetClosestPoints(b, a)[0].Normal);
Assert.AreEqual(new Vector3F(0.5f, 0.5f, 1.5f), algo.GetClosestPoints(b, a)[0].Position);
Assert.IsTrue(Numeric.AreEqual(-1, algo.GetClosestPoints(b, a)[0].PenetrationDepth));
Assert.AreEqual(0, algo.GetContacts(a, b).Count);
}
public void ComputeCollision()
{
MinkowskiPortalRefinement algo = new MinkowskiPortalRefinement(new CollisionDetection());
CollisionObject a = new CollisionObject(new GeometricObject
{
Shape = new TriangleShape(new Vector3F(0, 0, 0), new Vector3F(0, 1, 0), new Vector3F(0, 0, 1))
});
CollisionObject b = new CollisionObject(new GeometricObject
{
Shape = new SphereShape(1)
});
ContactSet set;
set = algo.GetContacts(a, b);
Assert.AreEqual(true, algo.HaveContact(a, b));
Assert.IsTrue(Numeric.AreEqual(1, set[0].PenetrationDepth));
((GeometricObject)b.GeometricObject).Pose = new Pose(new Vector3F(2, 0.1f, 0.2f));
algo.UpdateContacts(set, 0);
Assert.AreEqual(false, algo.HaveContact(a, b));
Assert.AreEqual(0, set.Count);
((GeometricObject)b.GeometricObject).Pose = new Pose(new Vector3F(0.9f, 0.1f, 0.2f));
algo.UpdateContacts(set, 0);
Assert.AreEqual(true, algo.HaveContact(a, b));
Assert.AreEqual(1, set.Count);
Assert.IsTrue(Vector3F.AreNumericallyEqual(new Vector3F(0, 0.1f, 0.2f), set[0].PositionAWorld, 0.02f));
}
public void DensitiesAndHeightFalloff()
{
var fog = new Fog();
fog.Height0 = 0;
fog.Height1 = 0;
fog.Density = 123;
Assert.AreEqual(fog.Density, fog.Density0);
Assert.AreEqual(fog.Density, fog.Density1);
fog.Height0 = -77;
fog.Height1 = 123;
fog.Density = 0.77f;
fog.HeightFalloff = 0.023f;
var d0 = fog.Density0;
var d1 = fog.Density1;
fog.Density = 123;
fog.HeightFalloff = 0.12312321312f;
fog.Density0 = d0;
fog.Density1 = d1;
Assert.IsTrue(Numeric.AreEqual(0.77f, fog.Density));
Assert.IsTrue(Numeric.AreEqual(0.023f, fog.HeightFalloff));
fog.Density = 12;
fog.HeightFalloff = 0;
Assert.AreEqual(fog.Density0, fog.Density1);
}
public void TestNormals()
{
MinkowskiPortalRefinement algo = new MinkowskiPortalRefinement(new CollisionDetection());
CollisionObject box = new CollisionObject(new GeometricObject
{
Pose = new Pose(new Vector3F(1.99999f, 0, 0)),
Shape = new BoxShape(2, 2, 2),
});
CollisionObject sphere = new CollisionObject(new GeometricObject
{
Shape = new SphereShape(1)
});
ContactSet set;
set = algo.GetContacts(box, sphere);
Assert.AreEqual(1, set.Count);
Assert.IsTrue(Numeric.AreEqual(0, set[0].PenetrationDepth));
Assert.IsTrue(Vector3F.AreNumericallyEqual(new Vector3F(-1, 0, 0), set[0].Normal, 0.001f));
((GeometricObject)sphere.GeometricObject).Pose = new Pose(new Vector3F(0.2f, 0, 0));
algo.UpdateContacts(set, 0);
Assert.AreEqual(1, set.Count);
Assert.IsTrue(Numeric.AreEqual(0.2f, set[0].PenetrationDepth));
Assert.IsTrue(Vector3F.AreNumericallyEqual(new Vector3F(-1, 0, 0), set[0].Normal, 0.001f));
}
public void Length2()
{
Vector3D v = new Vector3D(1.0, 2.0, 3.0);
v.Length = 0.5;
Assert.IsTrue(Numeric.AreEqual(0.5, v.Length));
}
/// <overloads>
/// <summary>
/// Determines whether two matrices are equal (regarding a given tolerance).
/// </summary>
/// </overloads>
///
/// <summary>
/// Determines whether two matrices are equal (regarding the tolerance
/// <see cref="Numeric.EpsilonD"/>).
/// </summary>
/// <param name="matrix1">The first matrix.</param>
/// <param name="matrix2">The second matrix.</param>
/// <returns>
/// <see langword="true"/> if the matrices are equal (within the tolerance
/// <see cref="Numeric.EpsilonD"/>); otherwise, <see langword="false"/>.
/// </returns>
/// <remarks>
/// The two matrices are compared component-wise. If the differences of the components are less
/// than <see cref="Numeric.EpsilonD"/> the matrices are considered as being equal.
/// </remarks>
public static bool AreNumericallyEqual(Matrix22D matrix1, Matrix22D matrix2)
{
return(Numeric.AreEqual(matrix1.M00, matrix2.M00) &&
Numeric.AreEqual(matrix1.M01, matrix2.M01) &&
Numeric.AreEqual(matrix1.M10, matrix2.M10) &&
Numeric.AreEqual(matrix1.M11, matrix2.M11));
}
public void Test3()
{
Func <float, float> f = delegate(float x) { return(x * x * x * x * (float)Math.Log(x + Math.Sqrt(x * x + 1))); };
RombergIntegratorF integrator = new RombergIntegratorF();
integrator.Epsilon = 0.000001f;
float result = integrator.Integrate(f, 0, 2);
// Compare number of iterations with trapezoidal integrator.
TrapezoidalIntegratorF trap = new TrapezoidalIntegratorF();
trap.Epsilon = integrator.Epsilon;
float result2 = trap.Integrate(f, 0, 2);
Assert.IsTrue(Numeric.AreEqual(result, result2, 0.000002f));
Assert.Greater(trap.NumberOfIterations, integrator.NumberOfIterations);
// Compare number of iterations with simpson integrator.
SimpsonIntegratorF simpson = new SimpsonIntegratorF();
simpson.Epsilon = integrator.Epsilon;
result2 = simpson.Integrate(f, 0, 2);
Assert.IsTrue(Numeric.AreEqual(result, result2, 0.000002f));
Assert.Greater(simpson.NumberOfIterations, integrator.NumberOfIterations);
}
/// <summary>
/// Determines whether two matrices are equal (regarding a specific tolerance).
/// </summary>
/// <param name="matrix1">The first matrix.</param>
/// <param name="matrix2">The second matrix.</param>
/// <param name="epsilon">The tolerance value.</param>
/// <returns>
/// <see langword="true"/> if the matrices are equal (within the tolerance
/// <paramref name="epsilon"/>); otherwise, <see langword="false"/>.
/// </returns>
/// <remarks>
/// The two matrices are compared component-wise. If the differences of the components are less
/// than <paramref name="epsilon"/> the matrices are considered as being equal.
/// </remarks>
public static bool AreNumericallyEqual(Matrix22D matrix1, Matrix22D matrix2, double epsilon)
{
return(Numeric.AreEqual(matrix1.M00, matrix2.M00, epsilon) &&
Numeric.AreEqual(matrix1.M01, matrix2.M01, epsilon) &&
Numeric.AreEqual(matrix1.M10, matrix2.M10, epsilon) &&
Numeric.AreEqual(matrix1.M11, matrix2.M11, epsilon));
}
public void GetLength()
{
HermiteSegment2F s = new HermiteSegment2F
{
Point1 = new Vector2F(1, 2),
Tangent1 = (new Vector2F(10, 3) - new Vector2F(1, 2)) * 3,
Tangent2 = (new Vector2F(10, 2) - new Vector2F(7, 8)) * 3,
Point2 = new Vector2F(10, 2),
};
BezierSegment2F b = new BezierSegment2F
{
Point1 = new Vector2F(1, 2),
ControlPoint1 = new Vector2F(10, 3),
ControlPoint2 = new Vector2F(7, 8),
Point2 = new Vector2F(10, 2),
};
float length1 = s.GetLength(0, 1, 20, Numeric.EpsilonF);
float length2 = b.GetLength(0, 1, 20, Numeric.EpsilonF);
Assert.IsTrue(Numeric.AreEqual(length1, length2));
float approxLength = 0;
const float step = 0.0001f;
for (float u = 0; u <= 1.0f; u += step)
{
approxLength += (s.GetPoint(u) - s.GetPoint(u + step)).Length;
}
Assert.IsTrue(Numeric.AreEqual(approxLength, length1, 0.01f));
Assert.IsTrue(Numeric.AreEqual(s.GetLength(0, 1, 100, Numeric.EpsilonF), s.GetLength(0, 0.5f, 100, Numeric.EpsilonF) + s.GetLength(0.5f, 1, 100, Numeric.EpsilonF)));
Assert.IsTrue(Numeric.AreEqual(s.GetLength(0, 1, 100, Numeric.EpsilonF), s.GetLength(1, 0, 100, Numeric.EpsilonF)));
}
public void Test2()
{
// Test following: A ray touches a plane at the ray-end. Then the plane moves so that
// they are separated.
PlaneRayAlgorithm algo = new PlaneRayAlgorithm(new CollisionDetection());
// Plane in xz plane.
CollisionObject plane = new CollisionObject(new GeometricObject
{
Shape = new PlaneShape(Vector3F.UnitY, 1),
Pose = new Pose(new Vector3F(0, -1, 0)),
});
// Ray
CollisionObject ray = new CollisionObject(new GeometricObject
{
Shape = new RayShape(new Vector3F(0, 0, 0), new Vector3F(0, -1, 0), 10),
Pose = new Pose(new Vector3F(0, 10, 0)),
});
ContactSet contacts = algo.GetContacts(plane, ray);
Assert.AreEqual(true, contacts.HaveContact);
Assert.AreEqual(1, contacts.Count);
Assert.AreEqual(10, contacts[0].PenetrationDepth);
// Move plane less than contact position tolerance, but into a separated state.
((GeometricObject)plane.GeometricObject).Pose = new Pose(new Vector3F(0, -1.001f, 0));
algo.UpdateClosestPoints(contacts, 0);
Assert.AreEqual(false, contacts.HaveContact);
Assert.AreEqual(1, contacts.Count);
Assert.IsTrue(Numeric.AreEqual(-0.001f, contacts[0].PenetrationDepth));
}
/// <summary>
/// Integrates the specified function within the given interval.
/// </summary>
/// <param name="function">The function.</param>
/// <param name="lowerBound">The lower bound.</param>
/// <param name="upperBound">The upper bound.</param>
/// <returns>
/// The integral of the given function over the interval
/// [<paramref name="lowerBound"/>, <paramref name="upperBound"/>].
/// </returns>
/// <exception cref="ArgumentNullException">
/// <paramref name="function"/> is <see langword="null"/>.
/// </exception>
public override double Integrate(Func <double, double> function, double lowerBound, double upperBound)
{
NumberOfIterations = 0;
if (function == null)
{
throw new ArgumentNullException("function");
}
double integral = 0;
double oldIntegral = 0;
for (int i = 0; i < MaxNumberOfIterations; i++)
{
NumberOfIterations++;
integral = Integrate(function, lowerBound, upperBound, oldIntegral, i + 1);
if (NumberOfIterations >= MinNumberOfIterations) // Avoid spurious early convergence
{
if (Numeric.AreEqual(integral, oldIntegral, Epsilon))
{
return(integral);
}
}
oldIntegral = integral;
}
return(integral);
}
public void Test1()
{
float[] xValues = new float[] { 0, 1, 2, 3, 4, 5, 6, 7, 8, 9 };
float[] yValues = new float[] { 0, 4, 5, 3, 1, 2, 3, 7, 8, 9 };
// Setup scattered interpolation with Shepard's method.
ShepardInterpolationF shepard = new ShepardInterpolationF();
for (int i = 0; i < xValues.Length; i++)
{
shepard.Add(new Pair <VectorF, VectorF>(new VectorF(1, xValues[i]), new VectorF(1, yValues[i])));
}
shepard.Power = 3f;
Assert.IsTrue(Numeric.AreEqual(yValues[0], shepard.Compute(new VectorF(1, 0))[0]));
Assert.IsTrue(Numeric.AreEqual(yValues[2], shepard.Compute(new VectorF(1, 2))[0]));
Assert.IsTrue(Numeric.AreEqual(yValues[3], shepard.Compute(new VectorF(1, 3))[0]));
Assert.IsTrue(Numeric.AreEqual(yValues[4], shepard.Compute(new VectorF(1, 4))[0]));
Assert.IsTrue(Numeric.AreEqual(yValues[8], shepard.Compute(new VectorF(1, 8))[0]));
// Test clear and reuse object.
shepard.Clear();
for (int i = 0; i < xValues.Length; i++)
{
shepard.Add(new Pair <VectorF, VectorF>(new VectorF(1, xValues[i]), new VectorF(1, yValues[i])));
}
Assert.IsTrue(Numeric.AreEqual(yValues[0], shepard.Compute(new VectorF(1, 0))[0]));
}
private void SetScale(double sliderPosition)
{
if (Numeric.IsLessOrEqual(sliderPosition, SliderPositionSmall))
{
Scale = 1.0;
}
else if (Numeric.IsLess(sliderPosition, SliderPositionMedium))
{
Scale = ((sliderPosition - 116) / 4) * 0.25 + 1.25;
}
else if (Numeric.AreEqual(sliderPosition, SliderPositionMedium))
{
Scale = 3;
}
else if (Numeric.IsLess(sliderPosition, SliderPositionLarge))
{
Scale = ((sliderPosition - 155) / 4) * 0.5 + 3.5;
}
else if (Numeric.AreEqual(sliderPosition, SliderPositionLarge))
{
Scale = 6;
}
else if (Numeric.IsLess(sliderPosition, SliderPositionExtraLarge))
{
Scale = ((sliderPosition - 184) / 4) + 7;
}
else
{
Scale = 16;
}
}
protected override double FindRoot(Func <double, double> function, double x0, double x1)
{
NumberOfIterations = 0;
double y0 = function(x0);
double yMid = function(x1);
// Is one of the bounds the solution?
if (Numeric.IsZero(y0, EpsilonY))
{
return(x0);
}
if (Numeric.IsZero(yMid, EpsilonY))
{
return(x1);
}
// Is the bracket valid?
if (Numeric.AreEqual(x0, x1, EpsilonX) || y0 * yMid >= 0)
{
return(double.NaN);
}
// Setup the step size dx and x0, such that f(x0) < 0.
double dx;
if (y0 < 0)
{
dx = x1 - x0;
}
else
{
dx = x0 - x1;
x0 = x1;
}
// Assert: The root is within x0 and x0 + dx.
// Assert: f(x0) < 0.
for (int i = 0; i < MaxNumberOfIterations; i++)
{
NumberOfIterations++;
dx *= 0.5;
double xMid = x0 + dx;
yMid = function(xMid);
// Stop if xMid is the result or if the stepsize dx is less than Epsilon.
if (Numeric.IsZero(yMid, EpsilonY) || Numeric.IsZero(dx, EpsilonX))
{
return(xMid);
}
if (yMid < 0)
{
x0 = xMid;
}
}
return(double.NaN);
}
public void Length2()
{
Vector4F v = new Vector4F(1.0f, 2.0f, 3.0f, 4.0f);
v.Length = 0.3f;
Assert.IsTrue(Numeric.AreEqual(0.3f, v.Length));
}
public void CompositeShapeWithRotatedChildren()
{
var s = new CompositeShape();
s.Children.Add(new GeometricObject(new BoxShape(1, 2, 3), new Vector3(1.1f, 0.3f, 0.8f), new Pose(new Vector3(100, 10, 0), RandomHelper.Random.NextQuaternion())));
s.Children.Add(new GeometricObject(new ConeShape(1, 2), new Vector3(1.1f, 0.3f, 0.8f), new Pose(new Vector3(-10, -10, 0), RandomHelper.Random.NextQuaternion())));
float m0;
Vector3 com0;
Matrix i0;
MassHelper.GetMass(s, new Vector3(2), 1, true, 0.001f, 10, out m0, out com0, out i0);
var m = s.GetMesh(0.001f, 6);
m.Transform(Matrix.CreateScale(2));
float m1;
Vector3 com1;
Matrix i1;
MassHelper.GetMass(m, out m1, out com1, out i1);
const float e = 0.01f;
Assert.IsTrue(Numeric.AreEqual(m0, m1, e * (1 + m0)));
Assert.IsTrue(Vector3.AreNumericallyEqual(com0, com1, e * (1 + com0.Length)));
Assert.IsTrue(Matrix.AreNumericallyEqual(i0, i1, e * (1 + i0.Trace)));
}
