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