/// <summary> /// Sets the parameters and regenerates the capsule if necessary /// </summary> /// <param name="p1"></param> /// <param name="p2"></param> /// <param name="rad"></param> public void SetParameters(Vector3 p1, Vector3 p2, float rad) { // Only regerenate capsule if something is different from last time if (p1 != Point1 || p2 != Point2 || rad != Radius) { Point1 = p1; Point2 = p2; Radius = rad; Height = (float)Math.Sqrt(Math3D.DistanceSquared(p1, p2)); Orientation = Matrix4.Identity; if (p1 != p2) { var dir = Vector3.Normalize(p2 - p1); if (dir.X == 0 && dir.Z == 0) { Orientation = new Matrix4(Vector4.UnitX, new Vector4(dir), Vector4.UnitZ, Vector4.UnitW); } else { var axis = Vector3.Cross(Vector3.UnitY, dir); var angle = (float)Math.Acos(Vector3.Dot(Vector3.UnitY, dir)); Orientation = Matrix4.CreateFromAxisAngle(axis, angle); } } Orientation *= Matrix4.CreateTranslation(Point1); CreateCapsule(); } }