public final void setAngle(int i_x, int i_y, int i_z) {
/*
* |cos(a) -sin(a) 0| |cos(b) 0 sin(b)| |cos(a-c) sin(a-c) 0| rot = |sin(a) cos(a) 0| |0 1 0 | |-sin(a-c) cos(a-c) 0| |0 0 1| |-sin(b) 0 cos(b)| |0 0 1|
*/
long Sa, Sb, Ca, Cb, Sac, Cac, CaCb, SaCb;
Sa = NyMath.sinFixedFloat24(i_x);
Ca = NyMath.cosFixedFloat24(i_x);
Sb = NyMath.sinFixedFloat24(i_y);
Cb = NyMath.cosFixedFloat24(i_y);
Sac = NyMath.sinFixedFloat24(i_x - i_z);
Cac = NyMath.cosFixedFloat24(i_x - i_z);
CaCb = (Ca * Cb) >> 24;
SaCb = (Sa * Cb) >> 24;
this.m00 = (CaCb * Cac + Sa * Sac) >> 24;
this.m01 = (CaCb * Sac - Sa * Cac) >> 24;
this.m02 = (Ca * Sb) >> 24;
this.m10 = (SaCb * Cac - Ca * Sac) >> 24;
this.m11 = (SaCb * Sac + Ca * Cac) >> 24;
this.m12 = (Sa * Sb) >> 24;
this.m20 = (-Sb * Cac) >> 24;
this.m21 = (-Sb * Sac) >> 24;
this.m22 = Cb;
// angleを逆計算せずに直接代入
this._angle.x = i_x;
this._angle.y = i_y;
this._angle.z = i_z;
return;
}
public final void initRotBySquare(
final NyARLinear[] i_linear, final NyARFixedFloat16Point2d[] i_sqvertex)
throws NyARException {
final NyARFixedFloatRotVector vec1 = this.__initRot_vec1;
final NyARFixedFloatRotVector vec2 = this.__initRot_vec2;
// 向かい合った辺から、２本のベクトルを計算
// 軸１
vec1.exteriorProductFromLinear(i_linear[0], i_linear[2]);
vec1.checkVectorByVertex(i_sqvertex[0], i_sqvertex[1]);
// 軸２
vec2.exteriorProductFromLinear(i_linear[1], i_linear[3]);
vec2.checkVectorByVertex(i_sqvertex[3], i_sqvertex[0]);
// 回転の最適化？
NyARFixedFloatRotVector.checkRotation(vec1, vec2);
this.m00 = vec1.v1 << 8;
this.m10 = vec1.v2 << 8;
this.m20 = vec1.v3 << 8;
this.m01 = vec2.v1 << 8;
this.m11 = vec2.v2 << 8;
this.m21 = vec2.v3 << 8;
// 最後の軸を計算
final long w02 = (vec1.v2 * vec2.v3 - vec1.v3 * vec2.v2) >> 16; // S16
final long w12 = (vec1.v3 * vec2.v1 - vec1.v1 * vec2.v3) >> 16; // S16
final long w22 = (vec1.v1 * vec2.v2 - vec1.v2 * vec2.v1) >> 16; // S16
final long w = NyMath.sqrtFixdFloat((w02 * w02 + w12 * w12 + w22 * w22) >> 16, 16); // S16
this.m02 = (w02 << 24) / w;
this.m12 = (w12 << 24) / w;
this.m22 = (w22 << 24) / w;
// Matrixからangleをロード
this.updateAngleFromMatrix();
return;
}
/**
* int arGetAngle( double rot[3][3], double *wa, double *wb, double *wc )
* Optimize:2008.04.20:STEP[481→433] 3x3変換行列から、回転角を復元して返します。
*
* @param o_angle
* @return
*/
private final void updateAngleFromMatrix() {
int a, b, c;
long sina, cosa, sinb, cosb, sinc, cosc;
if (this.m22 > NyMath.FIXEDFLOAT24_1) { // <Optimize/>if( rot[2][2] > 1.0 ) {
this.m22 = NyMath.FIXEDFLOAT24_1; // <Optimize/>rot[2][2] = 1.0;
} else if (this.m22 < -NyMath.FIXEDFLOAT24_1) { // <Optimize/>}else if( rot[2][2] < -1.0 ) {
this.m22 = -NyMath.FIXEDFLOAT24_1; // <Optimize/>rot[2][2] = -1.0;
}
cosb = this.m22; // <Optimize/>cosb = rot[2][2];
b = NyMath.acosFixedFloat16((int) cosb);
sinb = (long) NyMath.sinFixedFloat24(b);
final long rot02 = this.m02;
final long rot12 = this.m12;
if (sinb != 0) {
cosa = (rot02 << 24) / sinb; // <Optimize/>cosa = rot[0][2] / sinb;
sina = (rot12 << 24) / sinb; // <Optimize/>sina = rot[1][2] / sinb;
if (cosa > NyMath.FIXEDFLOAT24_1) {
/* printf("cos(alph) = %f\n", cosa); */
cosa = NyMath.FIXEDFLOAT24_1;
sina = 0;
}
if (cosa < -NyMath.FIXEDFLOAT24_1) {
/* printf("cos(alph) = %f\n", cosa); */
cosa = -NyMath.FIXEDFLOAT24_1;
sina = 0;
}
if (sina > NyMath.FIXEDFLOAT24_1) {
/* printf("sin(alph) = %f\n", sina); */
sina = NyMath.FIXEDFLOAT24_1;
cosa = 0;
}
if (sina < -NyMath.FIXEDFLOAT24_1) {
/* printf("sin(alph) = %f\n", sina); */
sina = -NyMath.FIXEDFLOAT24_1;
cosa = 0;
}
a = NyMath.acosFixedFloat16((int) cosa);
if (sina < 0) {
a = -a;
}
// <Optimize>
// sinc = (rot[2][1]*rot[0][2]-rot[2][0]*rot[1][2])/(rot[0][2]*rot[0][2]+rot[1][2]*rot[1][2]);
// cosc =
// -(rot[0][2]*rot[2][0]+rot[1][2]*rot[2][1])/(rot[0][2]*rot[0][2]+rot[1][2]*rot[1][2]);
final long tmp = DIV0CANCEL + ((rot02 * rot02 + rot12 * rot12) >> 24);
sinc = (this.m21 * rot02 - this.m20 * rot12) / tmp;
cosc = -(rot02 * this.m20 + rot12 * this.m21) / tmp;
// </Optimize>
if (cosc > NyMath.FIXEDFLOAT24_1) {
/* printf("cos(r) = %f\n", cosc); */
cosc = NyMath.FIXEDFLOAT24_1;
sinc = 0;
}
if (cosc < -NyMath.FIXEDFLOAT24_1) {
/* printf("cos(r) = %f\n", cosc); */
cosc = -NyMath.FIXEDFLOAT24_1;
sinc = 0;
}
if (sinc > NyMath.FIXEDFLOAT24_1) {
/* printf("sin(r) = %f\n", sinc); */
sinc = NyMath.FIXEDFLOAT24_1;
cosc = 0;
}
if (sinc < -NyMath.FIXEDFLOAT24_1) {
/* printf("sin(r) = %f\n", sinc); */
sinc = -NyMath.FIXEDFLOAT24_1;
cosc = 0;
}
c = (int) NyMath.acosFixedFloat16((int) cosc);
if (sinc < 0) {
c = -c;
}
} else {
a = b = 0;
cosa = cosb = NyMath.FIXEDFLOAT24_1;
sina = sinb = 0;
cosc = this.m00; // cosc = rot[0];// <Optimize/>cosc = rot[0][0];
sinc = this.m01; // sinc = rot[1];// <Optimize/>sinc = rot[1][0];
if (cosc > NyMath.FIXEDFLOAT24_1) {
/* printf("cos(r) = %f\n", cosc); */
cosc = NyMath.FIXEDFLOAT24_1;
sinc = 0;
}
if (cosc < -NyMath.FIXEDFLOAT24_1) {
/* printf("cos(r) = %f\n", cosc); */
cosc = -NyMath.FIXEDFLOAT24_1;
sinc = 0;
}
if (sinc > NyMath.FIXEDFLOAT24_1) {
/* printf("sin(r) = %f\n", sinc); */
sinc = NyMath.FIXEDFLOAT24_1;
cosc = 0;
}
if (sinc < -NyMath.FIXEDFLOAT24_1) {
/* printf("sin(r) = %f\n", sinc); */
sinc = -NyMath.FIXEDFLOAT24_1;
cosc = 0;
}
c = NyMath.acosFixedFloat16((int) cosc);
if (sinc < 0) {
c = -c;
}
}
this._angle.x = (long) a; // wa.value=a;//*wa = a;
this._angle.y = (long) b; // wb.value=b;//*wb = b;
this._angle.z = (long) c; // wc.value=c;//*wc = c;
return;
}
