00001 #include "Euler3.hpp" 00002 #include "Quat4.hpp" 00003 #include "util/error/Log.hpp" 00004 #include <cmath> 00005 #include <cstdio> 00006 00007 namespace se_core { 00008 00009 const char* Euler3 00010 ::toLog() const { 00011 sprintf(log_msg(), "(%d, %d, %d)", yaw_, pitch_, roll_); 00012 return log_msg(); 00013 } 00014 00015 00016 char* Euler3 00017 ::toString(char* buffer) const { 00018 sprintf(buffer, "(%d, %d, %d)", yaw_, pitch_, roll_); 00019 return buffer; 00020 } 00021 00022 se_err::Log& operator<< (se_err::Log& log, const Euler3& t) { 00023 t.toString(log.tmp()); 00024 log.copy(log.tmp()); 00025 return log; 00026 } 00027 00028 bool Euler3 00029 ::epsilonEquals(const Euler3& a1, bray_t epsilon) const { 00030 bool t = BrayT::abs(BrayT::sub(yaw_, a1.yaw_)) <= epsilon; 00031 t = t && (BrayT::abs(BrayT::sub(pitch_, a1.pitch_)) <= epsilon); 00032 t = t && (BrayT::abs(BrayT::sub(roll_, a1.roll_)) <= epsilon); 00033 00034 return t; 00035 } 00036 00037 00038 void Euler3 00039 ::set(const Quat4& q1) { 00040 float test = q1.x_*q1.y_ + q1.z_*q1.w_; 00041 if (test > 0.4999999) { // singularity at north pole 00042 yaw_ = BrayT::fromRad(2 * ::atan2(q1.x_, q1.w_)); 00043 roll_ = BrayT::DEG90; 00044 pitch_ = 0; 00045 return; 00046 } 00047 00048 if (test < -0.4999999) { // singularity at south pole 00049 yaw_ = BrayT::fromRad(-2 * atan2(q1.x_, q1.w_)); 00050 roll_ = BrayT::DEG270; 00051 pitch_ = 0; 00052 return; 00053 } 00054 00055 float sqx = q1.x_*q1.x_; 00056 float sqy = q1.y_*q1.y_; 00057 float sqz = q1.z_*q1.z_; 00058 00059 yaw_ = BrayT::fromRad(-::atan2(2*q1.y_*q1.w_-2*q1.x_*q1.z_ , 1 - 2*sqy - 2*sqz)); 00060 roll_ = BrayT::fromRad(::asin(2*test)); 00061 pitch_ = BrayT::fromRad(::atan2(2*q1.x_*q1.w_-2*q1.y_*q1.z_ , 1 - 2*sqx - 2*sqz)); 00062 00063 normalize(); 00064 } 00065 00066 00067 00068 void Euler3 00069 ::interpolate(const Euler3& a1, scale_t alpha) { 00070 // Scale distance from 0 braybrookians, rather than the number itself 00071 yaw_ += BrayT::scale(alpha, BrayT::mask(a1.yaw_ - yaw_)); 00072 pitch_ += BrayT::scale(alpha, BrayT::mask(a1.pitch_ - pitch_)); 00073 roll_ += BrayT::scale(alpha, BrayT::mask(a1.roll_ - roll_)); 00074 00075 normalize(); 00076 } 00077 00078 }
Home Page | SagaEngine trunk (updated nightly) reference generated Sun Dec 2 20:06:13 2007 by Doxygen version 1.3.9.1.