public override void Update(GameTime gameTime)
{
// Update _time.
_time += (float)gameTime.ElapsedGameTime.TotalSeconds;
var debugRenderer = GraphicsScreen.DebugRenderer;
debugRenderer.Clear();
// Draw the path key points.
foreach (var point in _path.Select(key => key.Point))
{
debugRenderer.DrawPoint(point, Color.Black, false);
}
// Draw the path.
for (float i = 0; i < _path.Last().Parameter; i += 0.1f)
{
var point0 = _path.GetPoint(i);
var point1 = _path.GetPoint((i + 0.1f));
debugRenderer.DrawLine(point0, point1, Color.Black, false);
}
// Move an object with constant speed along the path.
const float speed = 2;
var traveledDistance = _time * speed;
// Get path parameter where the path length is equal to traveledDistance.
var parameter = _path.GetParameterFromLength(traveledDistance, 10, 0.01f);
// Get path point at the traveledDistance.
Vector3F position = _path.GetPoint(parameter);
// Get the path tangent at traveledDistance and use it as the forward direction.
Vector3F forward = _path.GetTangent(parameter).Normalized;
// Draw an object on the path.
DrawObject(position, forward);
base.Update(gameTime);
}
public override void Update(GameTime gameTime)
{
float deltaTime = (float)gameTime.ElapsedGameTime.TotalSeconds;
// Move an object with constant speed along the path.
const float speed = 5;
float newPathPosition = _currentPathPosition + deltaTime * speed;
// Get path parameter where the path length is equal to newPathPosition.
float parameter = _path.GetParameterFromLength(newPathPosition, 10, 0.01f);
// Get path point at the newPathPosition.
Vector3 position = _path.GetPoint(parameter);
// Get the path tangent at newPathPosition and use it as the forward direction.
Vector3 forward = _path.GetTangent(parameter).Normalized;
Quaternion currentOrientation = Quaternion.CreateFromRotationMatrix(_kinematicBody.Pose.Orientation);
Quaternion targetOrientation = Quaternion.CreateFromRotationMatrix(Vector3.UnitY, forward);
Quaternion orientationDelta = targetOrientation * currentOrientation.Conjugated;
// Selective Negation:
// A certain rotation can be described by two quaternions: q and -q. For example, if you look
// to the north and want to look to the east you can either rotate 90 degrees clockwise or
// 270 degrees counter-clockwise. In the game we always want to rotate using the
// smaller angle. This is done using "Selective Negation": The dot product of two quaternions
// is proportional to the cosine of the rotation angle. If the cosine of the angle is < 0,
// the angle is larger than +/- 90 degrees. In this case we must use -orientationDelta to
// rotate using the smaller angle.
if (Quaternion.Dot(currentOrientation, targetOrientation) < 0)
{
orientationDelta = -orientationDelta;
}
// We could directly set the new position of the kinematic body. However, directly
// setting a position is like "teleporting" an object. The body would not interact
// properly with other objects in the physics simulation.
// Instead we apply a linear and angular velocity to the body. The simulation will
// automatically update the position of the body. If the body touches other objects
// along the way it will push these objects with the appropriate force.
// Note that the physics simulation may advance with a different time step than the
// rest of the game.
deltaTime = Math.Max(deltaTime, Simulation.Settings.Timing.FixedTimeStep);
// Determine the linear velocity that moves the body forward.
Vector3 linearVelocity = (position - _kinematicBody.Pose.Position) / deltaTime;
// Determine the angular velocity that rotates the body.
Vector3 angularVelocity;
Vector3 rotationAxis = orientationDelta.Axis;
if (!rotationAxis.IsNumericallyZero)
{
// The angular velocity is computed as rotationAxis * rotationSpeed.
// The rotation speed is computed as angle / time. (Note: The angle is given in radians.)
float rotationSpeed = (orientationDelta.Angle / deltaTime);
angularVelocity = rotationAxis * rotationSpeed;
}
else
{
// The axis of rotation is 0. That means the no rotation should be applied.
angularVelocity = Vector3.Zero;
}
_kinematicBody.LinearVelocity = linearVelocity;
_kinematicBody.AngularVelocity = angularVelocity;
_currentPathPosition = newPathPosition;
// Let the base class render the rigid bodies.
base.Update(gameTime);
// Draw the 3D path.
var debugRenderer = GraphicsScreen.DebugRenderer;
for (int i = 0; i < _pointList.Length; i++)
{
debugRenderer.DrawLine(_pointList[i], _pointList[(i + 1) % _pointList.Length], Color.Black, false);
}
}
public void GetParameterByLength()
{
Path3F empty = new Path3F();
empty.ParameterizeByLength(20, 0.001f); // No exception, do nothing.
Assert.IsTrue(float.IsNaN(empty.GetParameterFromLength(10, 20, 0.1f)));
Path3F path = CreatePath();
path.ParameterizeByLength(20, 0.001f);
Assert.AreEqual(0, path.GetParameterFromLength(0, 20, 0.001f));
Assert.AreEqual(3, path.GetParameterFromLength(3, 20, 0.001f));
Assert.AreEqual(path[4].Parameter, path.GetParameterFromLength(path[4].Parameter, 20, 0.01f));
Assert.AreEqual(path[5].Parameter, path.GetParameterFromLength(path[5].Parameter, 20, 0.01f));
Assert.AreEqual(path[6].Parameter, path.GetParameterFromLength(path[6].Parameter, 20, 0.01f));
Assert.AreEqual(path[7].Parameter, path.GetParameterFromLength(path[7].Parameter, 20, 0.01f));
Assert.AreEqual(path[8].Parameter, path.GetParameterFromLength(path[8].Parameter, 20, 0.01f));
Assert.AreEqual(path[9].Parameter, path.GetParameterFromLength(path[9].Parameter, 20, 0.01f));
float pathLength = path[9].Parameter;
float desiredLength = 11;
float actualLength = path.GetLength(0, path.GetParameterFromLength(desiredLength, 20, 0.01f), 20, 0.01f);
Assert.IsTrue(Numeric.AreEqual(desiredLength, actualLength, 0.01f));
desiredLength = 26;
actualLength = path.GetLength(0, path.GetParameterFromLength(desiredLength, 20, 0.01f), 20, 0.01f);
Assert.IsTrue(Numeric.AreEqual(desiredLength, actualLength, 0.01f));
desiredLength = 33.5f;
actualLength = path.GetLength(0, path.GetParameterFromLength(desiredLength, 20, 0.001f), 20, 0.001f);
Assert.IsTrue(Numeric.AreEqual(desiredLength, actualLength, 0.01f));
path.PreLoop = CurveLoopType.Linear;
path.PostLoop = CurveLoopType.Linear;
desiredLength = 60;
actualLength = path.GetLength(0, path.GetParameterFromLength(desiredLength, 20, 0.001f), 20, 0.001f);
Assert.IsTrue(Numeric.AreEqual(desiredLength, actualLength, 0.1f));
desiredLength = -10f;
actualLength = -path.GetLength(0, path.GetParameterFromLength(desiredLength, 20, 0.001f), 20, 0.001f);
Assert.IsTrue(Numeric.AreEqual(desiredLength, actualLength, 0.01f));
path.PreLoop = CurveLoopType.CycleOffset;
path.PostLoop = CurveLoopType.CycleOffset;
path.ParameterizeByLength(20, 0.001f);
desiredLength = -90;
actualLength = -path.GetLength(0, path.GetParameterFromLength(desiredLength, 20, 0.01f), 20, 0.01f);
Assert.IsTrue(Numeric.AreEqual(desiredLength, actualLength, 0.1f));
desiredLength = -50;
actualLength = -path.GetLength(0, path.GetParameterFromLength(desiredLength, 20, 0.01f), 20, 0.01f);
Assert.IsTrue(Numeric.AreEqual(desiredLength, actualLength, 0.1f));
desiredLength = -30;
actualLength = -path.GetLength(0, path.GetParameterFromLength(desiredLength, 20, 0.01f), 20, 0.01f);
Assert.IsTrue(Numeric.AreEqual(desiredLength, actualLength, 0.1f));
desiredLength = 50;
actualLength = path.GetLength(0, path.GetParameterFromLength(desiredLength, 20, 0.01f), 20, 0.01f);
Assert.IsTrue(Numeric.AreEqual(desiredLength, actualLength, 0.1f));
desiredLength = 100;
actualLength = path.GetLength(0, path.GetParameterFromLength(desiredLength, 20, 0.01f), 20, 0.01f);
Assert.IsTrue(Numeric.AreEqual(desiredLength, actualLength, 0.1f));
desiredLength = 130;
actualLength = path.GetLength(0, path.GetParameterFromLength(desiredLength, 20, 0.01f), 20, 0.01f);
Assert.IsTrue(Numeric.AreEqual(desiredLength, actualLength, 0.1f));
desiredLength = 200;
actualLength = path.GetLength(0, path.GetParameterFromLength(desiredLength, 20, 0.01f), 20, 0.01f);
Assert.IsTrue(Numeric.AreEqual(desiredLength, actualLength, 0.1f));
path.PreLoop = CurveLoopType.Oscillate;
path.PostLoop = CurveLoopType.Cycle;
path.ParameterizeByLength(20, 0.001f);
desiredLength = -110;
actualLength = -path.GetLength(0, path.GetParameterFromLength(desiredLength, 20, 0.01f), 20, 0.01f);
Assert.IsTrue(-3 * pathLength < path.GetParameterFromLength(desiredLength, 20, 0.01f));
Assert.IsTrue(-2 * pathLength > path.GetParameterFromLength(desiredLength, 20, 0.01f));
Assert.IsTrue(Numeric.AreEqual(desiredLength, actualLength, 0.1f));
desiredLength = -50;
actualLength = -path.GetLength(0, path.GetParameterFromLength(desiredLength, 20, 0.01f), 20, 0.01f);
Assert.IsTrue(-2 * pathLength < path.GetParameterFromLength(desiredLength, 20, 0.01f));
Assert.IsTrue(-1 * pathLength > path.GetParameterFromLength(desiredLength, 20, 0.01f));
Assert.IsTrue(Numeric.AreEqual(desiredLength, actualLength, 0.1f));
desiredLength = -30;
actualLength = -path.GetLength(0, path.GetParameterFromLength(desiredLength, 20, 0.01f), 20, 0.01f);
Assert.IsTrue(-1 * pathLength < path.GetParameterFromLength(desiredLength, 20, 0.01f));
Assert.IsTrue(0 > path.GetParameterFromLength(desiredLength, 20, 0.01f));
Assert.IsTrue(Numeric.AreEqual(desiredLength, actualLength, 0.1f));
desiredLength = 50;
actualLength = path.GetLength(0, path.GetParameterFromLength(desiredLength, 20, 0.01f), 20, 0.01f);
Assert.IsTrue(1 * pathLength < path.GetParameterFromLength(desiredLength, 20, 0.01f));
Assert.IsTrue(2 * pathLength > path.GetParameterFromLength(desiredLength, 20, 0.01f));
Assert.IsTrue(Numeric.AreEqual(desiredLength, actualLength, 0.1f));
desiredLength = 110;
actualLength = path.GetLength(0, path.GetParameterFromLength(desiredLength, 20, 0.01f), 20, 0.01f);
Assert.IsTrue(2 * pathLength < path.GetParameterFromLength(desiredLength, 20, 0.01f));
Assert.IsTrue(3 * pathLength > path.GetParameterFromLength(desiredLength, 20, 0.01f));
Assert.IsTrue(Numeric.AreEqual(desiredLength, actualLength, 0.1f));
desiredLength = 130;
actualLength = path.GetLength(0, path.GetParameterFromLength(desiredLength, 20, 0.01f), 20, 0.01f);
Assert.IsTrue(3 * pathLength < path.GetParameterFromLength(desiredLength, 20, 0.01f));
Assert.IsTrue(4 * pathLength > path.GetParameterFromLength(desiredLength, 20, 0.01f));
Assert.IsTrue(Numeric.AreEqual(desiredLength, actualLength, 0.1f));
desiredLength = 190;
actualLength = path.GetLength(0, path.GetParameterFromLength(desiredLength, 20, 0.01f), 20, 0.01f);
Assert.IsTrue(4 * pathLength < path.GetParameterFromLength(desiredLength, 20, 0.01f));
Assert.IsTrue(5 * pathLength > path.GetParameterFromLength(desiredLength, 20, 0.01f));
Assert.IsTrue(Numeric.AreEqual(desiredLength, actualLength, 0.2f));
path.PreLoop = CurveLoopType.Cycle;
path.PostLoop = CurveLoopType.Oscillate;
path.ParameterizeByLength(20, 0.001f);
desiredLength = -90;
actualLength = -path.GetLength(0, path.GetParameterFromLength(desiredLength, 20, 0.01f), 20, 0.01f);
Assert.IsTrue(-3 * pathLength < path.GetParameterFromLength(desiredLength, 20, 0.01f));
Assert.IsTrue(-2 * pathLength > path.GetParameterFromLength(desiredLength, 20, 0.01f));
Assert.IsTrue(Numeric.AreEqual(desiredLength, actualLength, 0.1f));
desiredLength = -50;
actualLength = -path.GetLength(0, path.GetParameterFromLength(desiredLength, 20, 0.01f), 20, 0.01f);
Assert.IsTrue(-2 * pathLength < path.GetParameterFromLength(desiredLength, 20, 0.01f));
Assert.IsTrue(-1 * pathLength > path.GetParameterFromLength(desiredLength, 20, 0.01f));
Assert.IsTrue(Numeric.AreEqual(desiredLength, actualLength, 0.1f));
desiredLength = -30;
actualLength = -path.GetLength(0, path.GetParameterFromLength(desiredLength, 20, 0.01f), 20, 0.01f);
Assert.IsTrue(-1 * pathLength < path.GetParameterFromLength(desiredLength, 20, 0.01f));
Assert.IsTrue(0 > path.GetParameterFromLength(desiredLength, 20, 0.01f));
Assert.IsTrue(Numeric.AreEqual(desiredLength, actualLength, 0.1f));
desiredLength = 50;
actualLength = path.GetLength(0, path.GetParameterFromLength(desiredLength, 20, 0.01f), 20, 0.01f);
Assert.IsTrue(pathLength < path.GetParameterFromLength(desiredLength, 20, 0.01f));
Assert.IsTrue(2 * pathLength > path.GetParameterFromLength(desiredLength, 20, 0.01f));
Assert.IsTrue(Numeric.AreEqual(desiredLength, actualLength, 0.1f));
desiredLength = 110;
actualLength = path.GetLength(0, path.GetParameterFromLength(desiredLength, 20, 0.01f), 20, 0.01f);
Assert.IsTrue(2 * pathLength < path.GetParameterFromLength(desiredLength, 20, 0.01f));
Assert.IsTrue(3 * pathLength > path.GetParameterFromLength(desiredLength, 20, 0.01f));
Assert.IsTrue(Numeric.AreEqual(desiredLength, actualLength, 0.1f));
desiredLength = 130;
actualLength = path.GetLength(0, path.GetParameterFromLength(desiredLength, 20, 0.01f), 20, 0.01f);
Assert.IsTrue(3 * pathLength < path.GetParameterFromLength(desiredLength, 20, 0.01f));
Assert.IsTrue(4 * pathLength > path.GetParameterFromLength(desiredLength, 20, 0.01f));
Assert.IsTrue(Numeric.AreEqual(desiredLength, actualLength, 0.1f));
desiredLength = 210;
actualLength = path.GetLength(0, path.GetParameterFromLength(desiredLength, 20, 0.01f), 20, 0.01f);
Assert.IsTrue(5 * pathLength < path.GetParameterFromLength(desiredLength, 20, 0.01f));
Assert.IsTrue(6 * pathLength > path.GetParameterFromLength(desiredLength, 20, 0.01f));
Assert.IsTrue(Numeric.AreEqual(desiredLength, actualLength, 0.1f));
// Test path with zero length.
path = new Path3F();
path.Add(new PathKey3F()
{
Parameter = 10,
Point = new Vector3F(0, 0, 1),
Interpolation = SplineInterpolation.Linear,
TangentIn = new Vector3F(1, 0, 0),
TangentOut = new Vector3F(1, 0, 0),
});
path.ParameterizeByLength(20, 0.001f);
Assert.AreEqual(0, path.GetParameterFromLength(0, 20, 0.1f));
path.Add(new PathKey3F()
{
Parameter = 20,
Point = new Vector3F(0, 0, 1),
Interpolation = SplineInterpolation.Linear,
TangentIn = new Vector3F(1, 0, 0),
TangentOut = new Vector3F(1, 0, 0),
});
path.ParameterizeByLength(20, 0.001f);
Assert.AreEqual(0, path.GetParameterFromLength(0, 20, 0.1f));
}