Lines Matching refs:costheta
321 T costheta = std::min(std::max((R(0, 0) + R(1, 1) + R(2, 2) - kOne) / kTwo,
333 const T theta = atan2(sintheta, costheta);
355 if (costheta > 0.0) {
371 const T inv_one_minus_costheta = kOne / (kOne - costheta);
379 angle_axis[i] = theta * sqrt((R(i, i) - costheta) * inv_one_minus_costheta);
407 const T costheta = cos(theta);
410 R(0, 0) = costheta + wx*wx*(kOne - costheta);
411 R(1, 0) = wz*sintheta + wx*wy*(kOne - costheta);
412 R(2, 0) = -wy*sintheta + wx*wz*(kOne - costheta);
413 R(0, 1) = wx*wy*(kOne - costheta) - wz*sintheta;
414 R(1, 1) = costheta + wy*wy*(kOne - costheta);
415 R(2, 1) = wx*sintheta + wy*wz*(kOne - costheta);
416 R(0, 2) = wy*sintheta + wx*wz*(kOne - costheta);
417 R(1, 2) = -wx*sintheta + wy*wz*(kOne - costheta);
418 R(2, 2) = costheta + wz*wz*(kOne - costheta);
587 // result = pt costheta +
589 // w (w . pt) (1 - costheta)
596 const T costheta = cos(theta);
610 (w[0] * pt[0] + w[1] * pt[1] + w[2] * pt[2]) * (T(1.0) - costheta);
612 result[0] = pt[0] * costheta + w_cross_pt[0] * sintheta + w[0] * tmp;
613 result[1] = pt[1] * costheta + w_cross_pt[1] * sintheta + w[1] * tmp;
614 result[2] = pt[2] * costheta + w_cross_pt[2] * sintheta + w[2] * tmp;
