//############################################################################
inline LaserBoy_3D_double rotate_vertex(LaserBoy_3D_double p, LaserBoy_3D_double a)
{
LaserBoy_3D_double sin_a(sin(a.x), sin(a.y), sin(a.z)),
cos_a(cos(a.x), cos(a.y), cos(a.z)),
rotated;
//------------------------------------------------------------------------
rotated.y = p.y * cos_a.x - p.z * sin_a.x;
rotated.z = p.z * cos_a.x + p.y * sin_a.x; // rotate around x
rotated.z = rotated.z * cos_a.y - p.x * sin_a.y;
rotated.x = p.x * cos_a.y + (p.z * cos_a.x + p.y * sin_a.x) * sin_a.y; // rotate around y
rotated.x = rotated.x * cos_a.z - (p.y * cos_a.x - p.z * sin_a.x) * sin_a.z;
rotated.y = rotated.y * cos_a.z + (p.x * cos_a.y + (p.z * cos_a.x + p.y * sin_a.x) * sin_a.y) * sin_a.z; // rotate around z
return rotated;
}
void toast::qarray::from_axisangle ( size_t n, double const * axis, double const * angle, double * q_out ) {
if ( n == 1 ) {
double sin_a;
for ( size_t i = 0; i < n; ++i ) {
sin_a = ::sin ( 0.5 * angle[i] );
for ( size_t j = 0; j < 3; ++j ) {
q_out[4*i + j] = axis[3*i + j] * sin_a;
}
q_out[4*i + 3] = ::cos ( 0.5 * angle[i] );
}
} else {
toast::mem::simd_array<double> a(n);
for ( size_t i = 0; i < n; ++i ) {
a[i] = 0.5 * angle[i];
}
toast::mem::simd_array<double> sin_a(n);
toast::mem::simd_array<double> cos_a(n);
toast::sf::sincos ( n, a, sin_a, cos_a );
for ( size_t i = 0; i < n; ++i ) {
for ( size_t j = 0; j < 3; ++j ) {
q_out[4*i + j] = axis[3*i + j] * sin_a[i];
}
q_out[4*i + 3] = cos_a[i];
}
}
return;
}
