From 3f63ae0e2e6597655ddc7a4119622dd55b4d40c2 Mon Sep 17 00:00:00 2001 From: Stephen Waits Date: Mon, 24 Jan 2011 14:18:30 -0800 Subject: [PATCH] add quaternion search example --- examples/quatsearch.cpp | 513 ++++++++++++++++++++++++++++++++++++++++ 1 file changed, 513 insertions(+) create mode 100644 examples/quatsearch.cpp diff --git a/examples/quatsearch.cpp b/examples/quatsearch.cpp new file mode 100644 index 0000000..cc728ea --- /dev/null +++ b/examples/quatsearch.cpp @@ -0,0 +1,513 @@ +#include +#include +#include +#include +//#include +//#include + + +const double PI = 3.141592653589793238462643383279502884197169399375105820974944592307816406286208998628034825342117; + + +#include + +#include "de.h" + + +class PolyDEother: public DE::Engine<5> +{ +public: + + PolyDEother() + { + SetRange(-20000000.0,200000000.0); + } + + virtual double CalculateError(const double testcase[5], bool &stop) + { + double err = 0.0; + int n = 0; + + for (double x=-10.0;x<=10.0;x+=0.025) + { + double diff = actual(x) - test(x,testcase); + err += (diff * diff); + n++; + } + + err /= (double)n; + + if ( err < 0.00000000000000000001 ) + { + stop = true; + } + + return err; + } + + static inline double actual(double x) + { + const double coeffs[] = { 1.0, 874.0, -451.0, 0.00000000001, -20000000000.0 }; + + return test(x,coeffs); + } + + static inline double test(double x, const double coeffs[5]) + { + return coeffs[0] + coeffs[1]*x + coeffs[2]*x*x + coeffs[3]*x*x*x + coeffs[4]*x*x*x*x; + } + + + +}; + + +#define DIM (10) + +class PolyDE: public DE::Engine +{ +public: + + double actual_scale0[401*401]; + double actual_scale1[401*401]; + + PolyDE() + { + SetRange(-0.2,0.2); + + int i=0; + for (double t=0.0; t<=1.0; t+=0.0025) + { + for (double c=0.0; c<=1.0; c+=0.0025) + { + slerp_actual_t(t,c,actual_scale0[i],actual_scale1[i]); + i++; + assert(i<(402*402)); + } + } + + } + + virtual double CalculateError(const double testcase[DIM], bool &stop) + { + double sumerr = 0.0; + unsigned int n = 0; + double err; + + int i = 0; + for (double t=0.0; t<=1.0; t+=0.0025) + { + for (double c=0.0; c<=1.0; c+=0.0025) + { + double test0,test1; + slerp_poly_t(t,c,testcase,test0,test1); + + err = actual_scale0[i] - test0; + sumerr += err * err; + err = actual_scale1[i] - test1; + sumerr += err * err; + + i++; + n++; + } + } + + sumerr /= (double)n; + + if ( sumerr < 0.0000000000000000000001 || _kbhit() ) + { + stop = true; + } + + return sumerr; + } + + + + static inline void slerp_actual_t(const double& t, const double& costheta, double& scale0, double& scale1) + { + // calculate coefficients + if ( costheta < 1.00000 ) + { + double theta = acos(costheta); + double sintheta = sin(theta); + scale0 = sin((1.0 - t) * theta) / sintheta; + scale1 = sin(t * theta) / sintheta; + } + else + { + scale0 = 1.0 - t; + scale1 = t; + } + } + + static inline void slerp_poly_t(const double& t, const double& c, const double coeffs[DIM], double& scale0, double& scale1) + { + scale0 = sum_poly2(t,c,&coeffs[0]); + scale1 = sum_poly2(t,c,&coeffs[DIM>>1]); + } + + + + static inline double sum_poly2(const double& x, const double& y, const double coeffs[5]) + { + const double x2 = x*x; + const double y2 = y*y; + + return coeffs[0] + coeffs[1]*x + coeffs[2]*x2 + coeffs[3]*y + coeffs[4]*y2; + } + + static inline double sum_poly3(const double& x, const double& y, const double coeffs[7]) + { + const double x2 = x*x; + const double y2 = y*y; + const double x3 = x2*x; + const double y3 = y2*y; + + return coeffs[0] + coeffs[1]*x + coeffs[2]*x2 + coeffs[3]*x3 + coeffs[4]*y + coeffs[5]*y2 + coeffs[6]*y3; + } + + static inline double sum_poly4(const double& x, const double& y, const double coeffs[9]) + { + const double x2 = x*x; + const double y2 = y*y; + const double x3 = x2*x; + const double y3 = y2*y; + const double x4 = x3*x; + const double y4 = y3*y; + + return coeffs[0] + coeffs[1]*x + coeffs[2]*x2 + coeffs[3]*x3 + coeffs[4]*x4 + coeffs[5]*y + coeffs[6]*y2 + coeffs[7]*y3 + coeffs[8]*y4; + } + + static inline double bivariate_poly3(const double& x, const double& y, const double coeffs[10]) + { + const double x2 = x*x; + const double y2 = y*y; + const double x3 = x2*x; + const double y3 = y2*y; + + return x3*(coeffs[9]*y3+coeffs[7]*y2)+x2*(coeffs[8]*y3+coeffs[6]*y2+coeffs[4]*y)+x*(coeffs[5]*y2+coeffs[3]*y+coeffs[1])+coeffs[2]*y+coeffs[0]; + } + + static inline double bivariate_poly2(const double& x, const double& y, const double coeffs[7]) + { + const double x2 = x*x; + const double y2 = y*y; + return x2*(coeffs[6]*y2 + coeffs[4]*y) + x*(coeffs[5]*y2 + coeffs[3]*y + coeffs[1]) + coeffs[2]*y + coeffs[0]; + } + + static inline double bivariate_poly1(const double& x, const double& y, const double coeffs[4]) + { + return x*(coeffs[3]*y + coeffs[1]) + coeffs[2]*y + coeffs[0]; + } + +}; + +int main() +{ + PolyDE* de = new PolyDE; + + de->Solve(); + + delete de; + + return 0; +} + + + + + + + + + + +#if 0 + +class quat +{ +public: + + double tuple[4]; + + static const quat ZERO; + static const quat IDENTITY; + + quat() + { + *this = quat::IDENTITY; + } + + ~quat() + { + } + + quat(const quat& other) + { + *this = other; + } + + quat& operator=(const quat& other) + { + tuple[0] = other.tuple[0]; + tuple[1] = other.tuple[1]; + tuple[2] = other.tuple[2]; + tuple[3] = other.tuple[3]; + return *this; + } + + quat(const double& x, const double& y, const double& z, const double& w) + { + tuple[0] = x; + tuple[1] = y; + tuple[2] = z; + tuple[3] = w; + } + + quat(const double axis[3], const double& angle) + { + double halfangle = angle / 2.0; + double sinangle = sin(halfangle); + + tuple[0] = axis[0] * sinangle; + tuple[1] = axis[1] * sinangle; + tuple[2] = axis[2] * sinangle; + tuple[3] = cos(halfangle); + } + + double dot(const quat& other) const + { + return (tuple[0]*other.tuple[0] + tuple[1]*other.tuple[1] + tuple[2]*other.tuple[2] + tuple[3]*other.tuple[3]); + } + + quat& operator*= (double scalar) + { + tuple[0] *= scalar; + tuple[1] *= scalar; + tuple[2] *= scalar; + tuple[3] *= scalar; + return *this; + } + + quat operator* (double scalar) const + { + return quat + ( + tuple[0] * scalar, + tuple[1] * scalar, + tuple[2] * scalar, + tuple[3] * scalar + ); + return *this; + } + + quat& operator/= (double scalar) + { + return (*this *= 1.0/scalar); + } + + quat operator/ (double scalar) const + { + return (*this * 1.0/scalar); + } + + double length_squared() const + { + return dot(*this); + } + + double length() const + { + return sqrt(length_squared()); + } + + quat& normalize() + { + return (*this /= length()); + } + + quat& operator-() + { + tuple[0] = -tuple[0]; + tuple[1] = -tuple[1]; + tuple[2] = -tuple[2]; + tuple[3] = -tuple[3]; + return *this; + } + + double distance(const quat& other) + { + quat tmp(tuple[0]-other.tuple[0],tuple[1]-other.tuple[1],tuple[2]-other.tuple[2],tuple[3]-other.tuple[3]); + return tmp.length(); + } +}; + +void slerp_coeffs(double t, const quat& q0, const quat& q1, double& scale0, double& scale1) +{ + quat from(q0), to(q1); + double costheta,sintheta,theta; + + // calculate cosine theta + costheta = from.dot(to); + + // adjust signs if necessary + if ( costheta < 0.0 ) + { + costheta = -costheta; + to = -to; + } + + // calculate coefficients + if ( costheta < 1.00000 ) + { + theta = acos(costheta); + sintheta = sin(theta); + scale0 = sin((1.0 - t) * theta) / sintheta; + scale1 = sin(t * theta) / sintheta; + } + else + { + scale0 = 1.0 - t; + scale1 = t; + } + + //printf("t=%f s0=%0.10f s1=%0.10f\n",t,scale0,scale1); +} + +quat slerp(double t, const quat& q0, const quat& q1) +{ + double scale0,scale1; + slerp_coeffs(t,q0,q1,scale0,scale1); + + // return result + quat result + ( + scale0 * q0.tuple[0] + scale1 * q1.tuple[0], + scale0 * q0.tuple[1] + scale1 * q1.tuple[1], + scale0 * q0.tuple[2] + scale1 * q1.tuple[2], + scale0 * q0.tuple[3] + scale1 * q1.tuple[3] + ); + + result.normalize(); + + return result; +} + +quat lerp(double t, const quat& q0, const quat& q1) +{ + double scale0 = 1.0 - t; + double scale1 = t; + + return quat + ( + scale0 * q0.tuple[0] + scale1 * q1.tuple[0], + scale0 * q0.tuple[1] + scale1 * q1.tuple[1], + scale0 * q0.tuple[2] + scale1 * q1.tuple[2], + scale0 * q0.tuple[3] + scale1 * q1.tuple[3] + ); +} + +quat slerp_via_lerp_and_normalize(double t, const quat& q0, const quat& q1) +{ + double scale0,scale1; + slerp_coeffs(t,q0,q1,scale0,scale1); + + double new_t = scale1 / (scale0+scale1); + + quat result = lerp(new_t,q0,q1); + + result.normalize(); + + return result; +} + +const quat quat::ZERO = quat(0.0,0.0,0.0,0.0); +const quat quat::IDENTITY = quat(0.0,0.0,0.0,1.0); + +double vec_length(double vec[3]) +{ + return sqrt(vec[0]*vec[0] + vec[1]*vec[1] + vec[2]*vec[2]); +} + +double random_double(double low, double high) +{ + double prn = (double)rand() / (double)RAND_MAX; + + return (prn * (high - low)) + low; +} + +quat random_quat() +{ + double axis[3]; + double angle; + + // random vector elements + axis[0] = random_double(-1.0,1.0); + axis[1] = random_double(-1.0,1.0); + axis[2] = random_double(-1.0,1.0); + + // normalize vector + double veclen = vec_length(axis); + axis[0] /= veclen; + axis[1] /= veclen; + axis[2] /= veclen; + + // random angle + angle = random_double(-PI*2.0,PI*2.0); + + // construct a quat + return quat(axis,angle); +} + + + +bool test() +{ + const double MAX_ERROR = 0.0000000000001; + + quat a = random_quat(); + quat b = random_quat(); + + for (double t=0.0; t < 1.01; t+= 0.01) + { + quat c = slerp(t,a,b); + quat d = slerp_via_lerp_and_normalize(t,a,b); + + double error = c.distance(d); + if ( error > MAX_ERROR ) + { + printf("actual error = %0.34f, max error = %0.34f\n",error,MAX_ERROR); + return false; + } + } + + return true; +} + +/* +quat slerp_approx(double t, const quat& q0, const quat& q1) +{ + quat from(q0),to(q1); + + // calculate cosine theta + double costheta = from.dot(to); + + // adjust signs if necessary + if ( costheta < 0.0 ) + { + costheta = -costheta; + to = -to; + } + + double new_t = poly[0] + poly[1]*t + poly[2]*t*t + poly[3] + poly[4]*costheta + poly[5]*costheta*costheta; + + quat result = lerp(new_t,q0,q1); + result.normalize(); + + return result; +} +*/ + + +#endif