void generateRandomTransform_ccd(FCL_REAL extents[6], std::vector<Transform3f>& transforms, std::vector<Transform3f>& transforms2, FCL_REAL delta_trans[3], FCL_REAL delta_rot, std::size_t n, const std::vector<Vec3f>& vertices1, const std::vector<Triangle>& triangles1, const std::vector<Vec3f>& vertices2, const std::vector<Triangle>& triangles2) { transforms.resize(n); transforms2.resize(n); for(std::size_t i = 0; i < n;) { FCL_REAL x = rand_interval(extents[0], extents[3]); FCL_REAL y = rand_interval(extents[1], extents[4]); FCL_REAL z = rand_interval(extents[2], extents[5]); const FCL_REAL pi = 3.1415926; FCL_REAL a = rand_interval(0, 2 * pi); FCL_REAL b = rand_interval(0, 2 * pi); FCL_REAL c = rand_interval(0, 2 * pi); Matrix3f R; eulerToMatrix(a, b, c, R); Vec3f T(x, y, z); Transform3f tf(R, T); std::vector<std::pair<int, int> > results; { transforms[i] = tf; FCL_REAL deltax = rand_interval(-delta_trans[0], delta_trans[0]); FCL_REAL deltay = rand_interval(-delta_trans[1], delta_trans[1]); FCL_REAL deltaz = rand_interval(-delta_trans[2], delta_trans[2]); FCL_REAL deltaa = rand_interval(-delta_rot, delta_rot); FCL_REAL deltab = rand_interval(-delta_rot, delta_rot); FCL_REAL deltac = rand_interval(-delta_rot, delta_rot); Matrix3f R2; eulerToMatrix(a + deltaa, b + deltab, c + deltac, R2); Vec3f T2(x + deltax, y + deltay, z + deltaz); transforms2[i].setTransform(R2, T2); ++i; } } }
void generateRandomTransforms(FCL_REAL extents[6], FCL_REAL delta_trans[3], FCL_REAL delta_rot, std::vector<Transform3f>& transforms, std::vector<Transform3f>& transforms2, std::size_t n) { transforms.resize(n); transforms2.resize(n); for(std::size_t i = 0; i < n; ++i) { FCL_REAL x = rand_interval(extents[0], extents[3]); FCL_REAL y = rand_interval(extents[1], extents[4]); FCL_REAL z = rand_interval(extents[2], extents[5]); const FCL_REAL pi = 3.1415926; FCL_REAL a = rand_interval(0, 2 * pi); FCL_REAL b = rand_interval(0, 2 * pi); FCL_REAL c = rand_interval(0, 2 * pi); { Matrix3f R; eulerToMatrix(a, b, c, R); Vec3f T(x, y, z); transforms[i].setTransform(R, T); } FCL_REAL deltax = rand_interval(-delta_trans[0], delta_trans[0]); FCL_REAL deltay = rand_interval(-delta_trans[1], delta_trans[1]); FCL_REAL deltaz = rand_interval(-delta_trans[2], delta_trans[2]); FCL_REAL deltaa = rand_interval(-delta_rot, delta_rot); FCL_REAL deltab = rand_interval(-delta_rot, delta_rot); FCL_REAL deltac = rand_interval(-delta_rot, delta_rot); { Matrix3f R; eulerToMatrix(a + deltaa, b + deltab, c + deltac, R); Vec3f T(x + deltax, y + deltay, z + deltaz); transforms2[i].setTransform(R, T); } } }
void generateRandomTransform(FCL_REAL extents[6], Transform3f& transform) { FCL_REAL x = rand_interval(extents[0], extents[3]); FCL_REAL y = rand_interval(extents[1], extents[4]); FCL_REAL z = rand_interval(extents[2], extents[5]); const FCL_REAL pi = 3.1415926; FCL_REAL a = rand_interval(0, 2 * pi); FCL_REAL b = rand_interval(0, 2 * pi); FCL_REAL c = rand_interval(0, 2 * pi); Matrix3f R; eulerToMatrix(a, b, c, R); Vec3f T(x, y, z); transform.setTransform(R, T); }
osg::Matrix OSGConverter::requestToModelPose(const SendRequest& req) { double pitch = 0; double roll = 0; double yaw = 0; double x = 0; double y = 0; double z = 0; CAST_PARAM(req.params, pitch, "pitch", double); CAST_PARAM(req.params, roll, "roll", double); CAST_PARAM(req.params, yaw, "yaw", double); CAST_PARAM(req.params, x, "x", double); CAST_PARAM(req.params, y, "y", double); CAST_PARAM(req.params, z, "z", double); osg::Matrix m = eulerToMatrix(pitch, roll, yaw) * osg::Matrix::translate(-1 * x, -1 * y, -1 * z); #if 0 dumpMatrix(m); #endif return m; }
void generateRandomTransforms(FCL_REAL extents[6], std::vector<Transform3f>& transforms, std::size_t n) { transforms.resize(n); for(std::size_t i = 0; i < n; ++i) { FCL_REAL x = rand_interval(extents[0], extents[3]); FCL_REAL y = rand_interval(extents[1], extents[4]); FCL_REAL z = rand_interval(extents[2], extents[5]); const FCL_REAL pi = 3.1415926; FCL_REAL a = rand_interval(0, 2 * pi); FCL_REAL b = rand_interval(0, 2 * pi); FCL_REAL c = rand_interval(0, 2 * pi); { Matrix3f R; eulerToMatrix(a, b, c, R); Vec3f T(x, y, z); transforms[i].setTransform(R, T); } } }