/// <summary>
/// Gets the bounding sphere of a geometric object.
/// </summary>
/// <param name="geometricObject">The geometric object.</param>
/// <param name="radius">The radius of the bounding sphere.</param>
/// <param name="center">The center of the bounding sphere in world space.</param>
internal static void GetBoundingSphere(IGeometricObject geometricObject, out float radius, out Vector3F center)
{
// Get sphere from AABB.
Aabb aabb = geometricObject.Aabb;
center = aabb.Center;
radius = aabb.Extent.Length * 0.5f;
}
public void LinearConvexHull()
{
var start = new Vector3F(1, 10, 20);
var end = new Vector3F(10, 10, 20);
var points = new[] { start, end };
var mesh = GeometryHelper.CreateConvexHull(points);
Assert.AreEqual(2, mesh.Vertices.Count);
Assert.IsTrue(mesh.Vertices.Any(v => v.Position == start));
Assert.IsTrue(mesh.Vertices.Any(v => v.Position == end));
Assert.AreEqual(null, mesh.Vertex.Edge.Face);
points = new[] { start, end, start, end, start, new Vector3F(1.00001f, 10.00001f, 20.00001f), };
mesh = GeometryHelper.CreateConvexHull(points);
Assert.AreEqual(2, mesh.Vertices.Count);
Assert.IsTrue(mesh.Vertices.Any(v => v.Position == start));
Assert.IsTrue(mesh.Vertices.Any(v => v.Position == end));
Assert.AreEqual(null, mesh.Vertex.Edge.Face);
points = new[] { new Vector3F(2, 10, 20), new Vector3F(9.00001f, 10, 20), start, end, start };
mesh = GeometryHelper.CreateConvexHull(points);
Assert.AreEqual(2, mesh.Vertices.Count);
Assert.IsTrue(mesh.Vertices.Any(v => v.Position == start));
Assert.IsTrue(mesh.Vertices.Any(v => v.Position == end));
Assert.AreEqual(null, mesh.Vertex.Edge.Face);
}
public override bool Prepare(Vector3I[] marks) {
a = marks[0];
b = marks[1];
c = marks[2];
if (a == b || b == c || c == a) {
if (a != c) b = c;
isLine = true;
}
Bounds = new BoundingBox(
Math.Min(Math.Min(a.X, b.X), c.X),
Math.Min(Math.Min(a.Y, b.Y), c.Y),
Math.Min(Math.Min(a.Z, b.Z), c.Z),
Math.Max(Math.Max(a.X, b.X), c.X),
Math.Max(Math.Max(a.Y, b.Y), c.Y),
Math.Max(Math.Max(a.Z, b.Z), c.Z)
);
Coords = Bounds.MinVertex;
if (!base.Prepare(marks)) return false;
normal = (b - a).Cross(c - a);
normalF = normal.Normalize();
BlocksTotalEstimate = GetBlockTotalEstimate();
s1 = normal.Cross(a - b).Normalize();
s2 = normal.Cross(b - c).Normalize();
s3 = normal.Cross(c - a).Normalize();
return true;
}
public void GetClosestPointCandidates(Vector3F scale, Pose pose, ISpatialPartition<int> otherPartition, Vector3F otherScale, Pose otherPose, Func<int, int, float> callback)
{
if (otherPartition == null)
throw new ArgumentNullException("otherPartition");
if (callback == null)
throw new ArgumentNullException("callback");
// Make sure we are up-to-date.
var otherBasePartition = otherPartition as BasePartition<int>;
if (otherBasePartition != null)
otherBasePartition.UpdateInternal();
else
otherPartition.Update(false);
Update(false);
if (_numberOfItems == 0)
return;
if (otherPartition is ISupportClosestPointQueries<int>)
{
// ----- CompressedAabbTree vs. ISupportClosestPointQueries<int>
GetClosestPointCandidatesImpl(scale, pose, (ISupportClosestPointQueries<int>)otherPartition, otherScale, otherPose, callback);
}
else
{
// ----- CompressedAabbTree vs. *
GetClosestPointCandidatesImpl(otherPartition, callback);
}
}
public SphereStackSample(Microsoft.Xna.Framework.Game game)
: base(game)
{
// Add basic force effects.
Simulation.ForceEffects.Add(new Gravity());
Simulation.ForceEffects.Add(new Damping());
// Add a ground plane.
RigidBody groundPlane = new RigidBody(new PlaneShape(Vector3F.UnitY, 0))
{
Name = "GroundPlane", // Names are not required but helpful for debugging.
MotionType = MotionType.Static,
};
Simulation.RigidBodies.Add(groundPlane);
// ----- Add a stack of spheres.
const int numberOfSpheres = 10;
const float sphereRadius = 0.4f;
Shape sphereShape = new SphereShape(sphereRadius);
// Optional: Use a small overlap between spheres to improve the stability.
float overlap = Simulation.Settings.Constraints.AllowedPenetration * 0.5f;
Vector3F position = new Vector3F(0, sphereRadius - overlap, 0);
for (int i = 0; i < numberOfSpheres; i++)
{
RigidBody sphere = new RigidBody(sphereShape)
{
Name = "Sphere" + i,
Pose = new Pose(position),
};
Simulation.RigidBodies.Add(sphere);
position.Y += 2 * sphereRadius - overlap;
}
}
public void PoseTest()
{
CameraInstance cameraInstance = new CameraInstance(new Camera(new PerspectiveProjection()));
Assert.IsNotNull(cameraInstance.PoseWorld);
Assert.AreEqual(Vector3F.Zero, cameraInstance.PoseWorld.Position);
Assert.AreEqual(Matrix33F.Identity, cameraInstance.PoseWorld.Orientation);
// Set new Pose
Vector3F position = new Vector3F(1, 2, 3);
QuaternionF orientation = QuaternionF.CreateRotation(new Vector3F(3, 4, 5), 0.123f);
cameraInstance.PoseWorld = new Pose(position, orientation);
Assert.AreEqual(position, cameraInstance.PoseWorld.Position);
Assert.AreEqual(orientation.ToRotationMatrix33(), cameraInstance.PoseWorld.Orientation);
Assert.IsTrue(Matrix44F.AreNumericallyEqual(cameraInstance.PoseWorld.ToMatrix44F(), cameraInstance.ViewInverse));
Assert.IsTrue(Matrix44F.AreNumericallyEqual(cameraInstance.PoseWorld.Inverse.ToMatrix44F(), cameraInstance.View));
// Set Position and Orientation
position = new Vector3F(5, 6, 7);
orientation = QuaternionF.CreateRotation(new Vector3F(1, -1, 6), -0.123f);
cameraInstance.PoseWorld = new Pose(position, orientation);
Assert.AreEqual(position, cameraInstance.PoseWorld.Position);
Assert.AreEqual(orientation.ToRotationMatrix33(), cameraInstance.PoseWorld.Orientation);
Assert.IsTrue(Matrix44F.AreNumericallyEqual(cameraInstance.PoseWorld.Inverse.ToMatrix44F(), cameraInstance.View));
Assert.IsTrue(Matrix44F.AreNumericallyEqual(cameraInstance.PoseWorld.ToMatrix44F(), cameraInstance.ViewInverse));
}
//--------------------------------------------------------------
public KinectWrapper(Game game)
: base(game)
{
Offset = new Vector3F(0, 0, 0);
Scale = new Vector3F(1, 1, 1);
InitializeSkeletonPoses();
}
//--------------------------------------------------------------
/// <summary>
/// Initializes a new instance of the <see cref="PlanarReflectionNode" /> class.
/// </summary>
/// <param name="renderToTexture">The render texture target.</param>
/// <exception cref="ArgumentNullException">
/// <paramref name="renderToTexture"/> is <see langword="null"/>.
/// </exception>
public PlanarReflectionNode(RenderToTexture renderToTexture)
: base(renderToTexture)
{
CameraNode = new CameraNode(new Camera(new PerspectiveProjection()));
FieldOfViewScale = 1;
_normalLocal = new Vector3F(0, 0, 1);
}
public override void Update(GameTime gameTime)
{
float deltaTime = (float)gameTime.ElapsedGameTime.TotalSeconds;
// ----- Move target if <NumPad4-9> are pressed.
Vector3F translation = new Vector3F();
if (InputService.IsDown(Keys.NumPad4))
translation.X -= 1;
if (InputService.IsDown(Keys.NumPad6))
translation.X += 1;
if (InputService.IsDown(Keys.NumPad8))
translation.Y += 1;
if (InputService.IsDown(Keys.NumPad5))
translation.Y -= 1;
if (InputService.IsDown(Keys.NumPad9))
translation.Z += 1;
if (InputService.IsDown(Keys.NumPad7))
translation.Z -= 1;
translation = translation * deltaTime;
_targetPosition += translation;
// Convert target world space position to model space. - The IK solvers work in model space.
Vector3F localTargetPosition = _pose.ToLocalPosition(_targetPosition);
// Reset the affected bones. This is optional. It removes unwanted twist from the bones.
_skeletonPose.ResetBoneTransforms(_ikSolver.RootBoneIndex, _ikSolver.TipBoneIndex);
// Let IK solver update the bones.
_ikSolver.Target = localTargetPosition;
_ikSolver.Solve(deltaTime);
base.Update(gameTime);
}
public static void ComputeBoundingSphere(IEnumerable<Vector3F> points, out float radius, out Vector3F center)
{
if (points == null)
throw new ArgumentNullException("points");
List<Vector3F> pointList = DigitalRune.ResourcePools<Vector3F>.Lists.Obtain();
// Copy points into new list because the order of the points need to be changed.
// (Try to avoid garbage when enumerating 'points'.)
if (points is Vector3F[])
{
foreach (Vector3F p in (Vector3F[])points)
pointList.Add(p);
}
else if (points is List<Vector3F>)
{
foreach (Vector3F p in (List<Vector3F>)points)
pointList.Add(p);
}
else
{
foreach (Vector3F p in points)
pointList.Add(p);
}
if (pointList.Count == 0)
throw new ArgumentException("The list of 'points' is empty.");
ComputeWelzlSphere(pointList, 0, pointList.Count - 1, 0, out radius, out center);
DigitalRune.ResourcePools<Vector3F>.Lists.Recycle(pointList);
}
private void InitializeSimulation(Simulation simulation, Vector3F offset)
{
// Add default force effects.
simulation.ForceEffects.Add(new Gravity());
simulation.ForceEffects.Add(new Damping());
// Add a ground plane.
RigidBody groundPlane = new RigidBody(new PlaneShape(Vector3F.UnitY, 0))
{
Name = "GroundPlane",
MotionType = MotionType.Static,
};
simulation.RigidBodies.Add(groundPlane);
// Add a stack of boxes.
const float boxSize = 0.8f;
float overlap = simulation.Settings.Constraints.AllowedPenetration * 0.5f;
float yPosition = boxSize / 2 - overlap;
BoxShape boxShape = new BoxShape(boxSize, boxSize, boxSize);
for (int i = 0; i < 15; i++)
{
RigidBody stackBox = new RigidBody(boxShape)
{
Name = "StackBox" + i,
Pose = new Pose(new Vector3F(0, yPosition, 0) + offset),
};
simulation.RigidBodies.Add(stackBox);
yPosition += boxSize - overlap;
}
}
public void NegativeUniformScaling()
{
Vector3F point0 = new Vector3F(1, 0.5f, 0.5f);
Vector3F point1 = new Vector3F(0.5f, 1, 0.5f);
Vector3F point2 = new Vector3F(0.5f, 0.5f, 1);
Plane plane = new Plane(point0, point1, point2);
Vector3F pointAbove = plane.Normal * plane.DistanceFromOrigin * 2;
Vector3F pointBelow = plane.Normal * plane.DistanceFromOrigin * 0.5f;
Vector3F scale = new Vector3F(-3.5f);
point0 *= scale;
point1 *= scale;
point2 *= scale;
pointAbove *= scale;
pointBelow *= scale;
plane.Scale(ref scale);
Assert.IsTrue(plane.Normal.IsNumericallyNormalized);
Vector3F dummy;
Assert.IsTrue(GeometryHelper.GetClosestPoint(plane, point0, out dummy));
Assert.IsTrue(GeometryHelper.GetClosestPoint(plane, point1, out dummy));
Assert.IsTrue(GeometryHelper.GetClosestPoint(plane, point2, out dummy));
Assert.IsTrue(Vector3F.Dot(plane.Normal, pointAbove) > plane.DistanceFromOrigin);
Assert.IsTrue(Vector3F.Dot(plane.Normal, pointBelow) < plane.DistanceFromOrigin);
}
/// <summary>
/// Determines whether the specified point is contained in the mesh. (This method assumes that
/// the mesh is a convex polyhedron.)
/// </summary>
/// <param name="point">The point.</param>
/// <param name="epsilon">
/// The epsilon tolerance. A point counts as "contained" if the distance to the mesh surface is
/// less than this value. Use a small positive value, e.g. 0.001f, for numerical robustness.
/// </param>
/// <returns>
/// <see langword="true"/> if the specified point is contained; otherwise,
/// <see langword="false"/>. (The result is undefined if the mesh is not a convex polyhedron.)
/// </returns>
public bool Contains(Vector3F point, float epsilon)
{
foreach (var face in Faces)
{
// Get normal vector.
var normal = face.Normal;
// Skip degenerate faces.
float normalLength = normal.Length;
if (Numeric.IsZero(normalLength))
continue;
// Normalize.
normal = normal / normalLength;
// Create a plane for the face.
Plane plane = new Plane(normal, face.Boundary.Origin.Position);
// Get distance from plane.
float d = Vector3F.Dot(point, plane.Normal) - plane.DistanceFromOrigin;
// Check distance with epsilon tolerance.
if (d > epsilon * (1 + normalLength + (point - face.Boundary.Origin.Position).Length))
return false;
}
return true;
}
public static bool GetClosestPoint(Line line, Vector3F point, out Vector3F closestPointOnLine)
{
float parameter;
GetLineParameter(new LineSegment(line.PointOnLine, line.PointOnLine + line.Direction), point, out parameter);
closestPointOnLine = line.PointOnLine + parameter * line.Direction;
return Vector3F.AreNumericallyEqual(point, closestPointOnLine);
}
/// <summary>
/// Diagonalizes the inertia matrix.
/// </summary>
/// <param name="inertia">The inertia matrix.</param>
/// <param name="inertiaDiagonal">The inertia of the principal axes.</param>
/// <param name="rotation">
/// The rotation that rotates from principal axis space to parent/world space.
/// </param>
/// <remarks>
/// All valid inertia matrices can be transformed into a coordinate space where all elements
/// non-diagonal matrix elements are 0. The axis of this special space are the principal axes.
/// </remarks>
internal static void DiagonalizeInertia(Matrix33F inertia, out Vector3F inertiaDiagonal, out Matrix33F rotation)
{
// Alternatively we could use Jacobi transformation (iterative method, see Bullet/btMatrix3x3.diagonalize()
// and Numerical Recipes book) or we could find the eigenvalues using the characteristic
// polynomial which is a cubic polynomial and then solve for the eigenvectors (see Numeric
// Recipes and "Mathematics for 3D Game Programming and Computer Graphics" chapter ray-tracing
// for cubic equations and computation of bounding boxes.
// Perform eigenvalue decomposition.
var eigenValueDecomposition = new EigenvalueDecompositionF(inertia.ToMatrixF());
inertiaDiagonal = eigenValueDecomposition.RealEigenvalues.ToVector3F();
rotation = eigenValueDecomposition.V.ToMatrix33F();
if (!rotation.IsRotation)
{
// V is orthogonal but not necessarily a rotation. If it is no rotation
// we have to swap two columns.
MathHelper.Swap(ref inertiaDiagonal.Y, ref inertiaDiagonal.Z);
Vector3F dummy = rotation.GetColumn(1);
rotation.SetColumn(1, rotation.GetColumn(2));
rotation.SetColumn(2, dummy);
Debug.Assert(rotation.IsRotation);
}
}
public override bool Prepare(Vector3I[] marks)
{
a = marks[0];
c = marks[1];
b = marks[2];
d = new Vector3I(a.X + c.X - b.X, a.Y + c.Y - b.Y, a.Z + c.Z - b.Z);
Bounds = new BoundingBox(
Math.Min(Math.Min(a.X, b.X), Math.Min(c.X, d.X)),
Math.Min(Math.Min(a.Y, b.Y), Math.Min(c.Y, d.Y)),
Math.Min(Math.Min(a.Z, b.Z), Math.Min(c.Z, d.Z)),
Math.Max(Math.Max(a.X, b.X), Math.Max(c.X, d.X)),
Math.Max(Math.Max(a.Y, b.Y), Math.Max(c.Y, d.Y)),
Math.Max(Math.Max(a.Z, b.Z), Math.Max(c.Z, d.Z))
);
Coords = Bounds.MinVertex;
if (!base.Prepare(marks))
return false;
normal = (b - a).Cross(c - a);
normalF = normal.Normalize();
BlocksTotalEstimate = GetBlockTotalEstimate();
s1 = normal.Cross(a - b).Normalize();
s2 = normal.Cross(b - c).Normalize();
s3 = normal.Cross(c - d).Normalize();
s4 = normal.Cross(d - a).Normalize();
return true;
}
private float[] GenData(Vector3F origin, int width, int height, int depth)
{
var data = new float[width*height*depth];
for (var x = 0; x < width; x++)
{
for (var y = 0; y < height; y++)
{
for (var z = 0; z < depth; z++)
{
var i = x + width*(y + height*z);
if (y == height - 1)
{
data[i] = -1;
continue;
}
if (y == 0)
{
data[i] = 1;
continue;
}
data[i] = (float) _b.Get(_seed, origin.x + x, origin.y + y, origin.z + z, OpenSimplex.Get);
}
}
}
return data;
}
public void Normalize()
{
Vector3F v, n1, n2;
float magnitude;
v = new Vector3F(3.0f, 4.0f, 0.0f);
n1 = v.Normalize();
n2 = v.Normalize(out magnitude);
Assert.AreEqual(1.0f, n1.Magnitude, 1e-7);
Assert.AreEqual(1.0f, n2.Magnitude, 1e-7);
Assert.AreEqual(5.0f, magnitude, 1e-7);
v = new Vector3F(3.0f, 0.0f, 4.0f);
n1 = v.Normalize();
n2 = v.Normalize(out magnitude);
Assert.AreEqual(1.0f, n1.Magnitude, 1e-7);
Assert.AreEqual(1.0f, n2.Magnitude, 1e-7);
Assert.AreEqual(5.0f, magnitude, 1e-7);
v = new Vector3F(0.0f, 3.0f, 4.0f);
n1 = v.Normalize();
n2 = v.Normalize(out magnitude);
Assert.AreEqual(1.0f, n1.Magnitude, 1e-7);
Assert.AreEqual(1.0f, n2.Magnitude, 1e-7);
Assert.AreEqual(5.0f, magnitude, 1e-7);
}
//--------------------------------------------------------------
/// <summary>
/// Initializes a new instance of the <see cref="GodRayFilter"/> class.
/// </summary>
/// <param name="graphicsService">The graphics service.</param>
/// <exception cref="ArgumentNullException">
/// <paramref name="graphicsService"/> is <see langword="null"/>.
/// </exception>
public GodRayFilter(IGraphicsService graphicsService)
: base(graphicsService)
{
Effect effect = GraphicsService.Content.Load<Effect>("DigitalRune/PostProcessing/GodRayFilter");
_viewportSizeParameter = effect.Parameters["ViewportSize"];
_parameters0Parameter = effect.Parameters["Parameters0"];
_parameters1Parameter = effect.Parameters["Parameters1"];
_intensityParameter = effect.Parameters["Intensity"];
_numberOfSamplesParameter = effect.Parameters["NumberOfSamples"];
_sourceTextureParameter = effect.Parameters["SourceTexture"];
_gBuffer0Parameter = effect.Parameters["GBuffer0"];
_rayTextureParameter = effect.Parameters["RayTexture"];
_createMaskPass = effect.CurrentTechnique.Passes["CreateMask"];
_blurPass = effect.CurrentTechnique.Passes["Blur"];
_combinePass = effect.CurrentTechnique.Passes["Combine"];
_downsampleFilter = graphicsService.GetDownsampleFilter();
Scale = 1;
LightDirection = new Vector3F(0, -1, 0);
LightRadius = 0.2f;
Intensity = new Vector3F(1, 1, 1);
DownsampleFactor = 4;
NumberOfSamples = 8;
NumberOfPasses = 2;
Softness = 1;
}
public void SceneWithStaticMeshLODandVisObj()
{
TestManager.Helpers.OpenSceneFromFile(TestManager.Helpers.TestDataDir + @"\VisibilityTests\VisibilityObjLinking.scene");
TestManager.Helpers.ProcessEvents();
// Start the simulation
EditorManager.EditorMode = EditorManager.Mode.EM_ANIMATING; // update view
TestManager.Helpers.ProcessEvents();
// Let the simulation run for some seconds
DateTime timeBefore = DateTime.Now;
EditorManager.ActiveView.SetCameraRotation(new Vector3F(90, 0, 0));
while (true)
{
TimeSpan passedTime = DateTime.Now - timeBefore;
if (passedTime.TotalSeconds > 5)
break;
Vector3F pos = new Vector3F(50.0f,(float)passedTime.TotalSeconds * -150.0f-150.0f, 120.0f);
EditorManager.ActiveView.SetCameraPosition(pos);
TestManager.Helpers.ProcessEvents();
}
// Stop simulation, set back
EditorManager.EditorMode = EditorManager.Mode.EM_NONE;
TextToWrite = null;
TestManager.Helpers.CloseActiveProject();
}
/// <summary>
/// Initializes the 1-dimensional constraint.
/// </summary>
public void Prepare(RigidBody bodyA, RigidBody bodyB, Vector3F jLinA, Vector3F jAngA, Vector3F jLinB, Vector3F jAngB)
{
JLinA = jLinA;
JAngA = jAngA;
JLinB = jLinB;
JAngB = jAngB;
WJTLinA.X = bodyA.MassInverse * jLinA.X;
WJTLinA.Y = bodyA.MassInverse * jLinA.Y;
WJTLinA.Z = bodyA.MassInverse * jLinA.Z;
Matrix33F matrix = bodyA.InertiaInverseWorld;
WJTAngA.X = matrix.M00 * jAngA.X + matrix.M01 * jAngA.Y + matrix.M02 * jAngA.Z;
WJTAngA.Y = matrix.M10 * jAngA.X + matrix.M11 * jAngA.Y + matrix.M12 * jAngA.Z;
WJTAngA.Z = matrix.M20 * jAngA.X + matrix.M21 * jAngA.Y + matrix.M22 * jAngA.Z;
WJTLinB.X = bodyB.MassInverse * jLinB.X;
WJTLinB.Y = bodyB.MassInverse * jLinB.Y;
WJTLinB.Z = bodyB.MassInverse * jLinB.Z;
matrix = bodyB.InertiaInverseWorld;
WJTAngB.X = matrix.M00 * jAngB.X + matrix.M01 * jAngB.Y + matrix.M02 * jAngB.Z;
WJTAngB.Y = matrix.M10 * jAngB.X + matrix.M11 * jAngB.Y + matrix.M12 * jAngB.Z;
WJTAngB.Z = matrix.M20 * jAngB.X + matrix.M21 * jAngB.Y + matrix.M22 * jAngB.Z;
float JWJT = jLinA.X * WJTLinA.X + jLinA.Y * WJTLinA.Y + jLinA.Z * WJTLinA.Z
+ jAngA.X * WJTAngA.X + jAngA.Y * WJTAngA.Y + jAngA.Z * WJTAngA.Z
+ jLinB.X * WJTLinB.X + jLinB.Y * WJTLinB.Y + jLinB.Z * WJTLinB.Z
+ jAngB.X * WJTAngB.X + jAngB.Y * WJTAngB.Y + jAngB.Z * WJTAngB.Z;
JWJT += Softness;
JWJTInverse = 1 / JWJT;
}
public void Construct01()
{
Vector3F v = new Vector3F(new Vector2F(1.0f, 2.0f), 3.0f);
Assert.AreEqual(1.0f, v.X);
Assert.AreEqual(2.0f, v.Y);
Assert.AreEqual(3.0f, v.Z);
}
public void Addition()
{
Vector3F a = new Vector3F(1.0f, 2.0f, 3.0f);
Vector3F b = new Vector3F(2.0f, 3.0f, 4.0f);
Vector3F c = Vector3F.Add(a, b);
Assert.AreEqual(new Vector3F(3.0f, 5.0f, 7.0f), c);
}
public void AdditionOperator()
{
Vector3F a = new Vector3F(1.0f, 2.0f, 3.0f);
Vector3F b = new Vector3F(2.0f, 3.0f, 4.0f);
Vector3F c = a + b;
Assert.AreEqual(new Vector3F(3.0f, 5.0f, 7.0f), c);
}
public static Matrix4x4F MatrixLookAtLH(Vector3F eye, Vector3F at, Vector3F up)
{
Vector3F right, vec;
vec = at - eye;
vec.NormalizeInPlace();
right = Vector3F.Cross(up, vec);
up = Vector3F.Cross(vec, right);
right.NormalizeInPlace();
up.NormalizeInPlace();
return new Matrix4x4F(
right.X,
up.X,
vec.X,
0.0f,
right.Y,
up.Y,
vec.Y,
0.0f,
right.Z,
up.Z,
vec.Z,
0.0f,
-Vector3F.Dot(right, eye),
-Vector3F.Dot(up, eye),
-Vector3F.Dot(vec, eye),
1.0f
);
}
/// <summary>
/// Initializes a new instance of the <see cref="SkyboxNode" /> class.
/// </summary>
/// <param name="texture">The cube map texture (using premultiplied alpha).</param>
public SkyboxNode(TextureCube texture)
{
Texture = texture;
Color = new Vector3F(1, 1, 1);
Alpha = 1.0f;
Encoding = ColorEncoding.SRgb;
}
public void AdditivePath()
{
var animation = new Path3FAnimation();
animation.Path = new Path3F
{
new PathKey3F { Parameter = 2.0f, Point = new Vector3F(2.0f, 22.0f, 10.0f), Interpolation = SplineInterpolation.Linear },
new PathKey3F { Parameter = 3.0f, Point = new Vector3F(3.0f, 33.0f, 20.0f), Interpolation = SplineInterpolation.Linear },
new PathKey3F { Parameter = 4.0f, Point = new Vector3F(4.0f, 44.0f, 30.0f), Interpolation = SplineInterpolation.Linear },
};
animation.Path.PreLoop = CurveLoopType.Linear;
animation.Path.PostLoop = CurveLoopType.Cycle;
animation.EndParameter = float.PositiveInfinity;
animation.IsAdditive = true;
Vector3F defaultSource = new Vector3F(1.0f, 2.0f, 3.0f);
Vector3F defaultTarget = new Vector3F(10.0f, 20.0f, 30.0f);
// Pre-Loop
Assert.AreEqual(defaultSource + new Vector3F(0.0f, 0.0f, -10.0f), animation.GetValue(TimeSpan.FromSeconds(0.0), defaultSource, defaultTarget));
Assert.AreEqual(defaultSource + new Vector3F(1.0f, 11.0f, 0.0f), animation.GetValue(TimeSpan.FromSeconds(1.0), defaultSource, defaultTarget));
Assert.AreEqual(defaultSource + new Vector3F(2.0f, 22.0f, 10.0f), animation.GetValue(TimeSpan.FromSeconds(2.0), defaultSource, defaultTarget));
Assert.AreEqual(defaultSource + new Vector3F(3.0f, 33.0f, 20.0f), animation.GetValue(TimeSpan.FromSeconds(3.0), defaultSource, defaultTarget));
Assert.AreEqual(defaultSource + new Vector3F(4.0f, 44.0f, 30.0f), animation.GetValue(TimeSpan.FromSeconds(4.0), defaultSource, defaultTarget));
// Post-Loop
Assert.AreEqual(defaultSource + new Vector3F(3.0f, 33.0f, 20.0f), animation.GetValue(TimeSpan.FromSeconds(5.0), defaultSource, defaultTarget));
}
public override void Update(GameTime gameTime)
{
// If <Space> is pressed, we fire a shot.
if (InputService.IsPressed(Keys.Space, true))
{
// Get a random angle and a random distance from the target.
float angle = _angleDistribution.Next(_random);
float distance = _distanceDistribution.Next(_random);
// Create a vector v with the length of distance.
Vector3F v = new Vector3F(0, distance, 0);
// Rotate v.
QuaternionF rotation = QuaternionF.CreateRotationZ(MathHelper.ToRadians(angle));
v = rotation.Rotate(v);
// Draw a small cross for the hit.
var debugRenderer = GraphicsScreen.DebugRenderer2D;
debugRenderer.DrawLine(
_center + v + new Vector3F(-10, -10, 0),
_center + v + new Vector3F(10, 10, 0),
Color.Black,
true);
debugRenderer.DrawLine(
_center + v + new Vector3F(10, -10, 0),
_center + v + new Vector3F(-10, 10, 0),
Color.Black, true);
}
base.Update(gameTime);
}
public void IdentityTest()
{
var traits = Vector3FTraits.Instance;
var value = new Vector3F(-1, -2, 3);
Assert.AreEqual(value, traits.Add(value, traits.Identity()));
Assert.AreEqual(value, traits.Add(traits.Identity(), value));
}
public RedSquareObject(Vector3F position)
{
_position = position;
_color = new Vector4(1.0f, 0.0f, 0.0f, 1.0f);
var graphicsService = ServiceLocator.Current.GetInstance<IGraphicsService>();
var screen = ((GameScreen)graphicsService.Screens["Default"]);
var contentManager = ServiceLocator.Current.GetInstance<ContentManager>();
_model = contentManager.Load<ModelNode>("redSquare").Clone();
screen.Scene.Children.Add(_model);
_model.PoseWorld = new Pose(_position);
foreach (var meshNode in _model.GetSubtree().OfType<MeshNode>())
{
Mesh mesh = meshNode.Mesh;
foreach (var material in mesh.Materials)
{
var effectBinding = material["Default"];
effectBinding.Set("DiffuseColor", _color);
((BasicEffectBinding)effectBinding).LightingEnabled = false;
}
}
InUse = true;
}
private static Matrix4F smethod_0(Vector3F xaxis, Vector3F zaxis)
{
Vector3F yaxis = Vector3F.CrossProduct(zaxis, xaxis);
return(Transformation4F.GetCoordSystem(xaxis, yaxis, zaxis));
}
/// <summary>
/// Initializes a new instance of the <see cref="Segment"/> class with serialized data.
/// </summary>
/// <param name="info">The object that holds the serialized object data.</param>
/// <param name="context">The contextual information about the source or destination.</param>
private Segment(SerializationInfo info, StreamingContext context)
{
_p0 = (Vector3F)info.GetValue("P0", typeof(Vector3F));
_p1 = (Vector3F)info.GetValue("P1", typeof(Vector3F));
}
/// <summary>
/// Initializes a new instance of the <see cref="Segment"/> class using an existing <see cref="Segment"/> instance.
/// </summary>
/// <param name="l">A <see cref="Segment"/> instance.</param>
public Segment(Segment l)
{
_p0 = l.P0;
_p1 = l.P1;
}
public override void ComputeCollision(ContactSet contactSet, CollisionQueryType type)
{
// Object A should be the height field.
CollisionObject heightFieldCollisionObject = contactSet.ObjectA;
CollisionObject otherCollisionObject = contactSet.ObjectB;
// Swap objects if necessary.
bool swapped = !(heightFieldCollisionObject.GeometricObject.Shape is HeightField);
if (swapped)
{
MathHelper.Swap(ref heightFieldCollisionObject, ref otherCollisionObject);
}
IGeometricObject heightFieldGeometricObject = heightFieldCollisionObject.GeometricObject;
IGeometricObject otherGeometricObject = otherCollisionObject.GeometricObject;
HeightField heightField = heightFieldGeometricObject.Shape as HeightField;
Shape otherShape = otherGeometricObject.Shape;
// Check if collision object shapes are correct.
if (heightField == null)
{
throw new ArgumentException("The contact set must contain a height field.", "contactSet");
}
if (heightField.UseFastCollisionApproximation && type != CollisionQueryType.ClosestPoints)
{
// If other object is convex, use the new fast collision detection algorithm.
ConvexShape convex = otherShape as ConvexShape;
if (convex != null)
{
ComputeCollisionFast(
contactSet,
type,
heightFieldGeometricObject,
otherGeometricObject,
heightField,
convex,
swapped);
return;
}
}
#region ----- Precomputations -----
Vector3F scaleHeightField = heightFieldGeometricObject.Scale;
Vector3F scaleOther = otherGeometricObject.Scale;
Pose heightFieldPose = heightFieldGeometricObject.Pose;
// We do not support negative scaling. It is not clear what should happen when y is
// scaled with a negative factor and triangle orders would be wrong... Not worth the trouble.
if (scaleHeightField.X < 0 || scaleHeightField.Y < 0 || scaleHeightField.Z < 0)
{
throw new NotSupportedException("Computing collisions for height fields with a negative scaling is not supported.");
}
// Get height field and basic info.
Vector3F heightFieldUpAxis = heightFieldPose.ToWorldDirection(Vector3F.UnitY);
int arrayLengthX = heightField.NumberOfSamplesX;
int arrayLengthZ = heightField.NumberOfSamplesZ;
Debug.Assert(arrayLengthX > 1 && arrayLengthZ > 1, "A height field should contain at least 2 x 2 elements (= 1 cell).");
float cellWidthX = heightField.WidthX * scaleHeightField.X / (arrayLengthX - 1);
float cellWidthZ = heightField.WidthZ * scaleHeightField.Z / (arrayLengthZ - 1);
// The search-space is the rectangular region on the height field where the closest points
// must lie in. For contacts we do not have to search neighbor cells. For closest-point
// queries and separation we have to search neighbor cells.
// We compute the search-space using a current maximum search distance.
float currentSearchDistance = 0;
Contact guessedClosestPair = null;
if (!contactSet.HaveContact && type == CollisionQueryType.ClosestPoints)
{
// Make a guess for the closest pair using SupportMapping or InnerPoints.
bool isOverHole;
guessedClosestPair = GuessClosestPair(contactSet, swapped, out isOverHole);
if (isOverHole)
{
// Guesses over holes are useless. --> Check the whole terrain.
currentSearchDistance = heightFieldGeometricObject.Aabb.Extent.Length;
}
else if (guessedClosestPair.PenetrationDepth < 0)
{
currentSearchDistance = -guessedClosestPair.PenetrationDepth;
}
else
{
contactSet.HaveContact = true;
}
}
else
{
// Assume no contact.
contactSet.HaveContact = false;
}
// Get AABB of the other object in local space of the height field.
Aabb aabbOfOther = otherShape.GetAabb(scaleOther, heightFieldPose.Inverse * otherGeometricObject.Pose);
float originX = heightField.OriginX * scaleHeightField.X;
float originZ = heightField.OriginZ * scaleHeightField.Z;
// ----- Compute the cell indices of the search-space.
// Estimate start and end indices from our search distance.
int xIndexStartEstimated = (int)((aabbOfOther.Minimum.X - currentSearchDistance - originX) / cellWidthX);
int xIndexEndEstimated = (int)((aabbOfOther.Maximum.X + currentSearchDistance - originX) / cellWidthX);
int zIndexStartEstimated = (int)((aabbOfOther.Minimum.Z - currentSearchDistance - originZ) / cellWidthZ);
int zIndexEndEstimated = (int)((aabbOfOther.Maximum.Z + currentSearchDistance - originZ) / cellWidthZ);
// Clamp indices to valid range.
int xIndexMax = arrayLengthX - 2;
int zIndexMax = arrayLengthZ - 2;
int xIndexStart = Math.Max(xIndexStartEstimated, 0);
int xIndexEnd = Math.Min(xIndexEndEstimated, xIndexMax);
int zIndexStart = Math.Max(zIndexStartEstimated, 0);
int zIndexEnd = Math.Min(zIndexEndEstimated, zIndexMax);
// Find collision algorithm for MinkowskiSum vs. other object's shape.
CollisionAlgorithm collisionAlgorithm = CollisionDetection.AlgorithmMatrix[typeof(ConvexShape), otherShape.GetType()];
int numberOfContactsInLastFrame = contactSet.Count;
#endregion
#region ----- Test all height field cells in the search space. -----
// Create several temporary test objects:
// Instead of the original height field geometric object, we test against a shape for each
// height field triangle. For the test shape we "extrude" the triangle under the height field.
// To create the extrusion we "add" a line segment to the triangle using a Minkowski sum.
// TODO: We can make this faster with a special shape that knows that the child poses are Identity (instead of the standard MinkowskiSumShape).
// This special shape could compute its InnerPoint without applying the poses.
var triangleShape = ResourcePools.TriangleShapes.Obtain();
// (Vertices will be set in the loop below.)
var triangleGeometricObject = TestGeometricObject.Create();
triangleGeometricObject.Shape = triangleShape;
var lineSegment = ResourcePools.LineSegmentShapes.Obtain();
lineSegment.Start = Vector3F.Zero;
lineSegment.End = -heightField.Depth * Vector3F.UnitY;
var lineSegmentGeometricObject = TestGeometricObject.Create();
lineSegmentGeometricObject.Shape = lineSegment;
var extrudedTriangleShape = TestMinkowskiSumShape.Create();
extrudedTriangleShape.ObjectA = triangleGeometricObject;
extrudedTriangleShape.ObjectB = lineSegmentGeometricObject;
var extrudedTriangleGeometricObject = TestGeometricObject.Create();
extrudedTriangleGeometricObject.Shape = extrudedTriangleShape;
extrudedTriangleGeometricObject.Pose = heightFieldPose;
var testCollisionObject = ResourcePools.TestCollisionObjects.Obtain();
testCollisionObject.SetInternal(heightFieldCollisionObject, extrudedTriangleGeometricObject);
var testContactSet = swapped ? ContactSet.Create(contactSet.ObjectA, testCollisionObject)
: ContactSet.Create(testCollisionObject, contactSet.ObjectB);
testContactSet.IsPerturbationTestAllowed = false;
// We compute closest points with a preferred normal direction: the height field up-axis.
// Loop over the cells in the search space.
// (The inner loop can reduce the search space. Therefore, when we increment the indices
// xIndex and zIndex we also check if the start indices have changed.)
for (int xIndex = xIndexStart; xIndex <= xIndexEnd; xIndex = Math.Max(xIndexStart, xIndex + 1))
{
for (int zIndex = zIndexStart; zIndex <= zIndexEnd; zIndex = Math.Max(zIndexStart, zIndex + 1))
{
// Test the two cell triangles.
for (int triangleIndex = 0; triangleIndex < 2; triangleIndex++)
{
// Get triangle 0 or 1.
var triangle = heightField.GetTriangle(xIndex, zIndex, triangleIndex != 0);
var triangleIsHole = Numeric.IsNaN(triangle.Vertex0.Y * triangle.Vertex1.Y * triangle.Vertex2.Y);
if (triangleIsHole)
{
continue;
}
triangleShape.Vertex0 = triangle.Vertex0 * scaleHeightField;
triangleShape.Vertex1 = triangle.Vertex1 * scaleHeightField;
triangleShape.Vertex2 = triangle.Vertex2 * scaleHeightField;
if (type == CollisionQueryType.Boolean)
{
collisionAlgorithm.ComputeCollision(testContactSet, CollisionQueryType.Boolean);
contactSet.HaveContact = contactSet.HaveContact || testContactSet.HaveContact;
if (contactSet.HaveContact)
{
// We can stop tests here for boolean queries.
// Update end indices to exit the outer loops.
xIndexEnd = -1;
zIndexEnd = -1;
break;
}
}
else
{
Debug.Assert(testContactSet.Count == 0, "testContactSet needs to be cleared.");
// If we know that we have a contact, then we can make a faster contact query
// instead of a closest-point query.
CollisionQueryType queryType = (contactSet.HaveContact) ? CollisionQueryType.Contacts : type;
collisionAlgorithm.ComputeCollision(testContactSet, queryType);
if (testContactSet.HaveContact)
{
contactSet.HaveContact = true;
if (testContactSet.Count > 0)
{
// Get neighbor triangle.
// To compute the triangle normal we take the normal of the unscaled triangle and transform
// the normal with: (M^-1)^T = 1 / scale
// Note: We cannot use the scaled vertices because negative scalings change the
// face-order of the vertices.
Vector3F triangleNormal = triangle.Normal / scaleHeightField;
triangleNormal = heightFieldPose.ToWorldDirection(triangleNormal);
triangleNormal.TryNormalize();
// Assuming the last contact is the newest. (With closest-point queries
// and the CombinedCollisionAlgo, testContactSet[0] could be a (not so useful)
// closest-point result, and testContactSet[1] the better contact query result.)
var testContact = testContactSet[testContactSet.Count - 1];
var contactNormal = swapped ? -testContact.Normal : testContact.Normal;
if (Vector3F.Dot(contactNormal, triangleNormal) < WeldingLimit)
{
// Contact normal deviates by more than the welding limit. --> Check the contact.
// If we do not find a neighbor, we assume the neighbor has the same normal.
var neighborNormal = triangleNormal;
#region ----- Get Neighbor Triangle Normal -----
// Get barycentric coordinates of contact position.
Vector3F contactPositionOnHeightField = swapped ? testContact.PositionBLocal / scaleHeightField : testContact.PositionALocal / scaleHeightField;
float u, v, w;
// TODO: GetBaryCentricFromPoint computes the triangle normal, which we already know - optimize.
GeometryHelper.GetBarycentricFromPoint(triangle, contactPositionOnHeightField, out u, out v, out w);
// If one coordinate is near 0, the contact is near an edge.
if (u < 0.05f || v < 0.05f || w < 0.05f)
{
if (triangleIndex == 0)
{
if (u < v && u < w)
{
neighborNormal = heightFieldPose.ToWorldDirection(heightField.GetTriangle(xIndex, zIndex, true).Normal / scaleHeightField);
neighborNormal.TryNormalize();
}
else if (v < w)
{
if (zIndex > 0)
{
neighborNormal = heightFieldPose.ToWorldDirection(heightField.GetTriangle(xIndex, zIndex - 1, true).Normal / scaleHeightField);
neighborNormal.TryNormalize();
}
else
{
// The contact is at the border of the whole height field. Set a normal which disables all bad contact filtering.
neighborNormal = new Vector3F(float.NaN);
}
}
else
{
if (xIndex > 0)
{
neighborNormal = heightFieldPose.ToWorldDirection(heightField.GetTriangle(xIndex - 1, zIndex, true).Normal / scaleHeightField);
neighborNormal.TryNormalize();
}
else
{
neighborNormal = new Vector3F(float.NaN);
}
}
}
else
{
if (u < v && u < w)
{
if (xIndex + 2 < arrayLengthX)
{
neighborNormal = heightFieldPose.ToWorldDirection(heightField.GetTriangle(xIndex + 1, zIndex, false).Normal / scaleHeightField);
neighborNormal.TryNormalize();
}
else
{
neighborNormal = new Vector3F(float.NaN);
}
}
else if (v < w)
{
neighborNormal = heightFieldPose.ToWorldDirection(heightField.GetTriangle(xIndex, zIndex, false).Normal / scaleHeightField);
neighborNormal.TryNormalize();
}
else
{
if (zIndex + 2 < arrayLengthZ)
{
neighborNormal = heightFieldPose.ToWorldDirection(heightField.GetTriangle(xIndex, zIndex + 1, true).Normal / scaleHeightField);
neighborNormal.TryNormalize();
}
else
{
neighborNormal = new Vector3F(float.NaN);
}
}
}
}
#endregion
// Contact normals in the range triangleNormal - neighborNormal are allowed.
// Others, especially vertical contacts in slopes or horizontal normals are not
// allowed.
var cosMinAngle = Vector3F.Dot(neighborNormal, triangleNormal) - CollisionDetection.Epsilon;
RemoveBadContacts(swapped, testContactSet, triangleNormal, cosMinAngle);
// If we have no contact yet, we retry with a preferred normal identical to the up axis.
// (Note: contactSet.Count will be > 0 for closest-point queries but will
// probably constraint separated contacts and not real contacts.)
if (testContactSet.Count == 0 &&
(contactSet.Count == 0 || type == CollisionQueryType.ClosestPoints))
{
testContactSet.PreferredNormal = (swapped) ? -heightFieldUpAxis : heightFieldUpAxis;
collisionAlgorithm.ComputeCollision(testContactSet, CollisionQueryType.Contacts);
testContactSet.PreferredNormal = Vector3F.Zero;
RemoveBadContacts(swapped, testContactSet, triangleNormal, cosMinAngle);
}
// If we have no contact yet, we retry with a preferred normal identical to the triangle normal.
// But only if the triangle normal differs significantly from the up axis.
if (testContactSet.Count == 0 &&
(contactSet.Count == 0 || type == CollisionQueryType.ClosestPoints) &&
Vector3F.Dot(heightFieldUpAxis, triangleNormal) < WeldingLimit)
{
testContactSet.PreferredNormal = (swapped) ? -triangleNormal : triangleNormal;
collisionAlgorithm.ComputeCollision(testContactSet, CollisionQueryType.Contacts);
testContactSet.PreferredNormal = Vector3F.Zero;
RemoveBadContacts(swapped, testContactSet, triangleNormal, cosMinAngle);
}
}
}
}
if (testContactSet.Count > 0)
{
// Remember separation distance for later.
float separationDistance = -testContactSet[0].PenetrationDepth;
// Set the shape feature of the new contacts.
// The features is the height field triangle index (see HeightField documentation).
int numberOfContacts = testContactSet.Count;
for (int i = 0; i < numberOfContacts; i++)
{
Contact contact = testContactSet[i];
int featureIndex = (zIndex * (arrayLengthX - 1) + xIndex) * 2 + triangleIndex;
if (swapped)
{
contact.FeatureB = featureIndex;
}
else
{
contact.FeatureA = featureIndex;
}
}
// Merge the contact info. (Contacts in testContactSet are recycled!)
ContactHelper.Merge(contactSet, testContactSet, type, CollisionDetection.ContactPositionTolerance);
#region ----- Update search space -----
// Update search space if possible.
// The best search distance is 0. For separation we can use the current smallest
// separation as search distance. As soon as we have a contact, we set the
// search distance to 0.
if (currentSearchDistance > 0 && // No need to update search space if search distance is already 0.
(contactSet.HaveContact || // If we have a contact, we set the search distance to 0.
separationDistance < currentSearchDistance)) // If we have closer separation, we use this.
{
// Note: We only check triangleContactSet[0] in the if condition.
// triangleContactSet could contain several contacts, but we don't bother with
// this special case.
// Update search distance.
if (contactSet.HaveContact)
{
currentSearchDistance = 0;
}
else
{
currentSearchDistance = Math.Max(0, separationDistance);
}
// Update search space indices.
xIndexStartEstimated = (int)((aabbOfOther.Minimum.X - currentSearchDistance - originX) / cellWidthX);
xIndexEndEstimated = (int)((aabbOfOther.Maximum.X + currentSearchDistance - originX) / cellWidthX);
zIndexStartEstimated = (int)((aabbOfOther.Minimum.Z - currentSearchDistance - originZ) / cellWidthZ);
zIndexEndEstimated = (int)((aabbOfOther.Maximum.Z + currentSearchDistance - originZ) / cellWidthZ);
xIndexStart = Math.Max(xIndexStart, xIndexStartEstimated);
xIndexEnd = Math.Min(xIndexEndEstimated, xIndexMax);
zIndexStart = Math.Max(zIndexStart, zIndexStartEstimated);
zIndexEnd = Math.Min(zIndexEndEstimated, zIndexMax);
}
#endregion
}
}
}
}
}
// Recycle temporary objects.
testContactSet.Recycle();
ResourcePools.TestCollisionObjects.Recycle(testCollisionObject);
extrudedTriangleGeometricObject.Recycle();
extrudedTriangleShape.Recycle();
lineSegmentGeometricObject.Recycle();
ResourcePools.LineSegmentShapes.Recycle(lineSegment);
triangleGeometricObject.Recycle();
ResourcePools.TriangleShapes.Recycle(triangleShape);
#endregion
#region ----- Handle missing contact info -----
if (contactSet.Count == 0 &&
(contactSet.HaveContact && type == CollisionQueryType.Contacts || type == CollisionQueryType.ClosestPoints))
{
// ----- Bad contact info:
// We should have contact data because this is either a contact query and the objects touch
// or this is a closest-point query.
// Use our guess as the contact info.
Contact closestPair = guessedClosestPair;
bool isOverHole = false;
if (closestPair == null)
{
closestPair = GuessClosestPair(contactSet, swapped, out isOverHole);
}
// Guesses over holes are useless. :-(
if (!isOverHole)
{
ContactHelper.Merge(contactSet, closestPair, type, CollisionDetection.ContactPositionTolerance);
}
}
#endregion
if (CollisionDetection.FullContactSetPerFrame &&
type == CollisionQueryType.Contacts &&
numberOfContactsInLastFrame == 0 &&
contactSet.Count > 0 &&
contactSet.Count < 4)
{
// Try to find full contact set.
// TODO: This can be optimized by not doing the whole overhead of ComputeCollision again.
ContactHelper.TestWithPerturbations(
CollisionDetection,
contactSet,
!swapped, // Perturb objectB not the height field.
_computeContactsMethod);
}
}
protected override void OnHandleInput(InputContext context)
{
if (_cameraObject.CameraNode == null)
{
return;
}
// The input context contains the mouse position that is used by the UI controls of this
// screen. The mouse position is stored in the following properties:
// - context.ScreenMousePosition
// - context.ScreenMousePositionDelta
// - context.MousePosition
// - context.MousePositionDelta
//
// Currently, these properties contain the mouse position relative to the game window.
// But the mouse position of the in-game screen is determined by the reticle of the
// game camera. We need to make a ray-cast to see which part of the screen is hit and
// override the properties.
bool screenHit = false;
// Get the camera position and the view direction in world space.
Vector3F cameraPosition = _cameraObject.CameraNode.PoseWorld.Position;
Vector3F cameraDirection = _cameraObject.CameraNode.PoseWorld.ToWorldDirection(Vector3F.Forward);
// Create a ray (ideally this shape should be cached and reused).
var ray = new RayShape(cameraPosition, cameraDirection, 1000);
// We are only interested in the first object that is hit by the ray.
ray.StopsAtFirstHit = true;
// Create a collision object for this shape.
var rayCollisionObject = new CollisionObject(new GeometricObject(ray, Pose.Identity));
// Use the CollisionDomain of the physics simulation to perform a ray cast.
ContactSet contactSet = _simulation.CollisionDomain.GetContacts(rayCollisionObject).FirstOrDefault();
if (contactSet != null && contactSet.Count > 0)
{
// We have hit something :-)
// Get the contact information of the ray hit.
Contact contact = contactSet[0];
// Get the hit object (one object in the contact set is the ray and the other object is the hit object).
CollisionObject hitCollisionObject = (contactSet.ObjectA == rayCollisionObject) ? contactSet.ObjectB : contactSet.ObjectA;
RigidBody hitBody = hitCollisionObject.GeometricObject as RigidBody;
if (hitBody != null && hitBody.UserData is string && (string)hitBody.UserData == "TV")
{
// We have hit a dynamic rigid body of a TV object.
// Get the normal vector of the contact.
var normal = (contactSet.ObjectA == rayCollisionObject) ? -contact.Normal : contact.Normal;
// Convert the normal vector to the local space of the TV box.
normal = hitBody.Pose.ToLocalDirection(normal);
// The InGameUIScreen texture is only mapped onto the -Y sides of the boxes. If the user
// looks onto another side, he cannot interact with the game screen.
if (normal.Y < 0.5f)
{
// The user looks onto the TV's front side. Now, we have to map the ray hit position
// to the texture coordinate of the InGameUIScreen render target/texture.
var localHitPosition = (contactSet.ObjectA == rayCollisionObject) ? contact.PositionBLocal : contact.PositionALocal;
var normalizedPosition = GetTextureCoordinate(localHitPosition);
// The texture coordinate is in the range [0, 0] to [1, 1]. If we multiply it with the
// screen extent to the position in pixels.
var inGameScreenMousePosition = normalizedPosition * new Vector2F(ActualWidth, ActualHeight);
var inGameScreenMousePositionDelta = inGameScreenMousePosition - _lastMousePosition;
// Finally, we can set the mouse positions that are relative to the InGame screen. Hurray!
context.ScreenMousePosition = inGameScreenMousePosition;
context.ScreenMousePositionDelta = inGameScreenMousePositionDelta;
context.MousePosition = inGameScreenMousePosition;
context.MousePositionDelta = inGameScreenMousePositionDelta;
// Store the mouse position so that we can compute MousePositionDelta in the next frame.
_lastMousePosition = context.MousePosition;
screenHit = true;
}
}
}
if (screenHit)
{
// Call base class to call HandleInput for all child controls. The child controls will
// use the overridden mouse positions.
base.OnHandleInput(context);
}
}
public static Matrix4F Translation(Vector3F v)
{
return(new Matrix4F(1f, 0.0f, 0.0f, v.X, 0.0f, 1f, 0.0f, v.Y, 0.0f, 0.0f, 1f, v.Z, 0.0f, 0.0f, 0.0f, 1f));
}
public static Matrix4F Scaling(Vector3F v, Point3F origin)
{
Vector3F v1 = (Vector3F)origin;
return(Transformation4F.Translation(v1) * Transformation4F.Scaling(v) * Transformation4F.Translation(-v1));
}
public static Point3D ToPoint3D(this Vector3F value) => new Point3D(value.X, value.Y, value.Z);
//--------------------------------------------------------------
#region Methods
//--------------------------------------------------------------
/// <summary>
/// Gets the cube map indices from a direction vector.
/// </summary>
/// <param name="direction">The direction vector.</param>
/// <param name="x">The x index.</param>
/// <param name="y">The y index.</param>
/// <param name="face">The face index.</param>
private void GetIndices(ref Vector3F direction, out int x, out int y, out int face)
{
// Precompute factor used for lookup.
int width = CubeMap.GetLength(1);
float s = 0.5f * width;
// Convert the direction vector to indices.
Vector3F abs = Vector3F.Absolute(direction);
if (abs.X >= abs.Y && abs.X >= abs.Z)
{
float oneOverX = 1.0f / abs.X;
if (direction.X >= 0)
{
x = (int)((direction.Z * oneOverX + 1.0f) * s);
y = (int)((direction.Y * oneOverX + 1.0f) * s);
face = PositiveX;
}
else
{
x = (int)((-direction.Z * oneOverX + 1.0f) * s);
y = (int)((direction.Y * oneOverX + 1.0f) * s);
face = NegativeX;
}
}
else if (abs.Y >= abs.Z)
{
float oneOverY = 1.0f / abs.Y;
if (direction.Y >= 0)
{
x = (int)((direction.X * oneOverY + 1.0f) * s);
y = (int)((direction.Z * oneOverY + 1.0f) * s);
face = PositiveY;
}
else
{
x = (int)((-direction.X * oneOverY + 1.0f) * s);
y = (int)((direction.Z * oneOverY + 1.0f) * s);
face = NegativeY;
}
}
else
{
float oneOverZ = 1.0f / abs.Z;
if (direction.Z >= 0)
{
x = (int)((-direction.X * oneOverZ + 1.0f) * s);
y = (int)((direction.Y * oneOverZ + 1.0f) * s);
face = PositiveZ;
}
else
{
x = (int)((direction.X * oneOverZ + 1.0f) * s);
y = (int)((direction.Y * oneOverZ + 1.0f) * s);
face = NegativeZ;
}
}
// Clamp indices. (Necessary if direction points exactly to edge.)
int maxIndex = width - 1;
if (x > maxIndex)
{
x = maxIndex;
}
if (y > maxIndex)
{
y = maxIndex;
}
}
//--------------------------------------------------------------
#region Properties & Events
//--------------------------------------------------------------
#endregion
//--------------------------------------------------------------
#region Creation & Cleanup
//--------------------------------------------------------------
#endregion
//--------------------------------------------------------------
#region Methods
//--------------------------------------------------------------
/// <summary>
/// Initializes the 1-dimensional constraint.
/// </summary>
public void Prepare(RigidBody bodyA, RigidBody bodyB, Vector3F jLinA, Vector3F jAngA, Vector3F jLinB, Vector3F jAngB)
{
JLinA = jLinA;
JAngA = jAngA;
JLinB = jLinB;
JAngB = jAngB;
//WJTLinA = bodyA.MassInverse * jLinA;
//WJTAngA = bodyA.InertiaInverseWorld * jAngA;
//WJTLinB = bodyB.MassInverse * jLinB;
//WJTAngB = bodyB.InertiaInverseWorld * jAngB;
// ----- Optimized version:
WJTLinA.X = bodyA.MassInverse * jLinA.X;
WJTLinA.Y = bodyA.MassInverse * jLinA.Y;
WJTLinA.Z = bodyA.MassInverse * jLinA.Z;
Matrix33F matrix = bodyA.InertiaInverseWorld;
WJTAngA.X = matrix.M00 * jAngA.X + matrix.M01 * jAngA.Y + matrix.M02 * jAngA.Z;
WJTAngA.Y = matrix.M10 * jAngA.X + matrix.M11 * jAngA.Y + matrix.M12 * jAngA.Z;
WJTAngA.Z = matrix.M20 * jAngA.X + matrix.M21 * jAngA.Y + matrix.M22 * jAngA.Z;
WJTLinB.X = bodyB.MassInverse * jLinB.X;
WJTLinB.Y = bodyB.MassInverse * jLinB.Y;
WJTLinB.Z = bodyB.MassInverse * jLinB.Z;
matrix = bodyB.InertiaInverseWorld;
WJTAngB.X = matrix.M00 * jAngB.X + matrix.M01 * jAngB.Y + matrix.M02 * jAngB.Z;
WJTAngB.Y = matrix.M10 * jAngB.X + matrix.M11 * jAngB.Y + matrix.M12 * jAngB.Z;
WJTAngB.Z = matrix.M20 * jAngB.X + matrix.M21 * jAngB.Y + matrix.M22 * jAngB.Z;
//float JWJT = Vector3F.Dot(jLinA, WJTLinA) + Vector3F.Dot(jAngA, WJTAngA)
// + Vector3F.Dot(jLinB, WJTLinB) + Vector3F.Dot(jAngB, WJTAngB);
// ----- Optimized version:
float JWJT = jLinA.X * WJTLinA.X + jLinA.Y * WJTLinA.Y + jLinA.Z * WJTLinA.Z
+ jAngA.X * WJTAngA.X + jAngA.Y * WJTAngA.Y + jAngA.Z * WJTAngA.Z
+ jLinB.X * WJTLinB.X + jLinB.Y * WJTLinB.Y + jLinB.Z * WJTLinB.Z
+ jAngB.X * WJTAngB.X + jAngB.Y * WJTAngB.Y + jAngB.Z * WJTAngB.Z;
JWJT += Softness;
JWJTInverse = 1 / JWJT;
}
/// <summary>
/// Initializes a new instance of the <see cref="Triangle"/> class.
/// </summary>
/// <param name="p0">A <see cref="Vector3F"/> instance.</param>
/// <param name="p1">A <see cref="Vector3F"/> instance.</param>
/// <param name="p2">A <see cref="Vector3F"/> instance.</param>
public Triangle(Vector3F p0, Vector3F p1, Vector3F p2)
{
_p0 = p0;
_p1 = p1;
_p2 = p2;
}
public static Matrix4F GetCoordSystem(Vector3F xaxis, Vector3F yaxis, Vector3F zaxis)
{
return(new Matrix4F(xaxis.X, yaxis.X, zaxis.X, 0.0f, xaxis.Y, yaxis.Y, zaxis.Y, 0.0f, xaxis.Z, yaxis.Z, zaxis.Z, 0.0f, 0.0f, 0.0f, 0.0f, 1f));
}
public static Matrix4F Scaling(Vector3F v)
{
return(new Matrix4F(v.X, 0.0f, 0.0f, 0.0f, 0.0f, v.Y, 0.0f, 0.0f, 0.0f, 0.0f, v.Z, 0.0f, 0.0f, 0.0f, 0.0f, 1f));
}
private void SetupConstraint(int index, float targetVelocity, Vector3F axis, Vector3F rA, Vector3F rB, float deltaTime)
{
Constraint1D constraint = _constraints[index];
constraint.TargetRelativeVelocity = targetVelocity;
// Impulse limits
float impulseLimit = MaxForce * deltaTime;
_minImpulseLimits[index] = -impulseLimit;
_maxImpulseLimits[index] = impulseLimit;
// Note: Softness must be set before!
constraint.Softness = Softness / deltaTime;
constraint.Prepare(BodyA, BodyB, -axis, -Vector3F.Cross(rA, axis), axis, Vector3F.Cross(rB, axis));
}
public static Matrix4F Scaling(float s, Point3F origin)
{
Vector3F v = (Vector3F)origin;
return(Transformation4F.Translation(v) * Transformation4F.Scaling(s) * Transformation4F.Translation(-v));
}
private void CreateScene()
{
var random = new Random(1234567);
// 3 empty scene nodes act as the root nodes for 3 different scene graphs.
_originalMeshNodes = new SceneNode {
Children = new SceneNodeCollection()
};
_staticInstancingNodes = new SceneNode {
Children = new SceneNodeCollection()
};
_staticBatchingNodes = new SceneNode {
Children = new SceneNodeCollection()
};
// Load several model nodes. Each of these models contains only one mesh node.
// Store the mesh nodes in a list.
MeshNode[] meshNodePrototypes =
{
(MeshNode)ContentManager.Load <ModelNode>("Grass/Grass").Children[0],
(MeshNode)ContentManager.Load <ModelNode>("Parviflora/Parviflora").Children[0],
(MeshNode)ContentManager.Load <ModelNode>("PalmTree/palm_tree").Children[0],
(MeshNode)ContentManager.Load <ModelNode>("Rock/rock_05").Children[0],
(MeshNode)ContentManager.Load <ModelNode>("GlassBox/GlassBox").Children[0],
(MeshNode)ContentManager.Load <ModelNode>("Building/wall_concrete_1").Children[0],
};
for (int meshIndex = 0; meshIndex < meshNodePrototypes.Length; meshIndex++)
{
var meshNode = meshNodePrototypes[meshIndex];
var mesh = meshNode.Mesh;
#if WINDOWS
int numberOfInstances = (meshIndex == 0) ? 5000 : 200;
#else
int numberOfInstances = (meshIndex == 0) ? 1000 : 50;
#endif
int extent = (meshIndex < 2) ? 20 : 100;
// Create a list of random scales and poses.
var scales = new Vector3F[numberOfInstances];
var poses = new Pose[numberOfInstances];
for (int i = 0; i < numberOfInstances; i++)
{
// Combine a random scale with the original scale of the mesh.
scales[i] = new Vector3F(random.NextFloat(0.5f, 1.2f)) * meshNode.ScaleWorld;
// Combine a random pose with the original pose of the mesh.
Vector3F position = new Vector3F(random.NextFloat(-extent, extent), 0, random.NextFloat(-extent, extent));
Matrix33F orientation = Matrix33F.CreateRotationY(random.NextFloat(0, ConstantsF.TwoPi));
poses[i] = new Pose(position, orientation) * meshNode.PoseLocal;
}
// Strategy A or B: Create a MeshNode for each mesh instance.
for (int i = 0; i < numberOfInstances; i++)
{
var clone = meshNode.Clone();
clone.ScaleLocal = scales[i];
clone.PoseLocal = poses[i];
_originalMeshNodes.Children.Add(clone);
}
// Strategy C: Create one MeshInstancingNode which contains all instances for one mesh.
var instances = new InstanceData[numberOfInstances];
for (int i = 0; i < numberOfInstances; i++)
{
instances[i] = new InstanceData(scales[i], poses[i], new Vector4F(1));
}
// Create MeshInstancingNode.
var instancingMeshNode = new MeshInstancingNode <InstanceData>(mesh, instances)
{
// It is recommended to manually set a suitable pose and shape, so that
// the bounding shape contains all instances.
PoseLocal = new Pose(new Vector3F(0, 2, 0)),
Shape = new BoxShape(2 * extent, 4, 2 * extent),
};
_staticInstancingNodes.Children.Add(instancingMeshNode);
// Strategy D: We merge all instances of the mesh into one huge static mesh.
var mergedMesh = MeshHelper.Merge(mesh, scales, poses);
_staticBatchingNodes.Children.Add(new MeshNode(mergedMesh));
}
// For static batching, instead of creating one merged mesh per mesh type,
// we could also merge all mesh instances into a single huge static mesh.
//var mergedMesh = MeshHelper.Merge(_originalMeshNodes.Children);
//_staticBatchingNodes = new MeshNode(mergedMesh);
}
public override void Render(IList <SceneNode> nodes, RenderContext context, RenderOrder order)
{
ThrowIfDisposed();
if (nodes == null)
{
throw new ArgumentNullException("nodes");
}
if (context == null)
{
throw new ArgumentNullException("context");
}
int numberOfNodes = nodes.Count;
if (nodes.Count == 0)
{
return;
}
context.Validate(_spriteBatch);
context.ThrowIfCameraMissing();
var graphicsDevice = context.GraphicsService.GraphicsDevice;
var savedRenderState = new RenderStateSnapshot(graphicsDevice);
// Camera properties
var cameraNode = context.CameraNode;
Matrix44F viewProjection = cameraNode.Camera.Projection * cameraNode.View;
var viewport = graphicsDevice.Viewport;
// Update SceneNode.LastFrame for all visible nodes.
int frame = context.Frame;
cameraNode.LastFrame = frame;
SpriteSortMode sortMode;
switch (order)
{
case RenderOrder.Default:
sortMode = SpriteSortMode.Texture;
break;
case RenderOrder.FrontToBack:
sortMode = SpriteSortMode.FrontToBack;
break;
case RenderOrder.BackToFront:
sortMode = SpriteSortMode.BackToFront;
break;
case RenderOrder.UserDefined:
default:
sortMode = SpriteSortMode.Deferred;
break;
}
_spriteBatch.Begin(sortMode, graphicsDevice.BlendState, null, graphicsDevice.DepthStencilState, null);
for (int i = 0; i < numberOfNodes; i++)
{
var node = nodes[i] as SpriteNode;
if (node == null)
{
continue;
}
// SpriteNode is visible in current frame.
node.LastFrame = frame;
// Position, size, and origin in pixels.
Vector3F position = new Vector3F();
Vector2 size = new Vector2();
Vector2 origin = new Vector2();
var bitmapSprite = node.Sprite as ImageSprite;
if (bitmapSprite != null)
{
var packedTexture = bitmapSprite.Texture;
if (packedTexture != null)
{
// Project into viewport and snap to pixels.
position = viewport.ProjectToViewport(node.PoseWorld.Position, viewProjection);
position.X = (int)(position.X + 0.5f);
position.Y = (int)(position.Y + 0.5f);
// Get source rectangle (pixel bounds).
var sourceRectangle = packedTexture.GetBounds(node.AnimationTime);
size = new Vector2(sourceRectangle.Width, sourceRectangle.Height);
// Premultiply color.
Vector3F color3F = node.Color;
float alpha = node.Alpha;
Color color = new Color(color3F.X * alpha, color3F.Y * alpha, color3F.Z * alpha, alpha);
// Get absolute origin (relative to pixel bounds).
origin = (Vector2)node.Origin * size;
// Draw using SpriteBatch.
_spriteBatch.Draw(
packedTexture.TextureAtlas, new Vector2(position.X, position.Y), sourceRectangle,
color, node.Rotation, origin, (Vector2)node.Scale, SpriteEffects.None, position.Z);
}
}
else
{
var textSprite = node.Sprite as TextSprite;
if (textSprite != null)
{
var font = textSprite.Font ?? _spriteFont;
if (font != null)
{
// Text can be a string or StringBuilder.
var text = textSprite.Text as string;
if (text != null)
{
if (text.Length > 0)
{
// Project into viewport and snap to pixels.
position = viewport.ProjectToViewport(node.PoseWorld.Position, viewProjection);
position.X = (int)(position.X + 0.5f);
position.Y = (int)(position.Y + 0.5f);
// Premultiply color.
Vector3F color3F = node.Color;
float alpha = node.Alpha;
Color color = new Color(color3F.X * alpha, color3F.Y * alpha, color3F.Z * alpha, alpha);
// Get absolute origin (relative to pixel bounds).
size = font.MeasureString(text);
origin = (Vector2)node.Origin * size;
// Draw using SpriteBatch.
_spriteBatch.DrawString(
font, text, new Vector2(position.X, position.Y),
color, node.Rotation, origin, (Vector2)node.Scale,
SpriteEffects.None, position.Z);
}
}
else
{
var stringBuilder = textSprite.Text as StringBuilder;
if (stringBuilder != null && stringBuilder.Length > 0)
{
// Project into viewport and snap to pixels.
position = viewport.ProjectToViewport(node.PoseWorld.Position, viewProjection);
position.X = (int)(position.X + 0.5f);
position.Y = (int)(position.Y + 0.5f);
// Premultiply color.
Vector3F color3F = node.Color;
float alpha = node.Alpha;
Color color = new Color(color3F.X * alpha, color3F.Y * alpha, color3F.Z * alpha, alpha);
// Get absolute origin (relative to pixel bounds).
size = font.MeasureString(stringBuilder);
origin = (Vector2)node.Origin * size;
// Draw using SpriteBatch.
_spriteBatch.DrawString(
font, stringBuilder, new Vector2(position.X, position.Y),
color, node.Rotation, origin, (Vector2)node.Scale,
SpriteEffects.None, position.Z);
}
}
}
}
}
// Store bounds an depth for hit tests.
node.LastBounds = new Rectangle(
(int)(position.X - origin.X),
(int)(position.Y - origin.Y),
(int)(size.X * node.Scale.X),
(int)(size.Y * node.Scale.Y));
node.LastDepth = position.Z;
}
_spriteBatch.End();
savedRenderState.Restore();
}
/// <summary>
/// Called when a mesh should be generated for the shape.
/// </summary>
/// <param name="absoluteDistanceThreshold">The absolute distance threshold.</param>
/// <param name="iterationLimit">The iteration limit.</param>
/// <returns>The triangle mesh for this shape.</returns>
protected override TriangleMesh OnGetMesh(float absoluteDistanceThreshold, int iterationLimit)
{
// Estimate required segment angle for given accuracy.
// (Easy to derive from simple drawing of a circle segment with a triangle used to represent
// the segment.)
float alpha = (float)Math.Acos((Radius - absoluteDistanceThreshold) / Radius) * 2;
int numberOfSegments = (int)Math.Ceiling(ConstantsF.PiOver2 / alpha) * 4;
// Apply the iteration limit - in case absoluteDistanceThreshold is 0.
// Lets say each iteration doubles the number of segments. This is an arbitrary interpretation
// of the "iteration limit".
numberOfSegments = Math.Min(numberOfSegments, 2 << iterationLimit);
alpha = ConstantsF.TwoPi / numberOfSegments;
TriangleMesh mesh = new TriangleMesh();
// The world space vertices are created by rotating "radius vectors" with this rotations.
QuaternionF rotationY = QuaternionF.CreateRotationY(alpha);
QuaternionF rotationZ = QuaternionF.CreateRotationZ(alpha);
// We use two nested loops: In each loop a "radius vector" is rotated further to get a
// new vertex.
Vector3F rLow = Vector3F.UnitX * Radius; // Radius vector for the lower vertex.
for (int i = 1; i <= numberOfSegments / 4; i++)
{
Vector3F rHigh = rotationZ.Rotate(rLow); // Radius vector for the higher vertex.
// In the inner loop we create lines and triangles between 4 vertices, which are created
// with the radius vectors rLow0, rLow1, rHigh0, rHigh1.
Vector3F rLow0 = rLow;
Vector3F rHigh0 = rHigh;
for (int j = 1; j <= numberOfSegments; j++)
{
Vector3F rLow1 = rotationY.Rotate(rLow0);
Vector3F rHigh1 = rotationY.Rotate(rHigh0);
// Two top hemisphere triangles
mesh.Add(new Triangle
{
Vertex0 = new Vector3F(0, Height / 2 - Radius, 0) + rLow0,
Vertex1 = new Vector3F(0, Height / 2 - Radius, 0) + rLow1,
Vertex2 = new Vector3F(0, Height / 2 - Radius, 0) + rHigh0,
}, false);
if (i < numberOfSegments / 4) // At the "northpole" only a triangle is needed. No quad.
{
mesh.Add(new Triangle
{
Vertex0 = new Vector3F(0, Height / 2 - Radius, 0) + rLow1,
Vertex1 = new Vector3F(0, Height / 2 - Radius, 0) + rHigh1,
Vertex2 = new Vector3F(0, Height / 2 - Radius, 0) + rHigh0,
}, false);
}
// Two bottom hemisphere triangles
mesh.Add(new Triangle
{
Vertex0 = new Vector3F(0, -Height / 2 + Radius, 0) - rLow0,
Vertex1 = new Vector3F(0, -Height / 2 + Radius, 0) - rHigh0,
Vertex2 = new Vector3F(0, -Height / 2 + Radius, 0) - rLow1,
}, false);
if (i < numberOfSegments / 4) // At the "southpole" only a triangle is needed. No quad.
{
mesh.Add(new Triangle
{
Vertex0 = new Vector3F(0, -Height / 2 + Radius, 0) - rLow1,
Vertex1 = new Vector3F(0, -Height / 2 + Radius, 0) - rHigh0,
Vertex2 = new Vector3F(0, -Height / 2 + Radius, 0) - rHigh1,
}, false);
}
// Two side triangles
if (i == 1)
{
mesh.Add(new Triangle
{
Vertex0 = new Vector3F(0, -Height / 2 + Radius, 0) + rLow0,
Vertex1 = new Vector3F(0, -Height / 2 + Radius, 0) + rLow1,
Vertex2 = new Vector3F(0, Height / 2 - Radius, 0) + rLow0,
}, false);
mesh.Add(new Triangle
{
Vertex0 = new Vector3F(0, -Height / 2 + Radius, 0) + rLow1,
Vertex1 = new Vector3F(0, Height / 2 - Radius, 0) + rLow1,
Vertex2 = new Vector3F(0, Height / 2 - Radius, 0) + rLow0,
}, false);
}
rLow0 = rLow1;
rHigh0 = rHigh1;
}
rLow = rHigh;
}
mesh.WeldVertices();
return(mesh);
}
public VsIn(Vector3F position, Vector4F color)
{
Position = position;
Color = color;
}
private static bool TRI_TRI_3D(ref Vector3F p1, ref Vector3F q1, ref Vector3F r1,
ref Vector3F p2, ref Vector3F q2, ref Vector3F r2,
float dp2, float dq2, float dr2, ref Vector3F N1)
{
// Permutation in a canonical form of T2's vertices
if (dp2 > 0.0f)
{
if (dq2 > 0.0f)
{
return(CHECK_MIN_MAX(ref p1, ref r1, ref q1, ref r2, ref p2, ref q2));
}
else if (dr2 > 0.0f)
{
return(CHECK_MIN_MAX(ref p1, ref r1, ref q1, ref q2, ref r2, ref p2));
}
else
{
return(CHECK_MIN_MAX(ref p1, ref q1, ref r1, ref p2, ref q2, ref r2));
}
}
else if (dp2 < 0.0f)
{
if (dq2 < 0.0f)
{
return(CHECK_MIN_MAX(ref p1, ref q1, ref r1, ref r2, ref p2, ref q2));
}
else if (dr2 < 0.0f)
{
return(CHECK_MIN_MAX(ref p1, ref q1, ref r1, ref q2, ref r2, ref p2));
}
else
{
return(CHECK_MIN_MAX(ref p1, ref r1, ref q1, ref p2, ref q2, ref r2));
}
}
else
{
if (dq2 < 0.0f)
{
if (dr2 >= 0.0f)
{
return(CHECK_MIN_MAX(ref p1, ref r1, ref q1, ref q2, ref r2, ref p2));
}
else
{
return(CHECK_MIN_MAX(ref p1, ref q1, ref r1, ref p2, ref q2, ref r2));
}
}
else if (dq2 > 0.0f)
{
if (dr2 > 0.0f)
{
return(CHECK_MIN_MAX(ref p1, ref r1, ref q1, ref p2, ref q2, ref r2));
}
else
{
return(CHECK_MIN_MAX(ref p1, ref q1, ref r1, ref q2, ref r2, ref p2));
}
}
else
{
if (dr2 > 0.0f)
{
return(CHECK_MIN_MAX(ref p1, ref q1, ref r1, ref r2, ref p2, ref q2));
}
else if (dr2 < 0.0f)
{
return(CHECK_MIN_MAX(ref p1, ref r1, ref q1, ref r2, ref p2, ref q2));
}
else
{
return(coplanar_tri_tri3d(ref p1, ref q1, ref r1, ref p2, ref q2, ref r2, ref N1));
}
}
}
}
protected override void OnUpdateParticles(TimeSpan deltaTime, int startIndex, int count)
{
if (_positionParameter == null ||
_targetPositionParameter == null ||
_speedParameter == null ||
_sizeXParameter == null)
{
return;
}
Vector3F[] positions = _positionParameter.Values;
Vector3F[] targetPositions = _targetPositionParameter.Values;
float[] speeds = _speedParameter.Values;
float[] sizes = _sizeXParameter.Values;
Pose cameraPose = (_cameraPoseParameter != null) ? _cameraPoseParameter.DefaultValue : Pose.Identity;
if (positions == null || targetPositions == null || speeds == null)
{
// This effector only works with varying parameters.
return;
}
Random random = ParticleSystem.Random;
Vector3F center = ParticleSystem.Pose.Position;
float dt = (float)deltaTime.TotalSeconds;
for (int i = startIndex; i < startIndex + count; i++)
{
Vector3F targetPosition = targetPositions[i];
Vector3F lineSegment = targetPosition - positions[i];
float distance = lineSegment.Length;
if (Numeric.IsZero(distance))
{
// The bee has reached the target position. Choose a new direction and distance.
Vector3F d = _distribution.Next(random);
// Keep the new target position within a certain range.
if (Math.Abs(targetPosition.X + d.X - center.X) > MaxRange)
{
d.X = -d.X;
}
if (Math.Abs(targetPosition.Y + d.Y - center.Y) > MaxRange)
{
d.Y = -d.Y;
}
if (Math.Abs(targetPosition.Z + d.Z - center.Z) > MaxRange)
{
d.Z = -d.Z;
}
targetPositions[i] = targetPosition + d;
}
else
{
// Move bee towards target position.
Vector3F movementDirection = lineSegment / distance;
float movementDistance = speeds[i] * dt;
if (movementDistance > distance)
{
movementDistance = distance;
}
positions[i] += movementDirection * movementDistance;
// The bee should look in the direction it is flying. Transform the direction
// into the view space and check the sign of x-component.
movementDirection = cameraPose.ToLocalDirection(movementDirection);
float sign = (movementDirection.X >= 0) ? +1 : -1;
if (InvertLookDirection)
{
sign *= -1;
}
if (Math.Sign(sizes[i]) != (int)sign)
{
sizes[i] *= -1;
}
}
}
}
/// <inheritdoc/>
public override Aabb GetAabb(Vector3F scale, Pose pose)
{
return(new Aabb(pose.Position, pose.Position));
}
/// <summary>
/// Computes a mass frame for the given shape and target mass.
/// </summary>
/// <param name="shape">The shape.</param>
/// <param name="scale">The scale of the shape.</param>
/// <param name="mass">
/// The target mass. The mass of the computed <see cref="MassFrame"/> will be equal to this
/// value. Other mass properties are adjusted to match the target mass.
/// </param>
/// <param name="relativeDistanceThreshold">
/// The relative distance threshold. If no mass or inertia formula for the given shape are known
/// the shape is approximated with a triangle mesh and the mass frame of this mesh is returned.
/// The relative distance threshold controls the accuracy of the approximated mesh. Good default
/// values are 0.05 to get an approximation with an error of about 5%.
/// </param>
/// <param name="iterationLimit">
/// The iteration limit. For some shapes the mass properties are computed with an iterative
/// algorithm. No more than <paramref name="iterationLimit"/> iterations will be performed.
/// A value of 3 gives good results in most cases. Use a value of -1 to get only a coarse
/// approximation.
/// </param>
/// <returns>
/// A new <see cref="MassFrame"/> for the given parameters is returned.
/// </returns>
/// <remarks>
/// <para>
/// <strong>Composite shapes:</strong> If the given <paramref name="shape"/> is a
/// <see cref="CompositeShape"/>, the computed mass properties are only correct if the children
/// of the composite shape do not overlap. Overlapping parts are counted twice and so the result
/// will not be correct. If a child of a composite shape is a <see cref="RigidBody"/> the
/// <see cref="MassFrame"/> of the rigid body will be used for this child.
/// </para>
/// <para>
/// <strong>Density:</strong> Since this method does not use a density value, the
/// <see cref="Density"/> property of the <see cref="MassFrame"/> is set to 0.
/// </para>
/// </remarks>
/// <exception cref="ArgumentNullException">
/// <paramref name="shape"/> is <see langword="null"/>.
/// </exception>
/// <exception cref="ArgumentOutOfRangeException">
/// <paramref name="mass"/> is negative or 0.
/// </exception>
/// <exception cref="ArgumentOutOfRangeException">
/// <paramref name="relativeDistanceThreshold"/> is negative.
/// </exception>
public static MassFrame FromShapeAndMass(Shape shape, Vector3F scale, float mass, float relativeDistanceThreshold, int iterationLimit)
{
return(ComputeMassProperties(shape, scale, mass, false, relativeDistanceThreshold, iterationLimit));
}
/// <summary>
/// Computes the mass properties for the given shape and parameters.
/// </summary>
/// <param name="shape">The shape.</param>
/// <param name="scale">The scale.</param>
/// <param name="densityOrMass">The density or target mass value.</param>
/// <param name="isDensity">
/// If set to <see langword="true"/> <paramref name="densityOrMass"/> is interpreted as density;
/// otherwise, <paramref name="densityOrMass"/> is interpreted as the desired target mass.
/// </param>
/// <param name="relativeDistanceThreshold">The relative distance threshold.</param>
/// <param name="iterationLimit">
/// The iteration limit. Can be -1 or 0 to use an approximation.
/// </param>
/// <returns>
/// A new <see cref="MassFrame"/> instance.
/// </returns>
/// <exception cref="ArgumentNullException">
/// <paramref name="shape"/> is <see langword="null"/>.
/// </exception>
/// <exception cref="ArgumentOutOfRangeException">
/// <paramref name="densityOrMass"/> is negative or 0.
/// </exception>
/// <exception cref="ArgumentOutOfRangeException">
/// <paramref name="relativeDistanceThreshold"/> is negative.
/// </exception>
private static MassFrame ComputeMassProperties(Shape shape, Vector3F scale, float densityOrMass, bool isDensity, float relativeDistanceThreshold, int iterationLimit)
{
if (shape == null)
{
throw new ArgumentNullException("shape");
}
if (densityOrMass <= 0)
{
throw new ArgumentOutOfRangeException("densityOrMass", "Density and mass must be greater than 0.");
}
if (relativeDistanceThreshold < 0)
{
throw new ArgumentOutOfRangeException("relativeDistanceThreshold", "Relative distance threshold must not be negative.");
}
// Call MassHelper to compute mass, COM and inertia matrix (not diagonalized).
float mass;
Vector3F centerOfMass;
Matrix33F inertia;
MassHelper.GetMass(shape, scale, densityOrMass, isDensity, relativeDistanceThreshold, iterationLimit,
out mass, out centerOfMass, out inertia);
// If anything is NaN, we use default values for a static/kinematic body.
if (Numeric.IsNaN(mass) || centerOfMass.IsNaN || inertia.IsNaN)
{
return new MassFrame {
Mass = 0, Inertia = Vector3F.Zero
}
}
;
if (!Numeric.IsZero(inertia.M01) || !Numeric.IsZero(inertia.M02) || !Numeric.IsZero(inertia.M12))
{
// Inertia off-diagonal elements are not 0.
// --> Have to make inertia a diagonal matrix.
Vector3F inertiaDiagonal;
Matrix33F rotation;
MassHelper.DiagonalizeInertia(inertia, out inertiaDiagonal, out rotation);
MassFrame massFrame = new MassFrame
{
Mass = mass,
Inertia = inertiaDiagonal,
Pose = new Pose(centerOfMass, rotation),
Density = isDensity ? densityOrMass : 0
};
return(massFrame);
}
else
{
// Inertia is already a diagonal matrix.
MassFrame massFrame = new MassFrame
{
Mass = mass,
Inertia = new Vector3F(inertia.M00, inertia.M11, inertia.M22),
Pose = new Pose(centerOfMass),
Density = isDensity ? densityOrMass : 0
};
return(massFrame);
}
}
internal static global::System.Runtime.InteropServices.HandleRef getCPtr(Vector3F obj)
{
return((obj == null) ? new global::System.Runtime.InteropServices.HandleRef(null, global::System.IntPtr.Zero) : obj.swigCPtr);
}
/// <summary>
/// Computes a mass frame for the given shape and density.
/// </summary>
/// <param name="shape">The shape.</param>
/// <param name="scale">The scale of the shape.</param>
/// <param name="density">The density.</param>
/// <param name="relativeDistanceThreshold">
/// The relative distance threshold. If no mass or inertia formula for the given shape are known
/// the shape is approximated with a triangle mesh and the mass frame of this mesh is returned.
/// The relative distance threshold controls the accuracy of the approximated mesh. Good default
/// values are 0.05 to get an approximation with an error of about 5%.
/// </param>
/// <param name="iterationLimit">
/// The iteration limit. For some shapes the mass properties are computed with an iterative
/// algorithm. No more than <paramref name="iterationLimit"/> iterations will be performed.
/// A value of 3 gives good results in most cases. Use a value of -1 to get only a coarse
/// approximation.
/// </param>
/// <returns>
/// A new <see cref="MassFrame"/> for the given parameters is returned.
/// </returns>
/// <remarks>
/// <strong>Composite shapes:</strong> If the given <paramref name="shape"/> is a
/// <see cref="CompositeShape"/>, the computed mass properties are only correct if the children
/// of the composite shape do not overlap. Overlapping parts are counted twice and so the result
/// will not be correct. If a child of a composite shape is a <see cref="RigidBody"/> the
/// <see cref="MassFrame"/> of the rigid body will be used for this child.
/// </remarks>
/// <exception cref="ArgumentNullException">
/// <paramref name="shape"/> is <see langword="null"/>.
/// </exception>
/// <exception cref="ArgumentOutOfRangeException">
/// <paramref name="density"/> is negative or 0.
/// </exception>
/// <exception cref="ArgumentOutOfRangeException">
/// <paramref name="relativeDistanceThreshold"/> is negative.
/// </exception>
public static MassFrame FromShapeAndDensity(Shape shape, Vector3F scale, float density, float relativeDistanceThreshold, int iterationLimit)
{
return(ComputeMassProperties(shape, scale, density, true, relativeDistanceThreshold, iterationLimit));
}
public VsIn(Vector3F position, Vector2F textureCoordinate)
{
Position = position;
TextureCoordinate = textureCoordinate;
}
/// <summary>
/// Initializes a new instance of the <see cref="Segment"/> class.
/// </summary>
/// <param name="p0">A <see cref="Vector3F"/> instance marking the segment's starting point.</param>
/// <param name="p1">A <see cref="Vector3F"/> instance marking the segment's ending point.</param>
public Segment(Vector3F p0, Vector3F p1)
{
_p0 = p0;
_p1 = p1;
}
/// <summary>
/// Initializes a new instance of the <see cref="Triangle"/> class using a given Triangle.
/// </summary>
/// <param name="t">A <see cref="Triangle"/> instance.</param>
public Triangle(Triangle t)
{
_p0 = t._p0;
_p1 = t._p1;
_p2 = t._p2;
}
/// <inheritdoc/>
/// <exception cref="ArgumentException">
/// Neither <paramref name="objectA"/> nor <paramref name="objectB"/> contains a
/// <see cref="HeightField"/>.
/// </exception>
/// <exception cref="ArgumentNullException">
/// <paramref name="objectA"/> or <paramref name="objectB"/> is <see langword="null"/>.
/// </exception>
/// <exception cref="NotSupportedException">
/// <paramref name="objectA"/> or <paramref name="objectB"/> contains a
/// <see cref="HeightField"/> with a negative scaling. Computing collisions for height fields
/// with a negative scaling is not supported.
/// </exception>
public override float GetTimeOfImpact(CollisionObject objectA, Pose targetPoseA, CollisionObject objectB, Pose targetPoseB, float allowedPenetration)
{
if (objectA == null)
{
throw new ArgumentNullException("objectA");
}
if (objectB == null)
{
throw new ArgumentNullException("objectB");
}
// Object A should be the height field, swap objects if necessary.
if (!(objectA.GeometricObject.Shape is HeightField))
{
MathHelper.Swap(ref objectA, ref objectB);
MathHelper.Swap(ref targetPoseA, ref targetPoseB);
}
IGeometricObject geometricObjectA = objectA.GeometricObject;
IGeometricObject geometricObjectB = objectB.GeometricObject;
HeightField heightFieldA = geometricObjectA.Shape as HeightField;
// Check if collision object shapes are correct.
if (heightFieldA == null)
{
throw new ArgumentException("One object must be a height field.");
}
// Height field vs height field makes no sense.
if (objectB.GeometricObject.Shape is HeightField)
{
return(1);
}
Pose startPoseA = geometricObjectA.Pose;
Pose startPoseB = geometricObjectB.Pose;
Vector3F scaleA = geometricObjectA.Scale;
Vector3F scaleB = geometricObjectB.Scale;
// We do not support negative scaling (see comments in ComputeCollision).
if (scaleA.X < 0 || scaleA.Y < 0 || scaleA.Z < 0)
{
throw new NotSupportedException("Computing collisions for height fields with a negative scaling is not supported.");
}
// Get an AABB of the swept B in the space of A.
// This simplified AABB can miss some rotational movement.
// To simplify, we assume that A is static and B is moving relative to A.
// In general, this is not correct! But for CCD we make this simplification.
// We convert everything to the space of A.
var aabbSweptBInA = geometricObjectB.Shape.GetAabb(scaleB, startPoseA.Inverse * startPoseB);
aabbSweptBInA.Grow(geometricObjectB.Shape.GetAabb(scaleB, targetPoseA.Inverse * targetPoseB));
// Use temporary object.
var triangleShape = ResourcePools.TriangleShapes.Obtain();
// (Vertices will be set in the loop below.)
var testGeometricObject = TestGeometricObject.Create();
testGeometricObject.Shape = triangleShape;
testGeometricObject.Scale = Vector3F.One;
testGeometricObject.Pose = startPoseA;
var testCollisionObject = ResourcePools.TestCollisionObjects.Obtain();
testCollisionObject.SetInternal(objectA, testGeometricObject);
var collisionAlgorithm = CollisionDetection.AlgorithmMatrix[typeof(TriangleShape), geometricObjectB.Shape.GetType()];
// Get height field and basic info.
int arrayLengthX = heightFieldA.NumberOfSamplesX;
int arrayLengthZ = heightFieldA.NumberOfSamplesZ;
Debug.Assert(arrayLengthX > 1 && arrayLengthZ > 1, "A height field should contain at least 2 x 2 elements (= 1 cell).");
float cellWidthX = heightFieldA.WidthX * scaleA.X / (arrayLengthX - 1);
float cellWidthZ = heightFieldA.WidthZ * scaleA.Z / (arrayLengthZ - 1);
float originX = heightFieldA.OriginX;
float originZ = heightFieldA.OriginZ;
// ----- Compute the cell indices for the AABB.
// Estimate start and end indices from our search distance.
int xIndexStartEstimated = (int)((aabbSweptBInA.Minimum.X - originX) / cellWidthX);
int xIndexEndEstimated = (int)((aabbSweptBInA.Maximum.X - originX) / cellWidthX);
int zIndexStartEstimated = (int)((aabbSweptBInA.Minimum.Z - originZ) / cellWidthZ);
int zIndexEndEstimated = (int)((aabbSweptBInA.Maximum.Z - originZ) / cellWidthZ);
// Clamp indices to valid range.
int xIndexMax = arrayLengthX - 2;
int zIndexMax = arrayLengthZ - 2;
int xIndexStart = Math.Max(xIndexStartEstimated, 0);
int xIndexEnd = Math.Min(xIndexEndEstimated, xIndexMax);
int zIndexStart = Math.Max(zIndexStartEstimated, 0);
int zIndexEnd = Math.Min(zIndexEndEstimated, zIndexMax);
float timeOfImpact = 1;
for (int xIndex = xIndexStart; xIndex <= xIndexEnd; xIndex = Math.Max(xIndexStart, xIndex + 1))
{
for (int zIndex = zIndexStart; zIndex <= zIndexEnd; zIndex = Math.Max(zIndexStart, zIndex + 1))
{
// Test the two cell triangles.
for (int triangleIndex = 0; triangleIndex < 2; triangleIndex++)
{
// Get triangle 0 or 1.
var triangle = heightFieldA.GetTriangle(xIndex, zIndex, triangleIndex != 0);
// Apply scale.
triangle.Vertex0 = triangle.Vertex0 * scaleA;
triangle.Vertex1 = triangle.Vertex1 * scaleA;
triangle.Vertex2 = triangle.Vertex2 * scaleA;
// Make AABB test of triangle vs. sweep of B.
if (!GeometryHelper.HaveContact(aabbSweptBInA, triangle.Aabb))
{
continue;
}
triangleShape.Vertex0 = triangle.Vertex0;
triangleShape.Vertex1 = triangle.Vertex1;
triangleShape.Vertex2 = triangle.Vertex2;
float triangleTimeOfImpact = collisionAlgorithm.GetTimeOfImpact(
testCollisionObject,
targetPoseA,
objectB,
targetPoseB,
allowedPenetration);
timeOfImpact = Math.Min(timeOfImpact, triangleTimeOfImpact);
}
}
}
// Recycle temporary objects.
ResourcePools.TestCollisionObjects.Recycle(testCollisionObject);
testGeometricObject.Recycle();
ResourcePools.TriangleShapes.Recycle(triangleShape);
return(timeOfImpact);
}
