本文整理汇总了C++中Transform3f类的典型用法代码示例。如果您正苦于以下问题:C++ Transform3f类的具体用法?C++ Transform3f怎么用?C++ Transform3f使用的例子?那么恭喜您, 这里精选的类代码示例或许可以为您提供帮助。
在下文中一共展示了Transform3f类的15个代码示例,这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞,您的评价将有助于系统推荐出更棒的C++代码示例。
示例1: copy
void Transform3f::transformLocal (const Transform3f &transform)
{
Transform3f tmp;
tmp.composition(transform, *this);
copy(tmp);
}
示例2: visit
/// @brief Compute the motion bound for a triangle along a given direction n
/// according to mu < |v * n| + ||w x n||(max||ci*||) where ||ci*|| = ||R0(ci) x w|| / \|w\|. w is the angular velocity
/// and ci are the triangle vertex coordinates.
/// Notice that the triangle is in the local frame of the object, but n should be in the global frame (the reason is that the motion (t1, t2 and t) is in global frame)
FCL_REAL TriangleMotionBoundVisitor::visit(const InterpMotion& motion) const
{
Transform3f tf;
motion.getCurrentTransform(tf);
const Vec3f& reference_p = motion.getReferencePoint();
const Vec3f& angular_axis = motion.getAngularAxis();
FCL_REAL angular_vel = motion.getAngularVelocity();
const Vec3f& linear_vel = motion.getLinearVelocity();
FCL_REAL proj_max = ((tf.getQuatRotation().transform(a - reference_p)).cross(angular_axis)).sqrLength();
FCL_REAL tmp;
tmp = ((tf.getQuatRotation().transform(b - reference_p)).cross(angular_axis)).sqrLength();
if(tmp > proj_max) proj_max = tmp;
tmp = ((tf.getQuatRotation().transform(c - reference_p)).cross(angular_axis)).sqrLength();
if(tmp > proj_max) proj_max = tmp;
proj_max = std::sqrt(proj_max);
FCL_REAL v_dot_n = linear_vel.dot(n);
FCL_REAL w_cross_n = (angular_axis.cross(n)).length() * angular_vel;
FCL_REAL mu = v_dot_n + w_cross_n * proj_max;
return mu;
}
示例3: transformVector
void Vec3f::rotate (const Vec3f &around, float angle)
{
Transform3f matr;
matr.rotation(around.x, around.y, around.z, angle);
transformVector(matr);
}
示例4:
FCL_REAL TBVMotionBoundVisitor<RSS>::visit(const InterpMotion& motion) const
{
Transform3f tf;
motion.getCurrentTransform(tf);
const Vec3f& reference_p = motion.getReferencePoint();
const Vec3f& angular_axis = motion.getAngularAxis();
FCL_REAL angular_vel = motion.getAngularVelocity();
const Vec3f& linear_vel = motion.getLinearVelocity();
FCL_REAL c_proj_max = ((tf.getQuatRotation().transform(bv.Tr - reference_p)).cross(angular_axis)).sqrLength();
FCL_REAL tmp;
tmp = ((tf.getQuatRotation().transform(bv.Tr + bv.axis[0] * bv.l[0] - reference_p)).cross(angular_axis)).sqrLength();
if(tmp > c_proj_max) c_proj_max = tmp;
tmp = ((tf.getQuatRotation().transform(bv.Tr + bv.axis[1] * bv.l[1] - reference_p)).cross(angular_axis)).sqrLength();
if(tmp > c_proj_max) c_proj_max = tmp;
tmp = ((tf.getQuatRotation().transform(bv.Tr + bv.axis[0] * bv.l[0] + bv.axis[1] * bv.l[1] - reference_p)).cross(angular_axis)).sqrLength();
if(tmp > c_proj_max) c_proj_max = tmp;
c_proj_max = std::sqrt(c_proj_max);
FCL_REAL v_dot_n = linear_vel.dot(n);
FCL_REAL w_cross_n = (angular_axis.cross(n)).length() * angular_vel;
FCL_REAL mu = v_dot_n + w_cross_n * (bv.r + c_proj_max);
return mu;
}
示例5: transformLocal
void Transform3f::rotateZLocal (float angle)
{
Transform3f rot;
rot.rotationZ(angle);
transformLocal(rot);
}
示例6: relativeTransform2
void relativeTransform2(const Transform3f& tf1, const Transform3f& tf2,
Transform3f& tf)
{
const Quaternion3f& q1inv = fcl::conj(tf1.getQuatRotation());
const Quaternion3f& q2_q1inv = tf2.getQuatRotation() * q1inv;
tf = Transform3f(q2_q1inv, tf2.getTranslation() - q2_q1inv.transform(tf1.getTranslation()));
}
示例7: transform
void Transform3f::rotateY (float angle)
{
Transform3f rot;
rot.rotationY(angle);
transform(rot);
}
示例8: shapeToGJK
/** Basic shape to ccd shape */
static void shapeToGJK(const ShapeBase& s, const Transform3f& tf, ccd_obj_t* o)
{
const Quaternion3f& q = tf.getQuatRotation();
const Vec3f& T = tf.getTranslation();
ccdVec3Set(&o->pos, T[0], T[1], T[2]);
ccdQuatSet(&o->rot, q.getX(), q.getY(), q.getZ(), q.getW());
ccdQuatInvert2(&o->rot_inv, &o->rot);
}
示例9: Error
bool GraphAffineMatcher::match (float rms_threshold)
{
if (cb_get_xyz == 0)
throw Error("cb_get_xyz not set");
int i;
Transform3f matr;
Vec3f pos;
QS_DEF(Array<Vec3f>, points);
QS_DEF(Array<Vec3f>, goals);
points.clear();
goals.clear();
if (fixed_vertices != 0)
{
for (i = 0; i < fixed_vertices->size(); i++)
{
if (_mapping[fixed_vertices->at(i)] < 0)
continue;
cb_get_xyz(_subgraph, fixed_vertices->at(i), pos);
points.push(pos);
cb_get_xyz(_supergraph, _mapping[fixed_vertices->at(i)], pos);
goals.push(pos);
}
}
else for (i = _subgraph.vertexBegin(); i < _subgraph.vertexEnd(); i = _subgraph.vertexNext(i))
{
if (_mapping[i] < 0)
continue;
cb_get_xyz(_subgraph, i, pos);
points.push(pos);
cb_get_xyz(_supergraph, _mapping[i], pos);
goals.push(pos);
}
if (points.size() < 1)
return true;
float sqsum;
if (!matr.bestFit(points.size(), points.ptr(), goals.ptr(), &sqsum))
return false;
if (sqsum > rms_threshold * rms_threshold)
return false;
return true;
}
示例10: draw
void draw()
{
int end = mCenters.size();
glEnable(GL_NORMALIZE);
for (int i=0; i<end; ++i)
{
Transform3f t = Translation3f(mCenters[i]) * Scaling3f(mRadii[i]);
gpu.pushMatrix(GL_MODELVIEW);
gpu.multMatrix(t.matrix(),GL_MODELVIEW);
mIcoSphere.draw(2);
gpu.popMatrix(GL_MODELVIEW);
}
glDisable(GL_NORMALIZE);
}
示例11: intersect_Triangle
bool Intersect::intersect_Triangle(const Vec3f& P1, const Vec3f& P2, const Vec3f& P3,
const Vec3f& Q1, const Vec3f& Q2, const Vec3f& Q3,
const Transform3f& tf,
Vec3f* contact_points,
unsigned int* num_contact_points,
FCL_REAL* penetration_depth,
Vec3f* normal)
{
Vec3f Q1_ = tf.transform(Q1);
Vec3f Q2_ = tf.transform(Q2);
Vec3f Q3_ = tf.transform(Q3);
return intersect_Triangle(P1, P2, P3, Q1_, Q2_, Q3_, contact_points, num_contact_points, penetration_depth, normal);
}
示例12: computeWorldPointFootDK
CMUK_ERROR_CODE cmuk::computeWorldPointFootDK( LegIndex leg,
JointOffset link,
const KState& state,
const vec3f& pworld,
mat3f* J_trans,
mat3f* J_rot,
float* det,
float det_tol,
float lambda ) const {
if ((int)leg < 0 || (int)leg >= NUM_LEGS) {
return CMUK_BAD_LEG_INDEX;
}
if (!J_trans || !J_rot) {
return CMUK_INSUFFICIENT_ARGUMENTS;
}
Transform3f xform = state.xform();
vec3f pbody = xform.transformInv(pworld);
vec3f fbody;
mat3f J;
CMUK_ERROR_CODE err =
computePointFootDK( leg, link, state.leg_rot[leg], pbody, &J,
&fbody, det, det_tol, lambda );
if (err != CMUK_OKAY && err != CMUK_SINGULAR_MATRIX) {
return err;
}
const mat3f& R = xform.rotFwd();
const mat3f& Rinv = xform.rotInv();
*J_trans = mat3f::identity() - R * J * Rinv;
//*J_rot = R * (-mat3f::cross(pbody) + J * mat3f::cross(fbody));
*J_rot = R * ( J * mat3f::cross(fbody) - mat3f::cross(pbody) ) * Rinv;
return err;
}
示例13: sqrt
FCL_REAL TBVMotionBoundVisitor<RSS>::visit(const ScrewMotion& motion) const
{
Transform3f tf;
motion.getCurrentTransform(tf);
const Vec3f& axis = motion.getAxis();
FCL_REAL linear_vel = motion.getLinearVelocity();
FCL_REAL angular_vel = motion.getAngularVelocity();
const Vec3f& p = motion.getAxisOrigin();
FCL_REAL c_proj_max = ((tf.getQuatRotation().transform(bv.Tr)).cross(axis)).sqrLength();
FCL_REAL tmp;
tmp = ((tf.getQuatRotation().transform(bv.Tr + bv.axis[0] * bv.l[0])).cross(axis)).sqrLength();
if(tmp > c_proj_max) c_proj_max = tmp;
tmp = ((tf.getQuatRotation().transform(bv.Tr + bv.axis[1] * bv.l[1])).cross(axis)).sqrLength();
if(tmp > c_proj_max) c_proj_max = tmp;
tmp = ((tf.getQuatRotation().transform(bv.Tr + bv.axis[0] * bv.l[0] + bv.axis[1] * bv.l[1])).cross(axis)).sqrLength();
if(tmp > c_proj_max) c_proj_max = tmp;
c_proj_max = sqrt(c_proj_max);
FCL_REAL v_dot_n = axis.dot(n) * linear_vel;
FCL_REAL w_cross_n = (axis.cross(n)).length() * angular_vel;
FCL_REAL origin_proj = ((tf.getTranslation() - p).cross(axis)).length();
FCL_REAL mu = v_dot_n + w_cross_n * (c_proj_max + bv.r + origin_proj);
return mu;
}
示例14: generateRandomTransform
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);
}
示例15: if
//.........这里部分代码省略.........
}
if (swap) {
std::swap(hip_rx_angles[0], hip_rx_angles[1]);
std::swap(badness[0], badness[1]);
std::swap(flags[0], flags[1]);
}
int hip_solution_cnt = 2;
if (badness[0] == 0 && badness[1] != 0) {
hip_solution_cnt = 1;
}
debug << "hip_rx_angles[0]=" << hip_rx_angles[0]
<< ", badness=" << badness[0]
<< ", flags=" << flags[0] << "\n";
debug << "hip_rx_angles[1]=" << hip_rx_angles[1]
<< ", badness=" << badness[1]
<< ", flags=" << flags[1] << "\n";
debug << "hip_solution_cnt = " << hip_solution_cnt << "\n";
vec3f qfwd[2], qrear[2];
for (int i=0; i<hip_solution_cnt; ++i) {
debug << "** computing ll solution " << (i+1) << " of " << (hip_solution_cnt) << "\n";
float hip_rx = hip_rx_angles[i];
// now make inv. transform to get rid of hip rotation
Transform3f tx = Transform3f::rx(hip_rx, jo(_kc, leg, HIP_RX_OFFSET, _centeredFootIK));
vec3f ptx = tx.transformInv(orig);
debug << "tx=[" << tx.translation() << ", " << tx.rotation() << "], ptx = " << ptx << "\n";
// calculate lengths for cosine law
float l1sqr = ol2(_kc, leg, KNEE_RY_OFFSET, _centeredFootIK);
float l2sqr = ol2(_kc, leg, FOOT_OFFSET, _centeredFootIK);
float l1 = ol(_kc, leg, KNEE_RY_OFFSET, _centeredFootIK);
float l2 = ol(_kc, leg, FOOT_OFFSET, _centeredFootIK);
float ksqr = ptx[0]*ptx[0] + ptx[2]*ptx[2];
float k = sqrt(ksqr);
debug << "l1=" << l1 << ", l2=" << l2 << ", k=" << k << "\n";
// check triangle inequality
if (k > l1 + l2) {
debug << "oops, violated the triangle inequality for lower segments: "
<< "k = " << k << ", "
<< "l1 + l2 = " << l1 + l2 << "\n";
if (k - (l1 + l2) > 1e-4) {
flags[i] = flags[i] | IK_LOWER_DISTANCE;
}
k = l1 + l2;
ksqr = k * k;
}
// 2*theta is the acute angle formed by the spread
// of the two hip rotations...
float costheta = (l1sqr + ksqr - l2sqr) / (2 * l1 * k);
if (fabs(costheta) > 1) {
debug << "costheta = " << costheta << " > 1\n";