public static Vector OuterProduct(Vector a, Vector b)
{
double x = a.y * b.z - a.z * b.y;
double y = a.z * b.x - a.x * b.z;
double z = a.x * b.y - a.y * b.x;
return new Vector(x, y, z);
}
public static Matrix GetRotate(double theta, Vector axis)
{
Matrix a = new Matrix();
a.Rotate(theta, axis);
return a;
}
/// <summary>
/// 軸ベクトル(axis)を中心にθ(theta)回転する行列を求める。
/// </summary>
/// <param name="theta">回転角</param>
/// <param name="axis">回転軸ベクトル</param>
public void Rotate(double theta, Vector axis)
{
axis /= axis.Abs;
double cos = Math.Cos(theta);
double sin = Math.Sin(theta);
double c1 = 1 - cos;
double yz = axis.y * axis.z * c1;
double zx = axis.z * axis.x * c1;
double xy = axis.x * axis.y * c1;
double x =axis.x * sin;
double y =axis.y * sin;
double z =axis.z * sin;
double ax = cos + c1 * axis.x * axis.x;
double ay = xy + z;
double az = zx - y;
double bx = xy - z;
double by = cos + c1 * axis.y * axis.y;
double bz = yz + x;
double cx = zx + y;
double cy = yz - x;
double cz = cos + c1 * axis.z * axis.z;
this.Set(
ax, bx, cx,
ay, by, cy,
az, bz, cz);
}
public Matrix(Vector x, Vector y, Vector z)
:this(
x.x, y.x, z.x,
x.y, y.y, z.y,
x.z, y.z, z.z){}
/// <summary>
/// 実部と虚部ベクトルを指定して初期化。
/// a + ii・uu、ii=(i,j,k)、uu＝(p,q,r)。
/// </summary>
/// <param name="a">実部</param>
/// <param name="u">虚部ベクトル</param>
public Quaternion(double a, Vector u)
{
this.a = a;
this.u = u;
}
/// <summary>
/// ベクトル(axis)を軸として、θ(theta)回転するための四元数を計算する。
/// </summary>
/// <param name="theta">回転角θ</param>
/// <param name="axis">回転軸ベクトル</param>
/// <returns>回転を表す四元数</returns>
public static Quaternion Rotator(double theta, Vector axis)
{
theta *= 0.5;
axis *= Math.Sin(theta) / axis.Abs;
return new Quaternion(Math.Cos(theta), axis);
}
/// <summary>
/// 四元数を使って3次元空間上の回転。
/// p × (0, x) × ~p を計算する(~p は p の共役)。
/// </summary>
/// <param name="p">回転軸/角を表す四元数</param>
/// <param name="x">回転させたい点のベクトル</param>
/// <returns>回転後の点のベクトル</returns>
public static Vector Rotate(Quaternion p, Vector x)
{
Vector y = (p.a * p.a - p.u.Norm) * x;
y += 2 * (Vector.InnerProduct(p.u, x) * p.u + p.a * Vector.OuterProduct(p.u, x));
return y;
}
public static double InnerProduct(Vector a, Vector b)
{
return a.x * b.x + a.y * b.y + a.z * b.z;
}
public AffineMatrix(Matrix a, Vector t)
{
this.a = a;
this.t = t;
}
public AffineMatrix(Vector t) : this(Matrix.I, t) {}