kmQuaternion *sls_trackball_calc_quat(kmQuaternion *out, float trackball_radius,
float trackball_speed, kmVec2 const *p1,
kmVec2 const *p2) {
//sls_log_info("p1 %f %f, p2 %f %f", p1->x, p1->y, p2->x, p2->y);
kmVec3 axis;
kmVec3 _p1, _p2, dir;
float phi;
float t;
float epsilon = 0.001;
if (sls_vec2_near(p1, p2, epsilon)) {
// zero rotation
kmQuaternionIdentity(out);
return out;
}
_p1 = (kmVec3){p1->x, p1->y, sls_tb_project_to_sphere(trackball_radius, p1)};
_p2 = (kmVec3){p2->x, p2->y, sls_tb_project_to_sphere(trackball_radius, p2)};
kmVec3Subtract(&dir, &_p1, &_p2);
kmVec3Cross(&axis, &_p2, &_p1);
t = kmVec3Length(&dir) / (2.0f * trackball_radius);
t = fminf(fmaxf(-1.f, t), 1.f);
phi = 2.0f * asinf(t) * trackball_speed;
kmQuaternion tmp_a, tmp_b;
kmQuaternionRotationAxisAngle(&tmp_a, &axis, phi);
tmp_b = *out;
return kmQuaternionMultiply(out, &tmp_a, &tmp_b);
;
}
slsTrackball *sls_trackball_init(slsTrackball *self, float radius,
float rotation_speed) {
self->radius = radius;
self->rotation_speed = rotation_speed;
kmQuaternionIdentity(&self->rotation);
kmMat4Identity(&self->rotation_mat);
return self;
}
/*
* 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);
}
lite3d_scene_node *lite3d_scene_node_init(lite3d_scene_node *node)
{
SDL_assert(node);
memset(node, 0, sizeof (lite3d_scene_node));
lite3d_list_link_init(&node->nodeLink);
kmMat4Identity(&node->localView);
kmMat4Identity(&node->worldView);
kmMat3Identity(&node->normalModel);
kmQuaternionIdentity(&node->rotation);
kmVec3Fill(&node->position, 0, 0, 0);
kmVec3Fill(&node->scale, 1.0f, 1.0f, 1.0f);
node->recalc = LITE3D_TRUE;
node->invalidated = LITE3D_TRUE;
node->rotationCentered = LITE3D_TRUE;
node->isCamera = LITE3D_FALSE;
node->renderable = LITE3D_TRUE;
node->enabled = LITE3D_TRUE;
node->visible = LITE3D_TRUE;
node->frustumTest = LITE3D_TRUE;
lite3d_list_init(&node->childNodes);
return node;
}
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;
}
