Ejemplo n.º 1
0
/**< Get the axis and angle of rotation from a quaternion*/
void kmQuaternionToAxisAngle(const kmQuaternion* pIn, kmVec3* pAxis, kmScalar* pAngle)
{
	kmScalar	scale;			/* temp vars*/
	kmQuaternion tmp;

	if(pIn->w > 1.0) {
		kmQuaternionNormalize(&tmp, pIn);
	} else {
		kmQuaternionAssign(&tmp, pIn);
	}

	*pAngle = 2.0 * acosf(tmp.w);
	scale = sqrtf(1.0 - kmSQR(tmp.w));

	if (scale < kmEpsilon) {	/* angle is 0 or 360 so just simply set axis to 0,0,1 with angle 0*/
		pAxis->x = 0.0f;
		pAxis->y = 0.0f;
		pAxis->z = 1.0f;
	} else {
		pAxis->x = tmp.x / scale;
		pAxis->y = tmp.y / scale;
		pAxis->z = tmp.z / scale;
		kmVec3Normalize(pAxis, pAxis);
	}
}
Ejemplo n.º 2
0
///< Create a quaternion from yaw, pitch and roll
kmQuaternion* kmQuaternionRotationYawPitchRoll(kmQuaternion* pOut,
												kmScalar yaw,
												kmScalar pitch,
												kmScalar roll)
{
	kmScalar	ex, ey, ez;		// temp half euler angles
	kmScalar	cr, cp, cy, sr, sp, sy, cpcy, spsy;		// temp vars in roll,pitch yaw

	ex = kmDegreesToRadians(pitch) / 2.0f;	// convert to rads and half them
	ey = kmDegreesToRadians(yaw) / 2.0f;
	ez = kmDegreesToRadians(roll) / 2.0f;

	cr = cosf(ex);
	cp = cosf(ey);
	cy = cosf(ez);

	sr = sinf(ex);
	sp = sinf(ey);
	sy = sinf(ez);

	cpcy = cp * cy;
	spsy = sp * sy;

	pOut->w = cr * cpcy + sr * spsy;

	pOut->x = sr * cpcy - cr * spsy;
	pOut->y = cr * sp * cy + sr * cp * sy;
	pOut->z = cr * cp * sy - sr * sp * cy;

	kmQuaternionNormalize(pOut, pOut);

	return pOut;
}
Ejemplo n.º 3
0
	void MotionRecognizer::normalize(vector<kmVecPair>& data, vector<kmVec3>& norm)
	{
		kmQuaternion q1, q2, q3;
		kmVec3 n;
		vector<kmVecPair>::iterator iter;
		
		for (iter = data.begin(); iter != data.end(); iter++) {
			q1.x = ((kmVecPair)*iter).accel.x;
			q1.y = ((kmVecPair)*iter).accel.y;
			q1.z = ((kmVecPair)*iter).accel.z;
			q1.w = 0;
			
			if (q1.x * q1.x + q1.y * q1.y + q1.z * q1.z < ACCELERATION_THRESHOLD)
				continue;
			
			q2.x = ((kmVecPair)*iter).rot.x;
			q2.y = ((kmVecPair)*iter).rot.y;
			q2.z = ((kmVecPair)*iter).rot.z;
			q2.w = ((kmVecPair)*iter).rot.w;

			// For every acclerations, rotate them by rotation quaternion.
			// Then, axes of acclerations are adjusted to global earth axes.
			kmQuaternionMultiply(&q3, &q2, &q1);
			kmQuaternionConjugate(&q1, &q2);
			kmQuaternionMultiply(&q2, &q3, &q1);
			
			// Normalize vector's size equal to size of codebook vector (=SQRT6)
			kmQuaternionNormalize(&q1, &q2);
			n.x = q1.x * SQRT6;
			n.y = q1.y * SQRT6;
			n.z = q1.z * SQRT6;
			
			norm.push_back(n);
		}
	}
