汎用演算ライブラリ
【StdAfx.h】
各種数値演算モジュールを提供します。
■シンボル
#define KOD_ERR -1 ERRORのシンボル
#define KOD_FALSE 0 偽のシンボル
#define KOD_TRUE 1 真のシンボル
#define KOD_DONE 2 実行済みを示すシンボル
#define KOD_ONEDGE 2 点がエッジ上にあることを示すシンボル
#define KOD_EQUAL 2 同値のシンボル
#define LOW_ACCURACY 0 低精度のシンボル
#define MID_ACCURACY 1 普通精度のシンボル
#define HIGH_ACCURACY 2 高精度のシンボル
#define FNAMEMAX 256 ファイル名の最大文字数
#define PI 3.141592653589793 円周率
#define APPROX_ZERO_L 1.0e-3 ゼロと見なせる値(低精度)
#define APPROX_ZERO 1.0e-6 ゼロと見なせる値(普通精度)
#define APPROX_ZERO_H 1.0e-12 ゼロと見なせる値(高精度)
#define LOOPCOUNTMAX 1000 収束計算回数の条件
■インクルードライブラリ
<stdio.h>
<stdlib.h>
<math.h>
<string.h>
<unistd.h>
<GL/gl.h>
<GL/glut.h>
<WSCfileSelect.h>
<WSCmessageDialog.h>
<WSCimageSet.h>
<WSDkeyboard.h>
<WSDmwindowDev.h>
<WSDmouse.h>
<WSCcolorSet.h>
"Kodatuno_Win.h"
■型定義
typedef double **Matirx double形の2次元配列をMatrixとして定義
typedef double *Vector double形の1次元配列をVectorとして定義
<座標用構造体>
typedef struct{
double x,y,z; x,y,z座標値
double dmy; 汎用
}Coord;
<同次変換行列用構造体>
typedef struct{
Coord Rot[3]; 回転行列
Coord Trl; 並進成分
}FRAME;
<表示属性用構造体>
typedef struct{
float Color[4]; 色(r,g,b,?)
}DispStat;
■クラス
なし
■関数(Global)
<Geometric, 2D/3D Vector>
void InitCoord(Coord *a)
void InitCoord(Coord *a,int n)
<説明>
座標値(x,y,z,dmy)を(0,0,0,0)で初期化する.
<引数>
*a:初期化したいCoord構造体へのポインタ/配列
n:配列数
<戻り値>
なし
Coord AddCoord(Coord A, Coord B)
Coord AddCoord(Coord A, double b)
Coord AddCoord(Coord A, double b, double c, doule d)
<説明>
座標値の足し算.
ans = (A.x+B.x, A.y+B.y, A.z+B.z) あるいは、
ans = (A.x+b, A.y+b, A.z+b) あるいは、
ans = (A.x+b, A.y+c, A.z+d)
<引数>
加算する2つのCoord. あるいは、片方をdouble値とすることも可能.
<戻り値>
加算の結果
Coord DivCoord(Coord A, Coord B);
Coord DivCoord(Coord A, double b);
Coord DivCoord(Coord A, double b, double c, double d)
<説明>
座標値の割り算
ans = (A.x/B.x, A.y/B.y, A.z/B.z) あるいは、
ans = (A.x/b, A.y/b, A.z/b) あるいは、
ans = (A.x/b, A.y/c, A.z/d)
<引数>
除算する2つのCoord. あるいは、片方をdouble値とすることも可能.
<戻り値>
除算の結果
Coord MulCoord(Coord A, Coord B);
Coord MulCoord(Coord A, double b);
Coord MulCoord(Coord A, double b, double c, double d);
<説明>
座標値の掛け算
ans = (A.x*B.x, A.y*B.y, A.z*B.z) あるいは、
ans = (A.x*b, A.y*b, A.z*b) あるいは、
ans = (A.x*b, A.y*c, A.z*d)
<引数>
乗算する2つのCoord. あるいは、片方をdouble値とすることも可能.
<戻り値>
乗算の結果
Coord SubCoord(Coord A, Coord B);
Coord SubCoord(Coord A, double b);
Coord SubCoord(Coord A, double b, double c, double d);
<説明>
座標値の掛け算
ans = (A.x-B.x, A.y-B.y, A.z-B.z) あるいは、
ans = (A.x-b, A.y-b, A.z-b) あるいは、
ans = (A.x-b, A.y-c, A.z-d)
<引数>
減算する2つのCoord. あるいは、片方をdouble値とすることも可能.
<戻り値>
減算の結果
Coord SetCoord(Coord A);
Coord SetCoord(double a, double b, double c);
<説明>
座標値の代入
ans = (A.x, A.y, A.z) あるいは
ans = (a,b,c)
<引数>
代入するCoord. あるいは、double型
<戻り値>
代入の結果
int DiffCoord(Coord A, Coord B);
<説明>
AとBを比較する. APPROX_ZERO以内で一致しているか
<引数>
比較したい座標値
<戻り値>
A = B:KOD_TRUE A != B:
Coord AbsCoord(Coord a)
<説明>
座標値の絶対値を返す
<引数>
絶対値を求めたいCoord.
<戻り値>
絶対値化されたCoord.
Coord NormalizeVec(Coord A);
Coord NormalizeVec(double a, double b, double c);
<説明>
空間ベクトルを単位化する
<引数>
単位化したいCoord. あるいはdouble型
<戻り値>
単位化されたCoord
double CalcEuclid(Coord A);
double CalcEuclid2D(double a, double b);
<説明>
原点からの3次元(2次元)ユークリッド距離を求める
<引数>
距離を求めたい座標値
<戻り値>
距離
double CalcDistance(Coord A,Coord B);
double CalcDistance2D(Coord A,Coord B);
<説明>
2点間の3次元(2次元)ユークリッド距離を求める
<引数>
距離を求めたい2つの座標値
<戻り値>
距離
double CalcInnerProduct(Coord A, Coord B);
double CalcInnerProduct(Coord A, double b, double c, double d);
<説明>
内積を求める
<引数>
内積を求めたい2つの空間ベクトル、あるいは一方をdouble型で指定
<戻り値>
内積(スカラー)
Coord CalcOuterProduct(Coord A, Coord B);
<説明>
外積を求める
<引数>
外積を求めたい2つの空間ベクトル
<戻り値>
外積(空間ベクトル)
double CalcVecAngle2D(Coord A, Coord B);
<説明>
2つの空間ベクトルと原点とのなす角を求める
<引数>
角度を求めたい2つの空間ベクトル
<戻り値>
角度(rad)
Coord CalcInterDivPt(Coord A, Coord B, double t);
<説明>
2点間の内分点を求める
<引数>
A,B:内分点を求めたい2つの座標値
t:内分率(0〜1)
<戻り値>
内分点
Coord CalcOrthoProjection(Coord p, Coord n, Coord q);
<説明>
任意の点を任意の平面へ正射影する
<引数>
p:任意の平面上の1点
n:任意の平面の単位法線ベクトル
q:正射影したい点
<戻り値>
正射影された平面上の点
double CalcDistPtToPlane(Coord Pt,Coord P0,Coord N)
<説明>
任意の点から任意の平面までの距離を求める
<引数>
Pt:任意の点 P0:平面上の1点 N:平面の法線ベクトル
<戻り値>
計算結果
double CalcScalarTriProduct(Coord A, Coord B, Coord C);
<説明>
スカラー三重積を求める
<引数>
スカラー三重積を求めたい3つの空間ベクトル
<戻り値>
スカラー三重積
Coord CalcRotVec(Coord A, Coord B, double Ang);
Coord CalcRotVec2D(Coord A, double Ang);
<説明>
任意のベクトルを原点を通る任意軸周り(2Dの場合はz軸周り)に回転させたベクトルを求める
<引数>
A:回転させたい位置ベクトル
B:回転軸方向ベクトル(単位ベクトル)
Ang:回転角(rad)
<戻り値>
回転後のベクトル
Coord CalcNormalLine(Coord P,Coord A,Coord u)
<説明>
任意の点Pから任意の直線(点Aを通り単位ベクトルuの方向を持つ)へ下ろした点を求める
<引数>
Coord P : 任意の点
Coord A : 直線上の点
Coord u : 直線の方向を示す単位ベクトル
<戻り値>
計算結果
Coord Arc_CP(Coord A, Coord B, double Cos);
<説明>
円の中心点から円上に接する任意の2本の接線が交わる点へのベクトルを求める(中心角0<θ<π)
<引数>
A:円弧をなすベクトル1
B:円弧をなすベクトル2
Cos:中心角の余弦
<戻り値>
計算結果
int IsPointInPolygon(Coord TargetPoint, Coord *BorderPoint, int CountPoint);
<説明>
注目点の多角形内外判別を行う(x-y平面内).
<引数>
TargetPoint:内外判定をする注目点
*BorderPoint:多角形の頂点群
CountPoint:頂点の数
<戻り値>
KOD_TRUE:内
KOD_FALSE:外
KOD_ONEDGE:エッジ上
<Homoheneous Transform>
<Rotation Matrix>
Coord MulFrameCoord(FRAME F, Coord A);
Coord MulFrameCoord(double R[3][3], double T[3], Coord A);
<説明>
同次変換行列と座標値(3Dベクトル)との掛け算
<引数>
F or R,T:同次変換行列をFまたは回転行列と並進ベクトルの成分で与える
A:座標値
<戻り値>
乗算結果
FRAME MulFrame(FRAME F, FRAME G);
<説明>
同次変換行列同士の掛け算
<引数>
2つの同次変換行列
<戻り値>
乗算結果
FRAME InvFrame(FRAME F);
<説明>
同次変換行列の逆行列を得る
<引数>
逆行列を求めたい同次変換行列
<戻り値>
逆行列を格納した同次変換行列
Coord RotToZYZEuler(Coord R[3]);
<説明>
回転行列をz-y-zオイラー角へ変換
<引数>
Pict 回転行列
<戻り値>
ans.x = z-y-zオイラー角a
ans.y = z-y-zオイラー角b
ans.z = z-y-zオイラー角c
InitFrame(FRAME *F);
<説明>
Frame内変数を全て0に初期化
<引数>
初期化したい同次変換行列へのポインタ
<戻り値>
なし
<Multi Dimension Vector>
<Multi Dimension Matrix>
void InitVector(Vector V, int n);
void InitMatrix(Matrix M, int m, int n);
<説明>
n次元ベクトル、m*n次元行列の初期化(0クリア)
<引数>
V,M:初期化したいベクトルorマトリックス
n,m:ベクトルorマトリックスの配列長
<戻り値>
なし
void MulMxMx(Matrix A, int m1, int n1, Matrix B, int m2, int n2, Matrix Ans);
<説明>
m1*n1行列Aとm2*n2行列Bの掛け算
<引数>
A,B:乗算したい行列
m1,n1:Aの行数、列数
m2,n2:Bの行数、列数
Ans:乗算結果
<戻り値>
なし
void MulMxVec(Matrix M, int m1, int n1 ,Vector V, int n2, Vector Ans);
void MulMxVec(Matrix M, int m1, int n1, Coord *A, Coord *Ans);
<説明>
m1*n1行列Aとn2次元ベクトルVの掛け算
m1*n1行列Aと座標値(3次元空間ベクトル)Aの掛け算
<引数>
M,V,A:乗算したい行列、ベクトル、座標値
m1,n1:Aの行数、列数
n2:Vの次元数
Ans:乗算結果
<戻り値>
なし
Coord MulMxCoord(Coord A[3], Coord B);
Coord MulMxCOord(Matrix M, Coord B);
<説明>
3*3行列(A[3] or M)と3次元ベクトルBの掛け算
<引数>
A[3],M:3*3行列
B:3次元ベクトルをCoordで指定
<戻り値>
乗算結果
void TranMx(Matrix M, int n, Matrix Ans);
void TranMx(Coord A[3], Coord B[3]);
<説明>
n*n行列Mまたは3*3行列A[3]の転置行列を得る
<引数>
M:転置するn*n行列
n:行列Mの行/列数
A[3]:転置する3*3行列 (注意)Mは次元任意だが、Aは3次元
Ans,B[3]:転置された行列
<戻り値>
なし
double Gauss(int n, Matrix A, Vector B, Vector X);
<説明>
ガウスの消去法を用いて連立1次方程式を解く
[A]{X} = {B} {X} = [A]^{B} [A]^:[A]の逆行列
<引数>
n:行/列数
A:n*nの係数行列 (注意)出力としてLU分解された結果が格納される
B:n次元の右辺ベクトル
X:n次元の変数ベクトル
<戻り値>
行列式
メモリー確保に失敗した場合はKOD_ERR
行列式の絶対値がAPPROX_ZERO以下の場合はKOD_FALSE
double LU(int n, Matrix M, int *Ip);
<説明>
LU分解ルーチン
<引数>
n:行/列数
M:n*n行列 (注意)出力としてLU分解された結果が格納される
*Ip:行交換の情報が格納される(n個のint配列を用意すること)
<戻り値>
行列式
double MatInv(int n, Matrix M, Matrix Ans);
double MatInv2(Matrix M, Matrix Ans);
double MatInv3(Matrix M, Matrix Ans);
<説明>
n*n行列の逆行列を求める
2*2/3*3行列の場合はMatInv2/MatInv3を用いた方が早い
<引数>
n:行/列数
M:n*n正方行列
Ans:逆行列
<戻り値>
行列式
行列式の絶対値がAPPROX_ZERO以下の場合はKOD_FALSE
<Numerical Calculation>
double DegToRad(double deg);
double RadToDeg(double rad);
<説明>
角度の単位を変換する(deg <--> rad)
<引数>
変換したい単位の角度
<戻り値>
変換された単位の角度
int CalcCubicEquation(double *A, double *Ans);
int CalcQuadraticEquation(double *A, double *Ans);
int CalcLinearEquation(double *A, double *Ans);
<説明>
3/2/1次方程式を解く
<引数>
*A:係数を格納した配列 ex)A[0]X^3 + A[1]X^2 + A[2]X + A[3] = 0
Ans:解 X = Ans[0], Ans[1], Ans[2]
<戻り値>
解の個数
解が存在しない場合はKOD_FALSE
int nCr(int n,int r);
<説明>
2項係数(nCrの組み合わせ総数)を求める。
<引数>
n:nCrのn
r:nCrのr
<戻り値>
計算結果
<Drawing>
void DrawPoint(Coord P, double Scale, double Width, double Color[3]);
<説明>
点を描画する
<引数>
P:点の座標値
Scale:PをScale倍する
Width:点のサイズ
Color[3]:点の色をRGBで指定 (0<= r,g,b <=1)
<戻り値>
なし
void DrawVector(Coord P, Coord T, double Len, double Width, double Color[3]);
<説明>
ベクトルを描画する
<引数>
P:始点の座標値
T:方向ベクトル
Len:描画する線分の長さ
Width:描画する線分の太さ
Color[3]:線の色をRGBで指定 (0<= r,g,b <=1)
<戻り値>
なし
void DrawVector(Coord P, Coord T, double Len, double Width, double Color[3]);
<説明>
ベクトルを描画する
<引数>
P:始点の座標値
T:方向ベクトル
Len:描画する線分の長さ
Width:描画する線分の太さ
Color[3]:線の色をRGBで指定 (0<= r,g,b <=1)
<戻り値>
なし
void DrawLine(Coord P, Coord Q, double Width, double Color[3]);
<説明>
2点を繋ぐ線分を描画する
<引数>
P:始点の座標値
Q:終点の座標値
Width:描画する線分の太さ
Color[3]:線の色をRGBで指定 (0<= r,g,b <=1)
<戻り値>
なし
<Memory Allocation>
Matrix NewMatrix(int m, int n);
<説明>
m*n行列配列のメモリー確保
<引数>
m,n:行、列の次元数
<戻り値>
確保されたMatrix配列のポインタ
メモリー確保に失敗した場合はNULL
Vector NewVector(int n);
<説明>
n次元ベクトルのメモリー確保
<引数>
n:次元数
<戻り値>
確保されたVector配列のポインタ
メモリー確保に失敗した場合はNULL
void FreeMatrix(Matrix M, int m);
<説明>
Matrix配列のメモリー解放
<引数>
M:メモリー解放したいMatrix配列
m:Mの行数
<戻り値>
なし
void FreeVector(Vector V);
<説明>
Vector配列のメモリー解放
<引数>
V:メモリー解放したいVector配列
<戻り値>
なし
Coord *NewCoord1(int n);
Coord **NewCoord2(int m, int n);
<説明>
1/2次元Coord配列のメモリー確保
<引数>
m,n:行、列の次元数
<戻り値>
確保されたCoord配列のポインタ
メモリー確保に失敗した場合はNULL
void FreeCoord1(Coord *A);
void FreeCoord2(Coord **A, int m);
<説明>
1/2次元Coord配列のメモリー解放
<引数>
A:1/2次元Coord配列
m:**Aの行数
<戻り値>
なし
<Console Operation>
void SetMessage(char *Mes);
<説明>
コンソールにメッセージを出力
<引数>
Mes:出力したいメッセージ文字列
<戻り値>
なし
void SetCommand(char *cmd);
<説明>
コンソールにコマンドを出力
<引数>
cmd:出力したいコマンド文字列
<戻り値>
なし
<Others>
double sgn(double N);
<説明>
数値の符号判定
<引数>
N:符号判定したいdouble型の数値
<戻り値>
Nが負の場合:-1
Nが0の場合:0
Nが正の場合:1
がdouble型で返る
int CheckZero(double val,int flag);
<説明>
値がAPPROX_ZEROの範囲で0であるかチェックする。
<引数>
val:入力値
flag:精度(HIGH_ACCURACY or LOW_ACCURACY)
<戻り値>
0とみなせる:KOD_TRUE
0とみなせない:KOD_FALSE
失敗:KOD_ERR
int CheckRange(double low,double up,double val,int flag);
<説明>
指定した値が指定した範囲内であるかをチェック
<引数>
low:下限
up:上限
val:調べたい値
flag = 0:(low <= val <= up) --> (low-ap < val < up+ap),
1:(low < val < up) --> (low+ap < val < up-ap),
2:(val <= up) --> (val < up+ap),
3:(val < up) --> (val < up-ap),
4:(low <= val) --> (low-ap < val),
v5:(low < val) --> (low+ap < val)
<戻り値>
範囲内:KOD_TRUE
範囲外:KOD_FALSE
失敗:KOD_ERR
void BubbleSort(int Arr[], int n);
void BubbleSort(double Arr[], int n);
<説明>
バブルソートを行う
<引数>
Arr[]:ソートしたいint/double型配列
n:配列長
<戻り値>
なし
int QCmp(const void *a, const void *b);
<説明>
C言語標準関数qsort()用比較関数
2つのdouble型数値の大小比較を行う
<引数>
a,b:比較したいdouble型の数値
<戻り値>
a < b:1
a > b:-1
それ以外は0
【Quaternion.h】
クォータニオン演算モジュールを提供します。
■シンボル
#define TMATELEMNUM -16 回転行列配列の要素数
■インクルードライブラリ
"StdAfx.h"
■型定義
<クォータニオン構造体>
typedef struct{
double t; 実部
double x,y,z; 虚部
}Quat;
■クラス
QUATERNION
<継承>
なし
■関数(Class QUATERNION)
public
Quat QInit(double t, double x, double y, double z);
<説明>
クォータニオンを引数で指定した数値で初期化します。
<引数>
t,x,y,z:Quat構造体変数
<戻り値>
初期化されたQuat
public
Quat QGenRot(double r, double x, double y, doublez);
<説明>
回転クォータニオンqを生成します。 q = {cos(r/2): x*sin(r/2), y*sin(r/2), z*sin(r/2)}
<引数>
r:回転角[rad]
x,y,z:回転軸ベクトル
<戻り値>
回転演算用Quat
public
Quat QRot(Quat r, Quat p, Quat q);
<説明>
クォータニオンによるベクトルの回転演算を実行します。
<引数>
r:qの共役クォータニオン
p:回転したい座標値を持ったクォータニオン
q:回転クォータニオン
<戻り値>
回転演算結果
public
Quat QConjugation(Quat r);
<説明>
共役クォータニオンを生成
<引数>
r:クォータニオン
<戻り値>
共役クォータニオン
public
Quat QMult(Quat p, Quat q);
<説明>
クォータニオン同士の積
<引数>
p,q:乗算する2つのクォータニオン
<戻り値>
計算結果
public
void QtoR(double r[15] ,Quat q);
void QtoR(double r[15], double t, double x, double y, double z);
<説明>
クォータニオンqを同次変換行列rへ変換する
<引数>
r:4*4行列を1次元配列で表現したもの。
q:クォータニオン
<戻り値>
なし
public
Coord QtoC(Quat q);
<説明>
クォータニオンの3つの虚部をCoord構造体に代入する。
<引数>
q:代入したいクォータニオン
<戻り値>
クォータニオン虚部x,y,zをそのままCoord x,y,zとしたもの
public
Quat CtoQ(Coord C);
<説明>
クォータニオン実部を0、虚部をCoordとしてクォータニオンを生成
<引数>
C:Coordで表現されたクォータニオンの虚部
<戻り値>
生成されたクォータニオン
public
Quat QSlerp(Quat p, Quat q, double t);
<説明>
クォータニオンによる球面線形補間
2つのベクトルp(t=0)、q(t=1)間をtで指定した内分上に球面線形補間する
※p,qは単位ベクトルであること
<引数>
p,q:分割したい両端の点の座標を虚部としたクォータニオン
t:内分比
<戻り値>
補間された点を表すクォータニオン