TEST( Rot2, rotate) { Matrix H1, H2; Point2 actual = R.rotate(P, H1, H2); CHECK(assert_equal(actual,R*P)); Matrix numerical1 = numericalDerivative21(rotate_, R, P); CHECK(assert_equal(numerical1,H1)); Matrix numerical2 = numericalDerivative22(rotate_, R, P); CHECK(assert_equal(numerical2,H2)); }
// rotate and derivatives inline Point2 rotate_(const Rot2 & R, const Point2& p) {return R.rotate(p);}
void go() { typedef Vec4<T> Vec; typedef Vec2<T> Vec2D; std::cout << std::endl; std::cout << sizeof(Vec) << std::endl; std::vector<Vec> vec1; vec1.reserve(50); std::vector<T> vect(23); std::vector<Vec> vec2(53); std::vector<Vec> vec3; vec3.reserve(50234); Vec x(2.0,4.0,5.0); Vec y(-3.0,2.0,-5.0); std::cout << x << std::endl; std::cout << Vec4<float>(x) << std::endl; std::cout << Vec4<double>(x) << std::endl; std::cout << -x << std::endl; std::cout << x.template get1<2>() << std::endl; std::cout << y << std::endl; std::cout << T(3.)*x << std::endl; std::cout << y*T(0.1) << std::endl; std::cout << (Vec(1) - y*T(0.1)) << std::endl; std::cout << mathSSE::sqrt(x) << std::endl; std::cout << dot(x,y) << std::endl; std::cout << dotSimple(x,y) << std::endl; std::cout << "equal" << (x==x ? " " : " not ") << "ok" << std::endl; std::cout << "not equal" << (x==y ? " not " : " ") << "ok" << std::endl; Vec z = cross(x,y); std::cout << z << std::endl; std::cout << "rotations" << std::endl; T a = 0.01; T ca = std::cos(a); T sa = std::sin(a); Rot3<T> r1( ca, sa, 0, -sa, ca, 0, 0, 0, 1); Rot2<T> r21( ca, sa, -sa, ca); Rot3<T> r2(Vec( 0, 1 ,0), Vec( 0, 0, 1), Vec( 1, 0, 0)); Rot2<T> r22(Vec2D( 0, 1), Vec2D( 1, 0)); { std::cout << "\n3D rot" << std::endl; Vec xr = r1.rotate(x); std::cout << x << std::endl; std::cout << xr << std::endl; std::cout << r1.rotateBack(xr) << std::endl; Rot3<T> rt = r1.transpose(); Vec xt = rt.rotate(xr); std::cout << x << std::endl; std::cout << xt << std::endl; std::cout << rt.rotateBack(xt) << std::endl; std::cout << r1 << std::endl; std::cout << rt << std::endl; std::cout << r1*rt << std::endl; std::cout << r2 << std::endl; std::cout << r1*r2 << std::endl; std::cout << r2*r1 << std::endl; std::cout << r1*r2.transpose() << std::endl; std::cout << r1.transpose()*r2 << std::endl; } { std::cout << "\n2D rot" << std::endl; Vec2D xr = r21.rotate(x.xy()); std::cout << x.xy() << std::endl; std::cout << xr << std::endl; std::cout << r21.rotateBack(xr) << std::endl; Rot2<T> rt = r21.transpose(); Vec2D xt = rt.rotate(xr); std::cout << x.xy() << std::endl; std::cout << xt << std::endl; std::cout << rt.rotateBack(xt) << std::endl; std::cout << r21 << std::endl; std::cout << rt << std::endl; std::cout << r21*rt << std::endl; std::cout << r22 << std::endl; std::cout << r21*r22 << std::endl; std::cout << r22*r21 << std::endl; std::cout << r21*r22.transpose() << std::endl; std::cout << r21.transpose()*r22 << std::endl; } }