Ejemplo n.º 4
0
uint8_t lite3d_scene_node_update(lite3d_scene_node *node)
{
    uint8_t updated = LITE3D_FALSE;
    SDL_assert(node);

    if (node->recalc)
    {
        kmMat4 transMat;
        kmMat4 scaleMat;
        kmQuaternionNormalize(&node->rotation,
            &node->rotation);

        kmMat4RotationQuaternion(&node->localView, &node->rotation);
        kmMat4Translation(&transMat,
            node->isCamera ? -node->position.x : node->position.x,
            node->isCamera ? -node->position.y : node->position.y,
            node->isCamera ? -node->position.z : node->position.z);

        if (node->scale.x != 1.0f ||
            node->scale.y != 1.0f ||
            node->scale.z != 1.0f)
        {
            kmMat4Scaling(&scaleMat, node->scale.x,
                node->scale.y,
                node->scale.z);

            kmMat4Multiply(&transMat, &transMat, &scaleMat);
        }

        if (node->rotationCentered)
            kmMat4Multiply(&node->localView,
            &transMat, &node->localView);
        else
            kmMat4Multiply(&node->localView,
            &node->localView, &transMat);

        if (node->baseNode)
        {
            kmMat4Multiply(&node->worldView,
                &node->baseNode->worldView, &node->localView);
        }
        else
        {
            node->worldView = node->localView;
        }
        
        kmMat3NormalMatrix(&node->normalModel, &node->worldView);

        node->recalc = LITE3D_FALSE;
        node->invalidated = LITE3D_TRUE;
        updated = LITE3D_TRUE;
    }

    return updated;
}
Ejemplo n.º 5
0
kmQuaternion* kmQuaternionLookRotation(kmQuaternion* pOut, const kmVec3* direction, const kmVec3* upDirection) {
    kmMat4 lookAt;
    kmMat3 rot;
    kmMat4LookAt(&lookAt, &KM_VEC3_ZERO, direction, upDirection);

    kmMat4ExtractRotationMat3(&lookAt, &rot);

    kmQuaternionRotationMatrix(pOut, &rot);
    kmQuaternionNormalize(pOut, pOut);
    return pOut;
}
Ejemplo n.º 6
0
/**< Interpolate between 2 quaternions*/
kmQuaternion* kmQuaternionSlerp(kmQuaternion* pOut,
								const kmQuaternion* q1,
								const kmQuaternion* q2,
								kmScalar t)
{

    kmQuaternion tmp;
    kmQuaternion t1, t2;
    kmScalar theta_0;
    kmScalar theta;

    kmScalar dot = kmQuaternionDot(q1, q2);
    const double DOT_THRESHOLD = 0.9995;

    if (dot > DOT_THRESHOLD) {
        kmQuaternion diff;
        kmQuaternionSubtract(&diff, q2, q1);
        kmQuaternionScale(&diff, &diff, t);

        kmQuaternionAdd(pOut, q1, &diff);
        kmQuaternionNormalize(pOut, pOut);
        return pOut;
    }

    dot = kmClamp(dot, -1, 1);

    theta_0 = acos(dot);
    theta = theta_0 * t;

    kmQuaternionScale(&tmp, q1, dot);
    kmQuaternionSubtract(&tmp, q2, &tmp);
    kmQuaternionNormalize(&tmp, &tmp);

    kmQuaternionScale(&t1, q1, cos(theta));
    kmQuaternionScale(&t2, &tmp, sin(theta));

    kmQuaternionAdd(pOut, &t1, &t2);

	return pOut;
}
Ejemplo n.º 7
0
kmScalar kmQuatToAxisAngle(struct kmVec3* pOut, const kmQuaternion* pIn)
{
	kmQuaternion q;
	kmQuaternionNormalize(&q, pIn);
	if (pOut)
	{
		pOut->x = q.x;
		pOut->y = q.y;
		pOut->z = q.z;
		kmVec3Normalize(pOut, pOut);
	}
	return 2.0f * acos(q.w);;
}
Ejemplo n.º 8
0
/**< Rotates a quaternion around an axis*/
kmQuaternion* kmQuaternionRotationAxisAngle(kmQuaternion* pOut,
									const kmVec3* pV,
									kmScalar angle)
{
    kmScalar rad = angle * 0.5f;
	kmScalar scale	= sinf(rad);

	pOut->x = pV->x * scale;
	pOut->y = pV->y * scale;
	pOut->z = pV->z * scale;
    pOut->w = cosf(rad);

	kmQuaternionNormalize(pOut, pOut);

	return pOut;
}
Ejemplo n.º 9
0
/*
 * Returns a Quaternion representing the angle between two vectors
 */
