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