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) {}