Vec3 Vec3::random_deviant(const Degrees& angle, const Vec3 up) const {
//Lovingly adapted from ogre
Vec3 new_up = (up == Vec3()) ? perpendicular() : up;
Quaternion q;
kmQuaternionRotationAxisAngle(&q, this, random_gen::random_float(0, 1) * (PI * 2.0));
kmQuaternionMultiplyVec3(&new_up, &q, &new_up);
kmQuaternionRotationAxisAngle(&q, &new_up, Radians(angle).value_);
Vec3 ret;
kmQuaternionMultiplyVec3(&ret, &q, this);
return ret;
}
/**
* Build a rotation matrix from an axis and an angle. Result is stored in pOut.
* pOut is returned.
*/
kmMat4* kmMat4RotationAxisAngle(kmMat4* pOut, const kmVec3* axis, kmScalar radians)
{
kmQuaternion quat;
kmQuaternionRotationAxisAngle(&quat, axis, radians);
kmMat4RotationQuaternion(pOut, &quat);
return pOut;
}
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);
;
}
lite3d_scene_node *lite3d_scene_node_rotate_angle(lite3d_scene_node *node, const kmVec3 *axis, float angle)
{
kmQuaternion tmpQuat;
SDL_assert(node && axis);
kmQuaternionRotationAxisAngle(&tmpQuat, axis, angle);
lite3d_scene_node_rotate(node, &tmpQuat);
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;
}
