public static V3f Original(Rot3f r)
{
var test = r.W * r.Y - r.X * r.Z;
if (test > 0.5f - Constant <float> .PositiveTinyValue) // singularity at north pole
{
return(new V3f(
2 * Fun.Atan2(r.X, r.W),
(float)Constant.PiHalf,
0));
}
if (test < -0.5f + Constant <float> .PositiveTinyValue) // singularity at south pole
{
return(new V3f(
2 * Fun.Atan2(r.X, r.W),
-(float)Constant.PiHalf,
0));
}
// From Wikipedia, conversion between quaternions and Euler angles.
return(new V3f(
Fun.Atan2(2 * (r.W * r.X + r.Y * r.Z),
1 - 2 * (r.X * r.X + r.Y * r.Y)),
Fun.AsinClamped(2 * test),
Fun.Atan2(2 * (r.W * r.Z + r.X * r.Y),
1 - 2 * (r.Y * r.Y + r.Z * r.Z))));
}
public static __v3t__ Original(__rot3t__ r)
{
var test = r.W * r.Y - r.X * r.Z;
if (test > __half__ - Constant <__rtype__> .PositiveTinyValue) // singularity at north pole
{
return(new __v3t__(
2 * Fun.Atan2(r.X, r.W),
__piHalf__,
0));
}
if (test < -__half__ + Constant <__rtype__> .PositiveTinyValue) // singularity at south pole
{
return(new __v3t__(
2 * Fun.Atan2(r.X, r.W),
-__piHalf__,
0));
}
// From Wikipedia, conversion between quaternions and Euler angles.
return(new __v3t__(
Fun.Atan2(2 * (r.W * r.X + r.Y * r.Z),
1 - 2 * (r.X * r.X + r.Y * r.Y)),
Fun.AsinClamped(2 * test),
Fun.Atan2(2 * (r.W * r.Z + r.X * r.Y),
1 - 2 * (r.Y * r.Y + r.Z * r.Z))));
}
public static __v3t__ ToAngleAxis(this __type__ r)
{
var sinTheta2 = r.V.LengthSquared;
if (sinTheta2 > Constant <__ftype__> .PositiveTinyValue)
{
__ftype__ sinTheta = Fun.Sqrt(sinTheta2);
__ftype__ cosTheta = r.W;
__ftype__ twoTheta = 2 * (cosTheta < 0 ? Fun.Atan2(-sinTheta, -cosTheta)
: Fun.Atan2(sinTheta, cosTheta));
return(r.V * (twoTheta / sinTheta));
}
else
{
return(__v3t__.Zero);
}
}
public static V3f CopySign(Rot3f r)
{
var test = r.W * r.Y - r.X * r.Z;
if (test.Abs() >= 0.5f - Constant <float> .PositiveTinyValue)
{
return(new V3f(
2 * Fun.Atan2(r.X, r.W),
Fun.CopySign((float)Constant.PiHalf, test),
0));
}
else
{
return(new V3f(
Fun.Atan2(2 * (r.W * r.X + r.Y * r.Z),
1 - 2 * (r.X * r.X + r.Y * r.Y)),
Fun.AsinClamped(2 * test),
Fun.Atan2(2 * (r.W * r.Z + r.X * r.Y),
1 - 2 * (r.Y * r.Y + r.Z * r.Z))));
}
}
public static __v3t__ CopySign(__rot3t__ r)
{
var test = r.W * r.Y - r.X * r.Z;
if (test.Abs() >= __half__ - Constant <__rtype__> .PositiveTinyValue)
{
return(new __v3t__(
2 * Fun.Atan2(r.X, r.W),
Fun.CopySign(__piHalf__, test),
0));
}
else
{
return(new __v3t__(
Fun.Atan2(2 * (r.W * r.X + r.Y * r.Z),
1 - 2 * (r.X * r.X + r.Y * r.Y)),
Fun.AsinClamped(2 * test),
Fun.Atan2(2 * (r.W * r.Z + r.X * r.Y),
1 - 2 * (r.Y * r.Y + r.Z * r.Z))));
}
}
/// <summary>
/// Creates Images of an optical illusion that tricks the mind into
/// seeing more different colors (4) than are actually present in the
/// image (3).
/// </summary>
public static PixImage <byte> CreateHowManyColorsIllusion(int size, bool parallel = true)
{
var scale = 1024.0 / size;
var delta = 0.5 * (double)(size - 1);
var pixImage = new PixImage <byte>(size, size, 3);
var colorMatrix = pixImage.GetMatrix <C3b>();
var orange = new C3b(255, 150, 0);
var magenta = new C3b(255, 0, 255);
var bluegreen = new C3b(0, 255, 150);
Func <long, long, C3b> pixelFun = (x, y) =>
{
var xd = scale * (x - delta); var yd = scale * (y - delta);
var r = Fun.Sqrt(xd * xd + yd * yd);
var phi = Fun.Atan2(yd, xd);
var lp1 = phi / Constant.PiTimesFour;
var lp2 = phi / Constant.Pi; // TimesTwo;
var lr = Fun.Log(r) / Constant.E;
var p1 = Fun.Frac(0.05 + 4 * (lr - lp1));
var p2 = Fun.Frac(96 * (lr + lp2)); // 64
return(p2 < 0.5
? (p1 >= 0.0 && p1 < 0.25 ? bluegreen : orange)
: (p1 >= 0.5 && p1 < 0.75 ? bluegreen : magenta));
};
if (parallel)
{
colorMatrix.SetByCoordParallelY(pixelFun);
}
else
{
colorMatrix.SetByCoord(pixelFun);
}
return(pixImage);
}
public static __ftype__ Distance(this __type__ r1, __type__ r2)
{
var q = r1.Inverse * r2;
return(2 * Fun.Atan2(q.V.Length, (q.W < 0) ? -q.W : q.W));
}