motionLib/func.cpp at master · songtianyi/motionLib · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
#include "func.h"

void __matrixMultiXYZ(FLOAT *rs,const FLOAT *a,const FLOAT *b,const int X,const int Y,const int Z)
{
    FLOAT *tmp = new FLOAT[X*Z];
	for(int i = 0;i < X*Z;i++)
	{
		tmp[i] = 0.0f;
	}
    for(int i = 0;i < X;i++)
    {
        for(int j = 0;j < Z;j++)
        {
            for(int k = 0;k < Y;k++)
            {
                tmp[i*Z + j] += a[i*Y+k]*b[k*Z+j];
            }
        }
    }
    memcpy(rs,tmp,sizeof(FLOAT)*X*Z);
    delete [] tmp; tmp = 0;
}
void 	Pitch(CVector3f tarVec,CVector3f curVec,FLOAT &pitch)//x
{
	tarVec.x = 0;
	tarVec.normalize();
	curVec.x =0;
	curVec.normalize();
	if( (curVec.y == 0 && curVec.z == 0)
	||  (tarVec.y == 0 && tarVec.z == 0))//if curVec or tarVec is zero-vector
	{
		pitch = 0;return;
	}
	FLOAT dot = dotpdut(tarVec,curVec);
	if (dot < 0.9999 && dot > -0.9999)
	{
		CVector3f axis = cropdut(curVec,tarVec);
		if(axis.x > 0)
		{
			pitch =  acos(dot) * H3DMath::M_RAD2DEG;
		}
		else
		{
			pitch =  -acos(dot) * H3DMath::M_RAD2DEG;
		}
	}
	else
	{
		pitch = 0;
	}
}
void 	Yaw(CVector3f tarVec,CVector3f curVec,FLOAT &yaw)//y
{
	tarVec.y = 0;
	tarVec.normalize();
	curVec.y =0;
	curVec.normalize();
	if((curVec.x == 0 && curVec.z == 0)
	||  (tarVec.x == 0 && tarVec.z == 0))
	{
		yaw = 0;return;
	}
	FLOAT dot = dotpdut(tarVec,curVec);
	if (dot < 0.9999 && dot > -0.9999)
	{
		CVector3f axis = cropdut(tarVec,curVec);
		if(axis.y > 0)
		{
			yaw =  acos(dot) * H3DMath::M_RAD2DEG;
		}
		else
		{
			yaw =  -acos(dot) * H3DMath::M_RAD2DEG;
		}
	}
	else
	{
		yaw = 0;
	}
}
void 	Roll(CVector3f tarVec,CVector3f curVec,FLOAT &roll)//z
{
	tarVec.z = 0;
	tarVec.normalize();
	curVec.z =0;
	curVec.normalize();
	if((curVec.x == 0 && curVec.y == 0)
	||  (tarVec.x == 0 && tarVec.y == 0))
	{
		roll = 0;return;
	}
	FLOAT dot = dotpdut(tarVec,curVec);
	if (dot < 0.9999 && dot > -0.9999)
	{
		CVector3f axis = cropdut(tarVec,curVec);
		if(axis.z > 0)
		{
			roll =  acos(dot) * H3DMath::M_RAD2DEG;
		}
		else
		{
			roll =  -acos(dot) * H3DMath::M_RAD2DEG;
		}
	}
	else
	{
		roll = 0;
	}
}

CQuaternion CCD(CVector3f root,CVector3f curEnd,CVector3f desireEnd)
{
	//Local Variables
	FLOAT	cosAngle,turnAngle;
	CVector3f curVector,targetVector;
	CQuaternion quat(0,0,0,1);
	FLOAT IK_POS_THRESH = 0.0000029403;
	// SEE IF I AM ALREADY CLOSE ENOUGH
	if (euclideanDist(curEnd, desireEnd) > IK_POS_THRESH){
			// CREATE THE VECTOR TO THE CURRENT EFFECTOR POS
			curVector	= curEnd	- root;
			// CREATE THE DESIRED EFFECTOR POSITION VECTOR
			targetVector= desireEnd - root;
			// NORMALIZE THE VECTORS (EXPENSIVE, REQUIRES A SQRT)
			curVector.normalize();
			targetVector.normalize();
			// THE DOT PRODUCT GIVES ME THE COSINE OF THE DESIRED ANGLE
			cosAngle = dotpdut(targetVector,curVector);

			assert(! (fabs(cosAngle)-0.0000001 > 1));//fabscosAngle = [0,1]
			// IF THE DOT PRODUCT RETURNS 1.0, I DON'T NEED TO ROTATE AS IT IS 0 DEGREES
			if(cosAngle < 0.99999999999)
			{
				// USE THE CROSS PRODUCT TO CHECK WHICH WAY TO ROTATE
				CVector3f crossResult = cropdut(curVector,targetVector);
				crossResult.normalize();			//normalize the vector

				turnAngle = acos((FLOAT)cosAngle);	// GET THE ANGLE

				//calculate quaternion
				CQuaternion quat(\
					crossResult.x*sin(turnAngle/2),\
					crossResult.y*sin(turnAngle/2),\
					crossResult.z*sin(turnAngle/2),\
					cos(turnAngle/2));

				quat.normalize();
				return quat;
			}
	}
	return quat;
}