public static bool Animate(
Point3D currentValue, Vector3D currentVelocity, Point3D targetValue,
double attractionFator, double dampening,
double terminalVelocity, double minValueDelta, double minVelocityDelta,
out Point3D newValue, out Vector3D newVelocity)
{
Debug.Assert(currentValue.IsValid());
Debug.Assert(currentVelocity.IsValid());
Debug.Assert(targetValue.IsValid());
Debug.Assert(dampening.IsValid());
Debug.Assert(dampening > 0 && dampening < 1);
Debug.Assert(attractionFator.IsValid());
Debug.Assert(attractionFator > 0);
Debug.Assert(terminalVelocity > 0);
Debug.Assert(minValueDelta > 0);
Debug.Assert(minVelocityDelta > 0);
Vector3D diff = targetValue - currentValue;
if (diff.Length > minValueDelta || currentVelocity.Length > minVelocityDelta)
{
newVelocity = currentVelocity * (1 - dampening);
newVelocity += diff * attractionFator;
newVelocity *= (currentVelocity.Length > terminalVelocity) ? terminalVelocity / currentVelocity.Length : 1;
newValue = currentValue + newVelocity;
return true;
}
else
{
newValue = targetValue;
newVelocity = new Vector3D();
return false;
}
}
private static Vector3D limitVector3D(Vector3D input, double max)
{
Debug.Assert(max > 0);
Debug.Assert(max.IsValid());
Debug.Assert(input.IsValid());
if (input.Length > max)
{
input.Normalize();
return input * max;
}
else
{
return input;
}
}
public static List<double> RayMeshIntersections(Point3D orig, Vector3D dir, MeshGeometry3D mesh, bool frontFacesOnly)
{
List<double> result = new List<double>();
if (mesh == null || !dir.IsValid())
return result;
double epsilon = 1E-9;
Int32Collection indices = mesh.TriangleIndices;
Point3DCollection positions = mesh.Positions;
for (int i = 0; i < indices.Count; i += 3)
{
Point3D vert0 = positions[indices[i]];
Point3D vert1 = positions[indices[i + 1]];
Point3D vert2 = positions[indices[i + 2]];
//--- find vectors for two edges sharing vert0
Vector3D edge1 = vert1 - vert0;
Vector3D edge2 = vert2 - vert0;
//--- begin calculating determinant also used to calculate U parameter
Vector3D pvec = dir.Cross(edge2);
//--- if determinant is near zero ray lies in plane of triangle
double det = edge1.Dot(pvec);
if (det < epsilon)
{
if (frontFacesOnly)
continue;
else if (det > -epsilon)
continue;
}
double inv_det = 1.0 / det;
//--- calculate distance from vert0 to ray origin
Vector3D tvec = orig - vert0;
//--- calculate U parameter and test bounds
double u = tvec.Dot(pvec) * inv_det;
if (u < 0.0 || u > 1.0)
continue;
//--- prepare to test V parameter
Vector3D qvec = tvec.Cross(edge1);
//--- calculate V parameter and test bounds
double v = dir.Dot(qvec) * inv_det;
if (v < 0.0 || u + v > 1.0)
continue;
//--- calculate t scale parameters, ray intersects triangle
double t = edge2.Dot(qvec) * inv_det;
result.Add(t);
}
return result;
}