Gram-Schmidtの直交化法で正規直交基底を求めるソースコード

Gram-Schmidtの直交化法で正規直交基底を作るソースコードを実装したので共有します。
(次元は3固定です)

参考にしたサイト

グラムシュミットの直交化法の意味と具体例
http://mathtrain.jp/gramschmidt

Gram-Schmidt の正規直交化法
http://li.nu/blog/2010/07/gram-schmidt.html

ソースコード(Java)

package hoge.piyo;

import javax.vecmath.Vector3d;

public class LinearAlgebraUtil {
	
	/**
	 * ベクトルa1, a2, a3の正規直交基底を作る。
	 * a1, a2, a3は一次独立である必要があります。
	 * ※引数チェックは省略
	 * @param a1 ベクトルa1
	 * @param a2 ベクトルa2
	 * @param a3 ベクトルa3
	 * @return 正規直交基底u1, u2, u3の順番に格納したVector3d配列
	 */
	public static Vector3d[] createOrthonormalBasis(
			Vector3d a1, Vector3d a2, Vector3d a3) {
		
		// u1
		Vector3d u1 = createNormalizedVector(a1);
		
		// u2
		Vector3d v2 = new Vector3d(a2);
		// v2からu1方向の成分を除く。
		removeOrthogonalProjectionVector(v2, u1);
		// v2を正規化
		Vector3d u2 = createNormalizedVector(v2);
		
		// u3
		Vector3d v3 = new Vector3d(a3);
		removeOrthogonalProjectionVector(v3, u1);
		removeOrthogonalProjectionVector(v3, u2);
		Vector3d u3 = createNormalizedVector(v3);
		
		return new Vector3d[] {
				u1, u2, u3
		};
	}
	
	/**
	 * ベクトルaの単位ベクトルを取得。
	 * @param a ベクトルa
	 * @return 正規化したベクトル
	 */
	private static Vector3d createNormalizedVector(Vector3d a) {
		Vector3d ret = new Vector3d(a);
		ret.normalize();
		return ret;
	}
	
	/**
	 * ベクトルaのv方向への正射影ベクトルを取得。
	 * @param a ベクトルa
	 * @param u ベクトルvの単位ベクトル
	 */
	private static Vector3d createOrthogonalProjectionVector(
			Vector3d a,
			Vector3d u) {
		Vector3d ret = new Vector3d(u);
		ret.scale( u.dot(a) );
		return ret;
	}
	
	
	/**
	 * ベクトルaからベクトルvの正射影成分を取り除く
	 * @param a ベクトルa
	 * @param u ベクトルvの単位ベクトル
	 */
	private static void removeOrthogonalProjectionVector(
			Vector3d a,
			Vector3d u) {
		Vector3d orthoProj = createOrthogonalProjectionVector(a, u);
		a.sub(orthoProj);
	}
	

}

テストコード。

package hoge.piyo;

import javax.vecmath.Vector3d;
import org.junit.Test;

public class LinearAlgebraUtilTest {
	
	
	@Test
	public void testCreateOrthonormalBasis() throws Exception {
		
		// 答え合わせ。
		{
			// 単純な例
			Vector3d[] S = new Vector3d[] {
					new Vector3d(0, 0, 10),
					new Vector3d(10, 0, 0),
					new Vector3d(0, 20, 0)
			};
			Vector3d[] T = LinearAlgebraUtil.createOrthonormalBasis(S[0], S[1], S[2]);
			dumpResult(S, T);
		}
		
		{
			// 単純な例２
			Vector3d[] S = new Vector3d[] {
					new Vector3d(0, 0, 10),
					new Vector3d(10, 10, 0),
					new Vector3d(5, 20, 0)
			};
			Vector3d[] T = LinearAlgebraUtil.createOrthonormalBasis(S[0], S[1], S[2]);
			dumpResult(S, T);
		}
		
		// orthonormal basis exampleで検索した結果から計算結果のサンプルを抜粋
		
		{
			// http://www.math4all.in/public_html/linear%20algebra/chapter8.2.html
			// 8.2.5 Examples:
			Vector3d[] S = new Vector3d[] {
					new Vector3d(1, 1, 1),
					new Vector3d(-1, 0, -1),
					new Vector3d(-1, 2, 3)
			};
			Vector3d[] T = LinearAlgebraUtil.createOrthonormalBasis(S[0], S[1], S[2]);
			dumpResult(S, T);
		}

		{
			// http://lyle.smu.edu/emis/8371/book/chap3/node12.html
			// Examples:
			Vector3d[] S = new Vector3d[] {
					new Vector3d(1, 2, 2),
					new Vector3d(1, 1, 0),
					new Vector3d(1, -1, 1)
			};
			Vector3d[] T = LinearAlgebraUtil.createOrthonormalBasis(S[0], S[1], S[2]);
			dumpResult(S, T);
		}
		
		{
			// http://www.mcu.edu.tw/department/management/stat/ch_web/etea/linear/CH6.8%20--%206.9%20Orthonormal%20Bases%20in%20R%5En.pdf
			// Example p6 - p12
			// -> こちらは答えがorthogonalなので正規化して考える必要あり。
			Vector3d[] S = new Vector3d[] {
					new Vector3d(1, 0, 0),
					new Vector3d(1, 2, 0),
					new Vector3d(0, 0, 3)
			};
			Vector3d[] T = LinearAlgebraUtil.createOrthonormalBasis(S[0], S[1], S[2]);
			dumpResult(S, T);
		}
		
	}
	
	private static void dumpResult(Vector3d[] S, Vector3d[] T) {
		System.out.println(String.format("S:%s", toString(S)));
		System.out.println(String.format("T:%s", toString(T)));
		System.out.println();
	}
	
	private static String toString(Vector3d[] vs) {
		StringBuilder sb = new StringBuilder();
		for (Vector3d v : vs) {
			sb.append(String.format("(%f, %f, %f) ", v.x, v.y, v.z)).append(" ");
		}
		return sb.toString();
	}
}

テストコードの出力結果

S:(0.000000, 0.000000, 10.000000)  (10.000000, 0.000000, 0.000000)  (0.000000, 20.000000, 0.000000)  
T:(0.000000, 0.000000, 1.000000)  (1.000000, 0.000000, 0.000000)  (0.000000, 1.000000, 0.000000)  

S:(0.000000, 0.000000, 10.000000)  (10.000000, 10.000000, 0.000000)  (5.000000, 20.000000, 0.000000)  
T:(0.000000, 0.000000, 1.000000)  (0.707107, 0.707107, 0.000000)  (-0.707107, 0.707107, 0.000000)  

S:(1.000000, 1.000000, 1.000000)  (-1.000000, 0.000000, -1.000000)  (-1.000000, 2.000000, 3.000000)  
T:(0.577350, 0.577350, 0.577350)  (-0.408248, 0.816497, -0.408248)  (-0.707107, -0.000000, 0.707107)  

S:(1.000000, 2.000000, 2.000000)  (1.000000, 1.000000, 0.000000)  (1.000000, -1.000000, 1.000000)  
T:(0.333333, 0.666667, 0.666667)  (0.666667, 0.333333, -0.666667)  (0.666667, -0.666667, 0.333333)  

S:(1.000000, 0.000000, 0.000000)  (1.000000, 2.000000, 0.000000)  (0.000000, 0.000000, 3.000000)  
T:(1.000000, 0.000000, 0.000000)  (0.000000, 1.000000, 0.000000)  (0.000000, 0.000000, 1.000000)