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; }