kmQuaternion* kmQuaternionBetweenVec3(kmQuaternion* pOut, const kmVec3* u, const kmVec3* v) {
    kmVec3 w;
    kmScalar len;
    kmQuaternion q;

    if(kmVec3AreEqual(u, v)) {
        kmQuaternionIdentity(pOut);
        return pOut;
    }

    len = sqrtf(kmVec3LengthSq(u) * kmVec3LengthSq(v));
    kmVec3Cross(&w, u, v);

    kmQuaternionFill(&q, w.x, w.y, w.z, kmVec3Dot(u, v) + len);
    return kmQuaternionNormalize(pOut, &q);
}
Ejemplo n.º 10
0
kmQuaternion* kmQuaternionExtractRotationAroundAxis(const kmQuaternion* pIn, const kmVec3* axis, kmQuaternion* pOut) {
    /**
        Given a quaternion, and an axis. This extracts the rotation around the axis into pOut as another quaternion.
        Uses the swing-twist decomposition.

        http://stackoverflow.com/questions/3684269/component-of-a-quaternion-rotation-around-an-axis/22401169?noredirect=1#comment34098058_22401169
    */

    kmVec3 ra;
    kmScalar d;

    kmVec3Fill(&ra, pIn->x, pIn->y, pIn->z);

    d = kmVec3Dot(&ra, axis);

    kmQuaternionFill(pOut, axis->x * d, axis->y * d, axis->z * d, pIn->w);
    kmQuaternionNormalize(pOut, pOut);
    return pOut;
}
Ejemplo n.º 11
0
kmQuaternion* kmQuaternionLookRotation(kmQuaternion* pOut, const kmVec3* direction, const kmVec3* up) {
    kmMat3 tmp;
    kmMat3LookAt(&tmp, &KM_VEC3_ZERO, direction, up);
    return kmQuaternionNormalize(pOut, kmQuaternionRotationMatrix(pOut, &tmp));
/*
    if(!direction->x && !direction->y && !direction->z) {
        return kmQuaternionIdentity(pOut);
    }

    kmVec3 right;
    kmVec3Cross(&right, up, direction);

    pOut->w = sqrtf(1.0f + right.x + up->y + direction->z) * 0.5f;

    float w4_recip = 1.0f / (4.0f * pOut->w);

    pOut->x = (up->z - direction->y) * w4_recip;
    pOut->y = (direction->x - right.z) * w4_recip;
    pOut->z = (right.y - up->x) * w4_recip;

    return kmQuaternionNormalize(pOut, pOut);*/
}
Ejemplo n.º 12
0
kmQuaternion* kmQuaternionRotationBetweenVec3(kmQuaternion* pOut, const kmVec3* vec1, const kmVec3* vec2, const kmVec3* fallback) {

	kmVec3 v1, v2;
    kmScalar a;

	kmVec3Assign(&v1, vec1);
	kmVec3Assign(&v2, vec2);

	kmVec3Normalize(&v1, &v1);
	kmVec3Normalize(&v2, &v2);

	a = kmVec3Dot(&v1, &v2);

	if (a >= 1.0) {
		kmQuaternionIdentity(pOut);
		return pOut;
	}

	if (a < (1e-6f - 1.0f))	{
		if (fabs(kmVec3LengthSq(fallback)) < kmEpsilon) {
            kmQuaternionRotationAxisAngle(pOut, fallback, kmPI);
		} else {
			kmVec3 axis;
			kmVec3 X;
			X.x = 1.0;
			X.y = 0.0;
			X.z = 0.0;


			kmVec3Cross(&axis, &X, vec1);

			/*If axis is zero*/
			if (fabs(kmVec3LengthSq(&axis)) < kmEpsilon) {
				kmVec3 Y;
				Y.x = 0.0;
				Y.y = 1.0;
				Y.z = 0.0;

				kmVec3Cross(&axis, &Y, vec1);
			}

			kmVec3Normalize(&axis, &axis);

            kmQuaternionRotationAxisAngle(pOut, &axis, kmPI);
		}
	} else {
		kmScalar s = sqrtf((1+a) * 2);
		kmScalar invs = 1 / s;

		kmVec3 c;
		kmVec3Cross(&c, &v1, &v2);

		pOut->x = c.x * invs;
		pOut->y = c.y * invs;
        pOut->z = c.z * invs;
        pOut->w = s * 0.5f;

		kmQuaternionNormalize(pOut, pOut);
	}

	return pOut;

}
Ejemplo n.º 13
0
///< Creates a quaternion from a rotation matrix
kmQuaternion* kmQuaternionRotationMatrix(kmQuaternion* pOut,
										const kmMat3* pIn)
{
/*
Note: The OpenGL matrices are transposed from the description below
taken from the Matrix and Quaternion FAQ

    if ( mat[0] > mat[5] && mat[0] > mat[10] )  {	// Column 0:
        S  = sqrt( 1.0 + mat[0] - mat[5] - mat[10] ) * 2;
        X = 0.25 * S;
        Y = (mat[4] + mat[1] ) / S;
        Z = (mat[2] + mat[8] ) / S;
        W = (mat[9] - mat[6] ) / S;
    } else if ( mat[5] > mat[10] ) {			// Column 1:
        S  = sqrt( 1.0 + mat[5] - mat[0] - mat[10] ) * 2;
        X = (mat[4] + mat[1] ) / S;
        Y = 0.25 * S;
        Z = (mat[9] + mat[6] ) / S;
        W = (mat[2] - mat[8] ) / S;
    } else {						// Column 2:
        S  = sqrt( 1.0 + mat[10] - mat[0] - mat[5] ) * 2;
        X = (mat[2] + mat[8] ) / S;
        Y = (mat[9] + mat[6] ) / S;
        Z = 0.25 * S;
        W = (mat[4] - mat[1] ) / S;
    }
*/

	float x, y, z, w;
	float *pMatrix = NULL;
	float m4x4[16] = {0};
	float scale = 0.0f;
	float diagonal = 0.0f;

	if(!pIn) {
		return NULL;
	}

/*	0 3 6
	1 4 7
	2 5 8

	0 1 2 3
	4 5 6 7
	8 9 10 11
	12 13 14 15*/

	m4x4[0]  = pIn->mat[0];
	m4x4[1]  = pIn->mat[3];
	m4x4[2]  = pIn->mat[6];
	m4x4[4]  = pIn->mat[1];
	m4x4[5]  = pIn->mat[4];
	m4x4[6]  = pIn->mat[7];
	m4x4[8]  = pIn->mat[2];
	m4x4[9]  = pIn->mat[5];
	m4x4[10] = pIn->mat[8];
	m4x4[15] = 1;
	pMatrix = &m4x4[0];

	diagonal = pMatrix[0] + pMatrix[5] + pMatrix[10] + 1;

	if(diagonal > kmEpsilon) {
		// Calculate the scale of the diagonal
		scale = (float)sqrt(diagonal ) * 2;

		// Calculate the x, y, x and w of the quaternion through the respective equation
		x = ( pMatrix[9] - pMatrix[6] ) / scale;
		y = ( pMatrix[2] - pMatrix[8] ) / scale;
		z = ( pMatrix[4] - pMatrix[1] ) / scale;
		w = 0.25f * scale;
	}
	else
	{
		// If the first element of the diagonal is the greatest value
		if ( pMatrix[0] > pMatrix[5] && pMatrix[0] > pMatrix[10] )
		{
			// Find the scale according to the first element, and double that value
			scale = (float)sqrt( 1.0f + pMatrix[0] - pMatrix[5] - pMatrix[10] ) * 2.0f;

			// Calculate the x, y, x and w of the quaternion through the respective equation
			x = 0.25f * scale;
			y = (pMatrix[4] + pMatrix[1] ) / scale;
			z = (pMatrix[2] + pMatrix[8] ) / scale;
			w = (pMatrix[9] - pMatrix[6] ) / scale;
		}
		// Else if the second element of the diagonal is the greatest value
		else if (pMatrix[5] > pMatrix[10])
		{
			// Find the scale according to the second element, and double that value
			scale = (float)sqrt( 1.0f + pMatrix[5] - pMatrix[0] - pMatrix[10] ) * 2.0f;

			// Calculate the x, y, x and w of the quaternion through the respective equation
			x = (pMatrix[4] + pMatrix[1] ) / scale;
			y = 0.25f * scale;
			z = (pMatrix[9] + pMatrix[6] ) / scale;
			w = (pMatrix[2] - pMatrix[8] ) / scale;
		}
		// Else the third element of the diagonal is the greatest value
		else
		{
			// Find the scale according to the third element, and double that value
			scale  = (float)sqrt( 1.0f + pMatrix[10] - pMatrix[0] - pMatrix[5] ) * 2.0f;

			// Calculate the x, y, x and w of the quaternion through the respective equation
			x = (pMatrix[2] + pMatrix[8] ) / scale;
			y = (pMatrix[9] + pMatrix[6] ) / scale;
			z = 0.25f * scale;
			w = (pMatrix[4] - pMatrix[1] ) / scale;
		}
	}

	pOut->x = x;
	pOut->y = y;
	pOut->z = z;
	pOut->w = w;

	return pOut;

#if 0
	kmScalar T = pIn->mat[0] + pIn->mat[5] + pIn->mat[10];

	if (T > kmEpsilon) {
		//If the trace is greater than zero we always use this calculation:
		/*  S = sqrt(T) * 2;
		  X = ( mat[9] - mat[6] ) / S;
		  Y = ( mat[2] - mat[8] ) / S;
		  Z = ( mat[4] - mat[1] ) / S;
		  W = 0.25 * S;*/

/*		kmScalar s = sqrtf(T) * 2;
		pOut->x = (pIn->mat[9] - pIn->mat[6]) / s;
		pOut->y = (pIn->mat[8] - pIn->mat[2]) / s;
		pOut->z = (pIn->mat[1] - pIn->mat[4]) / s;
		pOut->w = 0.25f * s;

		kmQuaternionNormalize(pOut, pOut);
		return pOut;
	}

	//Otherwise the calculation depends on which major diagonal element has the greatest value.

	if (pIn->mat[0] > pIn->mat[5] && pIn->mat[0] > pIn->mat[10]) {
		kmScalar s = sqrtf(1 + pIn->mat[0] - pIn->mat[5] - pIn->mat[10]) * 2;
		pOut->x = 0.25f * s;
		pOut->y = (pIn->mat[1] + pIn->mat[4]) / s;
		pOut->z = (pIn->mat[8] + pIn->mat[2]) / s;
		pOut->w = (pIn->mat[9] - pIn->mat[6]) / s;
	}
	else if (pIn->mat[5] > pIn->mat[10]) {
		kmScalar s = sqrtf(1 + pIn->mat[5] - pIn->mat[0] - pIn->mat[10]) * 2;
		pOut->x = (pIn->mat[1] + pIn->mat[4]) / s;
		pOut->y = 0.25f * s;
		pOut->z = (pIn->mat[9] + pIn->mat[6]) / s;
		pOut->w = (pIn->mat[8] - pIn->mat[2]) / s;
	}
	else {
		kmScalar s = sqrt(1.0f + pIn->mat[10] - pIn->mat[0] - pIn->mat[5]) * 2.0f;
        pOut->x = (pIn->mat[8] + pIn->mat[2] ) / s;
        pOut->y = (pIn->mat[6] + pIn->mat[9] ) / s;
        pOut->z = 0.25f * s;
        pOut->w = (pIn->mat[1] - pIn->mat[4] ) / s;
	}

	kmQuaternionNormalize(pOut, pOut);
	return pOut;*/
#endif // 0
}

///< Create a quaternion from yaw, pitch and roll
kmQuaternion* kmQuaternionRotationYawPitchRoll(kmQuaternion* pOut,
												kmScalar yaw,
												kmScalar pitch,
												kmScalar roll)
{
	kmScalar	ex, ey, ez;		// temp half euler angles
	kmScalar	cr, cp, cy, sr, sp, sy, cpcy, spsy;		// temp vars in roll,pitch yaw

	ex = kmDegreesToRadians(pitch) / 2.0f;	// convert to rads and half them
	ey = kmDegreesToRadians(yaw) / 2.0f;
	ez = kmDegreesToRadians(roll) / 2.0f;

	cr = cosf(ex);
	cp = cosf(ey);
	cy = cosf(ez);

	sr = sinf(ex);
	sp = sinf(ey);
	sy = sinf(ez);

	cpcy = cp * cy;
	spsy = sp * sy;

	pOut->w = cr * cpcy + sr * spsy;

	pOut->x = sr * cpcy - cr * spsy;
	pOut->y = cr * sp * cy + sr * cp * sy;
	pOut->z = cr * cp * sy - sr * sp * cy;

	kmQuaternionNormalize(pOut, pOut);

	return pOut;
}