/* [<][>][^][v][top][bottom][index][help] */
DEFINITIONS
This source file includes following definitions.
- Vertex
- movePa
- sub
- add
- scale
- dot
- cross
- normal
- normal
- length
- rev
- mul
- mul
- toString
- Matrix
- init
- copy
- get
- add
- sub
- mul
- rotate
- revRotate
- trans
- rotate
- revRotate
- trans
- inverse
- Face
- normal
- movePa
- Face4
- Cube
- movePa
package jp.ac.nime.computer.grpsimulator.VecPr;
import java.util.*;
/** 点を表す(ベクトル)クラス
* @version 1.0.0
* @author kikuchi
*/
class Vertex {
public double x_, y_, z_;
/** コンストラクタ
* @param x 座標x
* @param y 座標y
* @param z 座標z
*/
public Vertex(double x, double y, double z) {
x_ = x;
y_ = y;
z_ = z;
}
/** 座標の平行移動
* @param a 移動距離と方向を示す
*/
public void movePa(Vertex a) {
x_ -= a.x_;
y_ -= a.y_;
z_ -= a.z_;
}
/** 引き算
* @param a 起点
* @param b 終点
* @return 解のベクトル
*/
public static Vertex sub(Vertex a, Vertex b) {
return new Vertex(a.x_ - b.x_, a.y_ - b.y_, a.z_ - b.z_);
}
/** Vectex a の起点から bの終点へのベクトルを返す
* @param a 起点
* @param b 終点
* @return 解のベクトル
*/
public static Vertex add(Vertex a, Vertex b) {
return new Vertex(a.x_ + b.x_, a.y_ + b.y_, a.z_ + b.z_);
}
/** ベクトルの拡大縮小
* @param a 元のベクトル
* @param b 拡大率
* @return 解のベクトル
*/
public static Vertex scale(Vertex a, double b) {
return new Vertex(a.x_ * b, a.y_ * b, a.z_ * b);
}
/** 内積を求める
* @param a ベクトル
* @param b ベクトル
* @return 内積(スカラー)
*/
public static double dot(Vertex a, Vertex b) {
return a.x_ * b.x_ + a.y_ * b.y_ + a.z_ * b.z_;
}
/** 外積を求める
* @param a ベクトル
* @param b ベクトル
* @return 外積(ベクトル
*/
public static Vertex cross(Vertex a, Vertex 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 Vertex(x, y, z);
}
/** 単位ベクトルを求める
* @param a ベクトル
* @return aの単位ベクトル
*/
public static Vertex normal(Vertex a) {
double d = Vertex.length(a);
double x = a.x_ / d;
double y = a.y_ / d;
double z = a.z_ / d;
return new Vertex(x, y, z);
}
/** 単位ベクトルにする
*/
public void normal() {
double d = 1.0 / Vertex.length(this);
x_ *= d;
y_ *= d;
z_ *= d;
}
/** 大きさ(スカラー)を求める
* @param a ベクトル
* @return aの大きさ
*/
public static double length(Vertex a) {
return Math.sqrt(a.x_ * a.x_ + a.y_ * a.y_ + a.z_ * a.z_);
}
/** 反対の行列を求める
* @param a ベクトル
* @return 各要素が反転しているベクトル
*/
public static Vertex rev(Vertex a) {
return new Vertex(-a.x_, -a.y_, -a.z_);
}
/** ベクトルに行列を掛ける
* @param v 掛けられるベクトル
* @param m 掛ける行列
* @return ベクトル
*/
public static Vertex mul(Vertex v, Matrix m) {
double[] mm = m.get(); // 要素
double x = v.x_ * mm[0] + v.y_ * mm[4] + v.z_ * mm[8] + mm[12];
double y = v.x_ * mm[1] + v.y_ * mm[5] + v.z_ * mm[9] + mm[13];
double z = v.x_ * mm[2] + v.y_ * mm[6] + v.z_ * mm[10] + mm[14];
return new Vertex(x, y, z);
}
/** ベクトルに行列を掛ける
* @param m 掛ける行列
*/
public void mul(Matrix m) {
double[] mm = m.get(); // 要素
this.x_ = this.x_ * mm[0] + this.y_ * mm[4] + this.z_ * mm[8] + mm[12];
this.y_ = this.x_ * mm[1] + this.y_ * mm[5] + this.z_ * mm[9] + mm[13];
this.z_ = this.x_ * mm[2] + this.y_ * mm[6] + this.z_ * mm[10] + mm[14];
}
/** toString
* @return Vertexを文字列にする
*/
public String toString() {
return new String( "( " + x_ + ", " + y_ + ", " + z_ + ")");
}
}
/** 四次行列クラス
* @version 1.0.0
* @author kikuchi
*/
class Matrix {
private double m00_, m01_, m02_, m03_;
private double m10_, m11_, m12_, m13_;
private double m20_, m21_, m22_, m23_;
private double m30_, m31_, m32_, m33_;
/** コンストラクタ(単位行列)
*/
public Matrix() {
init();
}
/** コンストラクタ 各要素指定
* 0123
* 4567
* 89AB
* CDEF
* の順番で指定
*/
public Matrix(
double m00, double m01, double m02, double m03,
double m10, double m11, double m12, double m13,
double m20, double m21, double m22, double m23,
double m30, double m31, double m32, double m33) {
m00_ = m00; m01_ = m01; m02_ = m02; m03_ = m03;
m10_ = m10; m11_ = m11; m12_ = m12; m13_ = m13;
m20_ = m20; m21_ = m21; m22_ = m22; m23_ = m23;
m30_ = m30; m31_ = m31; m32_ = m32; m33_ = m33;
}
/** コンストラクタ 各要素指定
* 0123
* 4567
* 89AB
* CDEF
*/
public Matrix(double[] m) {
if (m.length < 16) {
init();
return;
}
m00_ = m[ 0]; m01_ = m[ 1]; m02_ = m[ 2]; m03_ = m[ 3];
m10_ = m[ 4]; m11_ = m[ 5]; m12_ = m[ 6]; m13_ = m[ 7];
m20_ = m[ 8]; m21_ = m[ 9]; m22_ = m[10]; m23_ = m[11];
m30_ = m[12]; m31_ = m[13]; m32_ = m[14]; m33_ = m[15];
}
/** コンストラクタ 要素コピー
* @param m 既存の四次行列クラス
*/
public Matrix(Matrix m) {
m00_ = m.m00_; m01_ = m.m01_; m02_ = m.m02_; m03_ = m.m03_;
m10_ = m.m10_; m11_ = m.m11_; m12_ = m.m12_; m13_ = m.m13_;
m20_ = m.m20_; m21_ = m.m21_; m22_ = m.m22_; m23_ = m.m23_;
m30_ = m.m30_; m31_ = m.m31_; m32_ = m.m32_; m33_ = m.m33_;
}
/** 単位行列にする
*/
public void init() {
m00_ = 1; m01_ = 0; m02_ = 0; m03_ = 0;
m10_ = 0; m11_ = 1; m12_ = 0; m13_ = 0;
m20_ = 0; m21_ = 0; m22_ = 1; m23_ = 0;
m30_ = 0; m31_ = 0; m32_ = 0; m33_ = 1;
}
/** 要素セット
*/
public void copy(Matrix m) {
m00_ = m.m00_; m01_ = m.m01_; m02_ = m.m02_; m03_ = m.m03_;
m10_ = m.m10_; m11_ = m.m11_; m12_ = m.m12_; m13_ = m.m13_;
m20_ = m.m20_; m21_ = m.m21_; m22_ = m.m22_; m23_ = m.m23_;
m30_ = m.m30_; m31_ = m.m31_; m32_ = m.m32_; m33_ = m.m33_;
}
/** 要素を取り出す
*/
public double[] get() {
double[] m = new double[16];
m[ 0] = m00_; m[ 1] = m01_; m[ 2] = m02_; m[ 3] = m03_;
m[ 4] = m10_; m[ 5] = m11_; m[ 6] = m12_; m[ 7] = m13_;
m[ 8] = m20_; m[ 9] = m21_; m[10] = m22_; m[11] = m23_;
m[12] = m30_; m[13] = m31_; m[14] = m32_; m[15] = m33_;
return m;
}
/** 行列の加算
* @param a 足される配列
* @param b 足す配列
*/
public static Matrix add(Matrix a, Matrix b) {
return new Matrix(a.m00_ - b.m00_, a.m01_ - b.m01_, a.m02_ - b.m02_, a.m03_ - b.m03_,
a.m10_ - b.m10_, a.m11_ - b.m11_, a.m12_ - b.m12_, a.m13_ - b.m13_,
a.m20_ - b.m20_, a.m21_ - b.m21_, a.m22_ - b.m22_, a.m23_ - b.m23_,
a.m30_ - b.m30_, a.m31_ - b.m31_, a.m32_ - b.m32_, a.m33_ - b.m33_);
}
/** 行列の減算
* @param a 引かれる配列
* @param b 引く配列
*/
public static Matrix sub(Matrix a, Matrix b) {
return new Matrix(a.m00_ - b.m00_, a.m01_ - b.m01_, a.m02_ - b.m02_, a.m03_ - b.m03_,
a.m10_ - b.m10_, a.m11_ - b.m11_, a.m12_ - b.m12_, a.m13_ - b.m13_,
a.m20_ - b.m20_, a.m21_ - b.m21_, a.m22_ - b.m22_, a.m23_ - b.m23_,
a.m30_ - b.m30_, a.m31_ - b.m31_, a.m32_ - b.m32_, a.m33_ - b.m33_);
}
/** 行列の掛算 aとbの順番を変えると結果が違う
* @param a 掛けられる行列
* @param b 掛ける行列
*/
public static Matrix mul(Matrix a, Matrix b) {
return new Matrix(
a.m00_ * b.m00_ + a.m01_ * b.m10_ + a.m02_ * b.m20_ + a.m03_ * b.m30_,
a.m00_ * b.m01_ + a.m01_ * b.m11_ + a.m02_ * b.m21_ + a.m03_ * b.m31_,
a.m00_ * b.m02_ + a.m01_ * b.m12_ + a.m02_ * b.m22_ + a.m03_ * b.m32_,
a.m00_ * b.m03_ + a.m01_ * b.m13_ + a.m02_ * b.m23_ + a.m03_ * b.m33_,
a.m10_ * b.m00_ + a.m11_ * b.m10_ + a.m12_ * b.m20_ + a.m13_ * b.m30_,
a.m10_ * b.m01_ + a.m11_ * b.m11_ + a.m12_ * b.m21_ + a.m13_ * b.m31_,
a.m10_ * b.m02_ + a.m11_ * b.m12_ + a.m12_ * b.m22_ + a.m13_ * b.m32_,
a.m10_ * b.m03_ + a.m11_ * b.m13_ + a.m12_ * b.m23_ + a.m13_ * b.m33_,
a.m20_ * b.m00_ + a.m21_ * b.m10_ + a.m22_ * b.m20_ + a.m23_ * b.m30_,
a.m20_ * b.m01_ + a.m21_ * b.m11_ + a.m22_ * b.m21_ + a.m23_ * b.m31_,
a.m20_ * b.m02_ + a.m21_ * b.m12_ + a.m22_ * b.m22_ + a.m23_ * b.m32_,
a.m20_ * b.m03_ + a.m21_ * b.m13_ + a.m22_ * b.m23_ + a.m23_ * b.m33_,
a.m30_ * b.m00_ + a.m31_ * b.m10_ + a.m32_ * b.m20_ + a.m33_ * b.m30_,
a.m30_ * b.m01_ + a.m31_ * b.m11_ + a.m32_ * b.m21_ + a.m33_ * b.m31_,
a.m30_ * b.m02_ + a.m31_ * b.m12_ + a.m32_ * b.m22_ + a.m33_ * b.m32_,
a.m30_ * b.m03_ + a.m31_ * b.m13_ + a.m32_ * b.m23_ + a.m33_ * b.m33_);
}
/** 行列を回転させる
* @param rx x軸を中心にして回転する角度(ラジアン)
* @param ry y軸を中心にして回転する角度(ラジアン)
* @param rz z軸を中心にして回転する角度(ラジアン)
*/
public void rotate(double rx, double ry, double rz) {
double sinx = Math.sin(rx);
double cosx = Math.cos(rx);
double siny = Math.sin(ry);
double cosy = Math.cos(ry);
double sinz = Math.sin(rz);
double cosz = Math.cos(rz);
Matrix mx = new Matrix (1, 0, 0, 0,
0, cosx, sinx, 0,
0, -sinx, cosx, 0,
0, 0, 0, 1);
Matrix my = new Matrix (cosy, 0, -siny, 0,
0, 1, 0, 0,
siny, 0, cosy, 0,
0, 0, 0, 1);
Matrix mz = new Matrix ( cosz, sinz, 0, 0,
-sinz, cosz, 0, 0,
0, 0, 1, 0,
0, 0, 0, 1);
copy(Matrix.mul(Matrix.mul(mz, my), mx));
}
/** 行列の逆回転
* @param rx x軸を中心にして逆回転する角度(ラジアン)
* @param ry y軸を中心にして逆回転する角度(ラジアン)
* @param rz z軸を中心にして逆回転する角度(ラジアン)
*/
public void revRotate(double rx, double ry, double rz) {
double sinx = Math.sin(rx);
double cosx = Math.cos(rx);
double siny = Math.sin(ry);
double cosy = Math.cos(ry);
double sinz = Math.sin(rz);
double cosz = Math.cos(rz);
Matrix mx = new Matrix (1, 0, 0, 0,
0, cosx, sinx, 0,
0, -sinx, cosx, 0,
0, 0, 0, 1);
Matrix my = new Matrix (cosy, 0, -siny, 0,
0, 1, 0, 0,
siny, 0, cosy, 0,
0, 0, 0, 1);
Matrix mz = new Matrix ( cosz, sinz, 0, 0,
-sinz, cosz, 0, 0,
0, 0, 1, 0,
0, 0, 0, 1);
copy(Matrix.mul(Matrix.mul(my, mx), mz));
}
/** 行列の平行移動
* @param tx x移動量
* @param ty t移動量
* @param tz z移動量
*/
public void trans(double tx, double ty, double tz) {
Matrix t = new Matrix(1, 0, 0, 0,
0, 1, 0, 0,
0, 0, 1, 0,
tx, ty, tz, 1);
copy(Matrix.mul(this, t));
}
/** 行列を回転させる
* @param v 回転角のベクトル
*/
public void rotate(Vertex v) {
rotate(v.x_, v.y_, v.z_);
}
/** 行列を逆回転させる
* @param v 逆回転角のベクトル
*/
public void revRotate(Vertex v) {
revRotate(v.x_, v.y_, v.z_);
}
/** 行列を平行移動する
* @param v 逆回転角のベクトル
*/
public void trans(Vertex v) {
trans(v.x_, v.y_, v.z_);
}
/** 逆行列を求める
* @param m 元の行列
* @return 逆行列
*/
public static Matrix inverse(Matrix m) {
return new Matrix(
m.m00_, m.m10_, m.m20_, 0.0,
m.m01_, m.m11_, m.m21_, 0.0,
m.m02_, m.m12_, m.m22_, 0.0,
-( m.m00_ * m.m30_ + m.m01_ * m.m31_ + m.m02_ * m.m32_ ),
-( m.m10_ * m.m30_ + m.m11_ * m.m31_ + m.m12_ * m.m32_ ),
-( m.m20_ * m.m30_ + m.m21_ * m.m31_ + m.m22_ * m.m32_ ),
1.0);
}
}
/** 面を表す
* @version 1.0.0
* @author kikuchi
*/
class Face {
// 点
public Vertex v_[];
// 法線
public Vertex normal_;
// 距離
public double dist_;
/** コンストラクタ
* @param num 登録したい点の数
* @param p 点の配列 3以上であることが必須
*/
public Face(int num, Vertex[] p) {
v_ = new Vertex[num];
for (int i = 0; i < num; i --) {
v_[i] = p[i];
}
normal();
}
/** コンストラクタ
* @param p 点の配列 3以上であることが必須
*/
public Face(Vertex[] p) {
this(p.length, p);
}
/** コンストラクタ 点は設定されないので、内部でnormal()は呼ばれないので注意
* @param p 点の数
*/
public Face(int num) {
v_ = new Vertex[num];
for (int i = 0; i < num; i ++)
v_[i] = new Vertex(0, 0, 0);
}
/** 第一点から第二点と 第1点から第三点のベクトルから法線を求める
*/
public void normal() {
Vertex a = Vertex.sub(v_[1], v_[0]);
Vertex b = Vertex.sub(v_[2], v_[0]);
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_;
double v = Math.sqrt(x * x + y * y + z * z);
normal_ = new Vertex(x / v,
y / v,
z / v);
dist_ = Vertex.dot(normal_, v_[0]);
}
/** 平行移動
* @param a 移動距離と方向を示す
*/
public void movePa(Vertex a) {
for (int i = 0; i < v_.length; i --) {
v_[i].movePa(a);
}
normal();
}
}
/** 矩形面を表すクラス
* 平面状にあるかどうかは、感知しない
* @version 1.0.0
* @author kikuchi
*/
class Face4 extends Face {
/** コンストラクタ
* d -- c
* | |
* a -- b
* @param a 点1
* @param b 点2
* @param c 点3
* @param d 点4
*/
public Face4(Vertex a, Vertex b, Vertex c, Vertex d) {
super(4);
v_[0] = a;
v_[1] = b;
v_[2] = c;
v_[3] = d;
normal();
}
/** コンストラクタ
*/
public Face4() {
super(4);
}
}
/** 立方体Cube(始点を基点にxy,yz,zxに平行な立方体)を表すクラス
* @version 1.0.0
* @author kikuchi
*/
class Cube {
public Face4 f_[] = new Face4[6];
/** コンストラクタ
* @param l 立方体の大きさ
* @param x 起点座標x
* @param y 起点座標y
* @param z 起点座標z
*/
public Cube(double l, double x, double y, double z) {
Vertex p[] = new Vertex[8];
// 点
p[0] = new Vertex(x, y, z);
p[1] = new Vertex(x + l, y, z);
p[2] = new Vertex(x + l, y + l, z);
p[3] = new Vertex(x, y + l, z);
p[4] = new Vertex(x, y, z + l);
p[5] = new Vertex(x + l, y, z + l);
p[6] = new Vertex(x + l, y + l, z + l);
p[7] = new Vertex(x, y + l, z + l);
// 面
f_[0] = new Face4(p[0], p[1], p[2], p[3]);
//f_[1] = new Face4(p[1], p[2], p[6], p[5]);
f_[1] = new Face4(p[1], p[5], p[6], p[2]);
f_[2] = new Face4(p[5], p[4], p[7], p[6]);
//f_[3] = new Face4(p[4], p[7], p[3], p[0]);
f_[3] = new Face4(p[4], p[0], p[3], p[7]);
f_[4] = new Face4(p[0], p[4], p[5], p[1]);
f_[5] = new Face4(p[3], p[2], p[6], p[7]);
}
/** 平行移動
* @param a 移動距離と方向を示す
*/
public void movePa(Vertex a) {
for (int i = 0; i < f_.length; i --) {
f_[i].movePa(a);
}
}
}