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; }