package a3test.debug;

import jp.sourceforge.acerola3d.a3.*;

import javax.vecmath.*;
import static java.lang.Math.*;

public class UtilTest {
    public static void main(String args[]) throws Exception {
        //euler2quatTest();//新旧のeuler2quatの比較
        //eqe();//オイラー角->四元数->オイラー角で誤差判定
        //qeq();//四元数->オイラー角->四元数で誤差判定
        //qmq();//四元数->行列->四元数で誤差判定
        //a2bQuatTest();//a2bQuatのテスト
        frontFacingQuatTest();//frontFacingQuatのテスト
    }

    //新旧のeuler2quatの比較
    static void euler2quatTest() {
        for (int i=0;i<10;i++) {
            //Vector3d v = randomEuler();
            Quat4d q = null;
            //q = Util.euler2quat_NEW(v);
            System.out.print("new:");printQuat(q);
            //q = Util.euler2quat_OLD(v);
            System.out.print("old:");printQuat(q);
        }
    }

    //オイラー角->四元数->オイラー角で誤差判定
    static void eqe() { 
        for (int i=0;i<100;i++) {
            Vector3d v1 = randomEuler();
            Quat4d q = Util.euler2quat(v1);
            Vector3d v2 = Util.quat2euler(q);
            v1.sub(v2);
            printVec(v1);
        }
    }

    //四元数->オイラー角->四元数で誤差判定
    static void qeq() { 
        for (int i=0;i<100;i++) {
            Quat4d q1 = randomQuat();
            Vector3d v = Util.quat2euler(q1);
            Quat4d q2 = Util.euler2quat(v);
            q1.sub(q2);
            printQuat(q1);
        }
    }

    //四元数->行列->四元数で誤差判定
    static void qmq() {
        for (int i=0;i<100;i++) {
            Quat4d q1 = randomQuat();
            Matrix3d m = Util.quat2matrix(q1);
            Quat4d q2 = Util.matrix2quat(m);
            q1.sub(q2);
            printQuat(q1);
        }
    }

    //a2bQuatのテスト
    static void a2bQuatTest() {
        for (int i=0;i<100;i++) {
            Vector3d a = randomVec();
            Vector3d b = randomVec();
            Quat4d q = Util.a2bQuat(a,b);
            Vector3d bb = Util.trans(q,a);
            bb.sub(b);
            printVec(bb);
        }
    }

    //frontFacingQuatのテスト
    static void frontFacingQuatTest() {
        for (int i=0;i<100;i++) {
            Vector3d a = randomVec();
            a.normalize();
            Vector3d aa = randomVec();
            double d = aa.dot(a);
            Vector3d aaa = new Vector3d(a);
            aaa.scale(d);
            aa.sub(aaa);
            Vector3d b = randomVec();
            b.normalize();
            Vector3d up = randomVec();
            Quat4d q = Util.frontFacingQuat(a,aa,b,up);
            Vector3d bb = Util.trans(q,a);
            bb.sub(b);
            printVec(bb);
        }
    }

    //ランダムなEuler角の生成
    static Vector3d randomEuler() {
        Vector3d v = new Vector3d();
        v.x = 1.0*PI*(random()-0.5);
        v.y = 1.0*PI*(random()-0.5);
        v.z = 1.0*PI*(random()-0.5);
        return v;
    }

    //ランダムな四元数の生成
    static Quat4d randomQuat() {
        Vector3d v = new Vector3d();
        v.x = 2.0*(random()-0.5);
        v.y = 2.0*(random()-0.5);
        v.z = 2.0*(random()-0.5);
        v.normalize();
        double t = PI*(random()-0.5)/2.0;
        Quat4d q = new Quat4d();
        q.set(sin(t)*v.x,sin(t)*v.y,sin(t)*v.z,cos(t));
        return q;
    }

    //ランダムなベクトルの生成
    static Vector3d randomVec() {
        Vector3d v = new Vector3d();
        v.x = 10.0*(random()-0.5);
        v.y = 10.0*(random()-0.5);
        v.z = 10.0*(random()-0.5);
        return v;
    }

    //Vector3dの表示
    static void printVec(Vector3d v) {
        System.out.printf("(%5.2f,%5.2f,%5.2f)\n",v.x,v.y,v.z);
    }

    //Quat4dの表示
    static void printQuat(Quat4d q) {
        System.out.printf("(%5.2f,%5.2f,%5.2f,%5.2f)\n",q.x,q.y,q.z,q.w);
    }
}
