/// <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();
}
}
