void Transform3f::transformLocal (const Transform3f &transform)
{
Transform3f tmp;
tmp.composition(transform, *this);
copy(tmp);
}
/// @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;
}
void Vec3f::rotate (const Vec3f &around, float angle)
{
Transform3f matr;
matr.rotation(around.x, around.y, around.z, angle);
transformVector(matr);
}
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;
}
void Transform3f::rotateZLocal (float angle)
{
Transform3f rot;
rot.rotationZ(angle);
transformLocal(rot);
}
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()));
}
void Transform3f::transform (const Transform3f &transform)
{
Transform3f tmp;
tmp.composition(*this, transform);
copy(tmp);
}
void Transform3f::rotateY (float angle)
{
Transform3f rot;
rot.rotationY(angle);
transform(rot);
}
/** 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);
}
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;
}
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);
}
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);
}
FCL_REAL TriangleMotionBoundVisitor::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 proj_max = ((tf.getQuatRotation().transform(a) + tf.getTranslation() - p).cross(axis)).sqrLength();
FCL_REAL tmp;
tmp = ((tf.getQuatRotation().transform(b) + tf.getTranslation() - p).cross(axis)).sqrLength();
if(tmp > proj_max) proj_max = tmp;
tmp = ((tf.getQuatRotation().transform(c) + tf.getTranslation() - p).cross(axis)).sqrLength();
if(tmp > proj_max) proj_max = tmp;
proj_max = std::sqrt(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 mu = v_dot_n + w_cross_n * proj_max;
return mu;
}
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;
}
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;
}
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);
}
CMUK_ERROR_CODE cmuk::computeFootIK( LegIndex leg,
const vec3f& pos,
vec3f* q_bent_forward,
vec3f* q_bent_rearward ) const {
if ((int)leg < 0 || (int)leg >= NUM_LEGS) {
return CMUK_BAD_LEG_INDEX;
} else if (!q_bent_forward || !q_bent_rearward) {
return CMUK_INSUFFICIENT_ARGUMENTS;
}
debug << "*** computing IK...\n";
int hipflags = 0;
// subtract off hip position
vec3f p = pos - jo(_kc, leg, HIP_RX_OFFSET, _centeredFootIK);
vec3f orig = pos;
// get dist from hip rx joint to y rotation plane
const float& d = jo(_kc, leg, HIP_RY_OFFSET, _centeredFootIK)[1];
// get the squared length of the distance on the plane
float yz = p[1]*p[1] + p[2]*p[2];
// alpha is the angle of the foot in the YZ plane with respect to the Y axis
float alpha = atan2(p[2], p[1]);
// h is the distance of foot from hip in YZ plane
float h = sqrt(yz);
// beta is the angle between the foot-hip vector (projected in YZ
// plane) and the top hip link.
float cosbeta = d / h;
debug << "p = " << p << ", d = " << d << ", yz = " << yz << "\nalpha = " << alpha << ", h = " << h << ", cosbeta=" << cosbeta << "\n";
if (fabs(cosbeta) > 1) {
debug << "violated triangle inequality when calculating hip_rx_angle!\n" ;
if (fabs(cosbeta) - 1 > 1e-4) {
hipflags = hipflags | IK_UPPER_DISTANCE;
}
cosbeta = (cosbeta < 0) ? -1 : 1;
if (yz < 1e-4) {
p[1] = d;
p[2] = 0;
} else {
float scl = fabs(d) / h;
p[1] *= scl;
p[2] *= scl;
orig = p + jo(_kc, leg, HIP_RX_OFFSET, _centeredFootIK);
}
}
float beta = acos(cosbeta);
// Now compute the two possible hip angles
float hip_rx_angles[2], badness[2];
int flags[2];
flags[0] = hipflags;
flags[1] = hipflags;
hip_rx_angles[0] = fix_angle(alpha - beta, -M_PI, M_PI);
hip_rx_angles[1] = fix_angle(alpha + beta, -M_PI, M_PI);
const float& min = jl(_kc, leg, HIP_RX, 0);
const float& max = jl(_kc, leg, HIP_RX, 1);
// See how badly we violate the joint limits for this hip angles
for (int i=0; i<2; ++i) {
float& angle = hip_rx_angles[i];
badness[i] = fabs(compute_badness(angle, min, max));
if (badness[i]) { flags[i] = flags[i] | IK_UPPER_ANGLE_RANGE; }
}
// Put the least bad (and smallest) hip angle first
bool swap = false;
if ( badness[1] <= badness[0] ) {
// We want the less bad solution for hip angle
swap = true;
} else if (badness[0] == 0 && badness[1] == 0) {
// We want the solution for hip angle that leaves the hip up.
if ((leg == FL || leg == HL) && hip_rx_angles[0] > hip_rx_angles[1]) {
swap = true;
} else if ((leg == FR || leg == HR) && hip_rx_angles[0] < hip_rx_angles[1]) {
swap = true;
}
}
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";
if (fabs(costheta) - 1 > 1e-4) {
flags[i] = flags[i] | IK_LOWER_DISTANCE;
}
costheta = (costheta < 0) ? -1 : 1;
}
float theta = acos(costheta);
// gamma is the angle of the foot with respect to the z axis
float gamma = atan2(-ptx[0], -ptx[2]);
// hip angles are just offsets off of gamma now
float hip_ry_1 = gamma - theta;
float hip_ry_2 = gamma + theta;
// phi is the obtuse angle of the parallelogram
float cosphi = (l1sqr + l2sqr - ksqr) / (2 * l1 * l2);
if (fabs(cosphi) > 1) {
debug << "cosphi = " << cosphi << " > 1\n";
if (fabs(cosphi) - 1 > 1e-4) {
flags[i] = flags[i] | IK_LOWER_DISTANCE;
}
cosphi = (cosphi < 0) ? -1 : 1;
}
float phi = acos(cosphi);
// epsilon is the "error" caused by not having feet offset directly
// along the z-axis (if they were, epsilon would equal zero)
float epsilon = le(_kc, leg, _centeredFootIK);
// now we can directly solve for knee angles
float knee_ry_1 = M_PI - phi - epsilon;
float knee_ry_2 = -M_PI + phi - epsilon;
// now fill out angle structs and check limits
qfwd[i] = vec3f(hip_rx, hip_ry_1, knee_ry_1);
qrear[i] = vec3f(hip_rx, hip_ry_2, knee_ry_2);
debug << "before wrap, qfwd = " << qfwd[i] << "\n";
debug << "before wrap, qrear = " << qrear[i] << "\n";
check_wrap(_kc, qfwd[i], leg);
check_wrap(_kc, qrear[i], leg);
debug << "after wrap, qfwd = " << qfwd[i] << "\n";
debug << "after wrap, qrear = " << qrear[i] << "\n";
if (!check_limits(_kc, qfwd[i], leg)) {
debug << "violated limits forward!\n";
flags[i] = flags[i] | IK_LOWER_ANGLE_RANGE_FWD;
}
if (!check_limits(_kc, qrear[i], leg)) {
debug << "violated limits rearward!\n";
flags[i] = flags[i] | IK_LOWER_ANGLE_RANGE_REAR;
}
} // for each viable hip solution
int best = 0;
if (hip_solution_cnt == 2) {
if (howbad(flags[0]) > howbad(flags[1])) {
best = 1;
}
debug << "best overall solution is " << (best+1) << "\n";
}
*q_bent_forward = qfwd[best];
*q_bent_rearward = qrear[best];
return flags_to_errcode(flags[best]);
}
CMUK_ERROR_CODE cmuk::getRelTransform( const cmuk::XformCache& cache,
FrameIndex f0,
FrameIndex f1,
Transform3f* xform ) const {
int if0 = int(f0);
int if1 = int(f1);
if (if0 != FRAME_GROUND && (if0 < 0 || if0 >= NUM_FRAMES)) {
return CMUK_BAD_FRAME_INDEX;
}
if (if1 !=FRAME_GROUND && (if1 < 0 || if1 >= NUM_FRAMES)) {
return CMUK_BAD_FRAME_INDEX;
}
if (!xform) {
return CMUK_INSUFFICIENT_ARGUMENTS;
}
int leg0 = -1;
int leg1 = -1;
if (if0 > int(FRAME_TRUNK)) { leg0 = (if0 - 1) / 4; }
if (if1 > int(FRAME_TRUNK)) { leg1 = (if0 - 1) / 4; }
if (leg0 >= 0 && leg1 >= 0 && leg0 != leg1) {
Transform3f t1, t2;
CMUK_ERROR_CODE status;
status = getRelTransform(cache, f0, FRAME_TRUNK, &t1);
if (status != CMUK_OKAY) { return status; }
status = getRelTransform(cache, FRAME_TRUNK, f1, &t2);
if (status != CMUK_OKAY) { return status; }
*xform = t1 * t2;
return CMUK_OKAY;
}
if (if0 == if1) {
*xform = Transform3f();
return CMUK_OKAY;
}
// if1 should be smaller than if0
bool swap = if0 < if1;
if (swap) {
int tmp = if0;
if0 = if1;
if1 = tmp;
}
Transform3f rval = cache.relXforms[if0];
if0 = parentFrame(if0);
// now we are going to decrement if0
while (if1 < if0 && if0 > 0) {
rval = rval * cache.relXforms[if0];
if0 = parentFrame(if0);
}
if (swap) {
rval = rval.inverse();
}
*xform = rval;
return CMUK_OKAY;
}
bool Transform3f::bestFit (int npoints, const Vec3f points[], const Vec3f goals[], float *sqsum_out)
{
QS_DEF(Array<double>, X); //set of points
QS_DEF(Array<double>, Y); //set of goals
Matr3x3d R, RT, RTR, evectors_matrix;
//
Matr3x3d rotation;
double scale;
Vec3f translation;
//
bool res = 1;
Vec3f vec, tmp;
double cpoints[3] = {0.0}, cgoals[3] = {0.0}; // centroid of points, of goals
int i, j, k;
for (i = 0; i < npoints; i++)
{
cpoints[0] += points[i].x;
cpoints[1] += points[i].y;
cpoints[2] += points[i].z;
cgoals[0] += goals[i].x;
cgoals[1] += goals[i].y;
cgoals[2] += goals[i].z;
}
for (i = 0; i < 3; i++)
{
cpoints[i] /= npoints;
cgoals[i] /= npoints;
}
X.resize(npoints * 3);
Y.resize(npoints * 3);
//move each set to origin
for (i = 0; i < npoints; i++)
{
X[i * 3 + 0] = points[i].x - cpoints[0];
X[i * 3 + 1] = points[i].y - cpoints[1];
X[i * 3 + 2] = points[i].z - cpoints[2];
Y[i * 3 + 0] = goals[i].x - cgoals[0];
Y[i * 3 + 1] = goals[i].y - cgoals[1];
Y[i * 3 + 2] = goals[i].z - cgoals[2];
}
if (npoints > 1)
{
/* compute R */
for (i = 0; i < 3; i++)
{
for (j = 0; j < 3; j++)
{
R.elements[i * 3 + j] = 0.0;
for (k = 0; k < npoints; k++)
{
R.elements[i * 3 + j] += Y[k * 3 + i] * X[k * 3 + j];
}
}
}
//Compute R^T * R
R.getTransposed(RT);
RT.matrixMatrixMultiply(R, RTR);
RTR.eigenSystem(evectors_matrix);
if (RTR.elements[0] > 2 * EPSILON)
{
float norm_b0,norm_b1,norm_b2;
Vec3f a0, a1, a2;
Vec3f b0, b1, b2;
a0.set((float)evectors_matrix.elements[0], (float)evectors_matrix.elements[3], (float)evectors_matrix.elements[6]);
a1.set((float)evectors_matrix.elements[1], (float)evectors_matrix.elements[4], (float)evectors_matrix.elements[7]);
a2.cross(a0, a1);
R.matrixVectorMultiply(a0, b0);
R.matrixVectorMultiply(a1, b1);
norm_b0 = b0.length();
norm_b1 = b1.length();
Line3f l1, l2;
float sqs1, sqs2;
l1.bestFit(npoints, points, &sqs1);
l2.bestFit(npoints, goals, &sqs2);
if( sqs1 < 2 * EPSILON && sqs2 < 2 * EPSILON)
{
Transform3f temp;
temp.rotationVecVec(l1.dir, l2.dir);
for (i = 0; i < 3; i++)
for (j = 0; j < 3; j++)
rotation.elements[i * 3 + j] = temp.elements[j * 4 + i];
}
else
{
b0.normalize();
b1.normalize();
b2.cross(b0, b1);
norm_b2 = b2.length();
evectors_matrix.elements[2] = a2.x;
evectors_matrix.elements[5] = a2.y;
evectors_matrix.elements[8] = a2.z;
evectors_matrix.transpose();
RTR.elements[0] = b0.x; RTR.elements[1] = b1.x; RTR.elements[2] = b2.x;
RTR.elements[3] = b0.y; RTR.elements[4] = b1.y; RTR.elements[5] = b2.y;
RTR.elements[6] = b0.z; RTR.elements[7] = b1.z; RTR.elements[8] = b2.z;
RTR.matrixMatrixMultiply(evectors_matrix, rotation);
}
}
else
{
res = 0;
}
}
else
{
res = 0;
}
if (!res)
{
rotation.identity();
}
//Calc scale
scale = 1.0;
if (res && npoints > 1)
{
float l1 = 0.0;
float l2 = 0.0;
Vec3f vx, vy;
for (i = 0; i < npoints; i++)
{
Vec3f vx((float)X[i * 3 + 0], (float)X[i * 3 + 1], (float)X[i * 3 + 2]);
Vec3f vy((float)Y[i * 3 + 0], (float)Y[i * 3 + 1], (float)Y[i * 3 + 2]);
rotation.matrixVectorMultiply(vx, vec);
l1 += Vec3f::dot(vy, vec);
l2 += Vec3f::dot(vec, vec);
}
scale = l1 / l2;
}
X.clear();
Y.clear();
//Calc translation
translation.set((float)cgoals[0], (float)cgoals[1], (float)cgoals[2]);
tmp = Vec3f((float)cpoints[0], (float)cpoints[1], (float)cpoints[2]);
rotation.matrixVectorMultiply(tmp, vec);
vec.scale((float)scale);
translation.sub(vec);
identity();
for (i = 0; i < 3; i++)
{
for (j = 0; j < 3; j++)
{
elements[i * 4 + j] = (float)rotation.elements[j * 3 + i];
}
}
elements[15] = 1.0f;
translate(translation);
for (i = 0; i < 3; i++)
{
for (j = 0; j < 3; j++)
{
elements[i * 4 + j] *= (float)scale;
}
}
//Deviation
if (sqsum_out)
{
*sqsum_out = 0;
float d = .0f;
for (i = 0; i < npoints; i++)
{
vec.pointTransformation(points[i], *this);
d = Vec3f::dist(vec, goals[i]);
*sqsum_out += d * d;
}
}
return true;
}
bool conservativeAdvancementMeshOriented(const BVHModel<BV>& o1,
const MotionBase* motion1,
const BVHModel<BV>& o2,
const MotionBase* motion2,
const CollisionRequest& request,
CollisionResult& result,
FCL_REAL& toc)
{
Transform3f tf1, tf2;
motion1->getCurrentTransform(tf1);
motion2->getCurrentTransform(tf2);
// whether the first start configuration is in collision
if(collide(&o1, tf1, &o2, tf2, request, result))
{
toc = 0;
return true;
}
ConservativeAdvancementOrientedNode node;
initialize(node, o1, tf1, o2, tf2);
node.motion1 = motion1;
node.motion2 = motion2;
do
{
node.motion1->getCurrentTransform(tf1);
node.motion2->getCurrentTransform(tf2);
// compute the transformation from 1 to 2
Transform3f tf;
relativeTransform(tf1, tf2, tf);
node.R = tf.getRotation();
node.T = tf.getTranslation();
node.delta_t = 1;
node.min_distance = std::numeric_limits<FCL_REAL>::max();
distanceRecurse(&node, 0, 0, NULL);
if(node.delta_t <= node.t_err)
{
// std::cout << node.delta_t << " " << node.t_err << std::endl;
break;
}
node.toc += node.delta_t;
if(node.toc > 1)
{
node.toc = 1;
break;
}
node.motion1->integrate(node.toc);
node.motion2->integrate(node.toc);
}
while(1);
toc = node.toc;
if(node.toc < 1)
return true;
return false;
}
void relativeTransform(const Transform3f& tf1, const Transform3f& tf2,
Transform3f& tf)
{
const Quaternion3f& q1_inv = fcl::conj(tf1.getQuatRotation());
tf = Transform3f(q1_inv * tf2.getQuatRotation(), q1_inv.transform(tf2.getTranslation() - tf1.getTranslation()));
}
/** \reimpl
*/
void
FeatureLabelSetGeometry::render(RenderContext& rc, double /* clock */) const
{
const float visibleSizeThreshold = 20.0f; // in pixels
// No need to draw anything if the labels are turned off with an opacity
// setting near 0.
if (ms_globalOpacity <= 0.01f)
{
return;
}
// Render during the opaque pass if opaque or during the translucent pass if not.
if (rc.pass() == RenderContext::TranslucentPass)
{
// Get the position of the camera in the body-fixed frame of the labeled object
Transform3f inv = Transform3f(rc.modelview().inverse(Affine)); // Assuming an affine modelview matrix
Vector3f cameraPosition = inv.translation();
float overallPixelSize = boundingSphereRadius() / (rc.pixelSize() * cameraPosition.norm());
// Only draw individual labels if the overall projected size of the set exceeds the threshold
if (overallPixelSize > visibleSizeThreshold)
{
// Labels are treated as either completely visible or completely occluded. A label is
// visible when the labeled point isn't blocked by the occluding ellipsoid.
AlignedEllipsoid testEllipsoid(m_occludingEllipsoid.semiAxes() * 0.999);
Vector3f ellipsoidSemiAxes = testEllipsoid.semiAxes().cast<float>();
Vector3f viewDir = -cameraPosition.normalized();
double distanceToEllipsoid = 0.0;
// Instead of computing the ellipsoid intersection (as the line below), just treat the planet as a sphere
//TestRayEllipsoidIntersection(cameraPosition, viewDir, ellipsoidSemiAxes, &distanceToEllipsoid);
distanceToEllipsoid = (cameraPosition.norm() - ellipsoidSemiAxes.maxCoeff()) * 0.99f;
// We don't want labels partially hidden by the planet ellipsoid, so we'll project them onto a
// plane that lies just in front of the planet ellipsoid and which is parallel to the view plane
Hyperplane<float, 3> labelPlane(viewDir, cameraPosition + viewDir * float(distanceToEllipsoid));
for (vector<Feature, Eigen::aligned_allocator<Feature> >::const_iterator iter = m_features.begin(); iter != m_features.end(); ++iter)
{
Vector3f r = iter->position - cameraPosition;
Vector3f labelPosition = labelPlane.projection(iter->position);
float k = -(labelPlane.normal().dot(cameraPosition) + labelPlane.offset()) / (labelPlane.normal().dot(r));
labelPosition = cameraPosition + k * r;
rc.pushModelView();
rc.translateModelView(labelPosition);
float featureDistance = rc.modelview().translation().norm();
float pixelSize = iter->size / (rc.pixelSize() * featureDistance);
float d = r.norm();
r /= d;
double t = 0.0;
TestRayEllipsoidIntersection(cameraPosition, r, ellipsoidSemiAxes, &t);
if (pixelSize > visibleSizeThreshold && d < t)
{
rc.drawEncodedText(Vector3f::Zero(), iter->label, m_font.ptr(), TextureFont::Utf8, iter->color, ms_globalOpacity);
}
rc.popModelView();
}
}
}
}
bool EdgeRotationMatcher::match (float rms_threshold, float eps)
{
if (cb_get_xyz == 0)
throw Error("cb_get_xyz not specified");
if (_subgraph.vertexCount() < 2 || _subgraph.edgeCount() < 1)
return true;
QS_DEF(Array<int>, in_cycle);
QS_DEF(Array<_DirEdge>, edge_queue);
QS_DEF(Array<int>, vertex_queue);
QS_DEF(Array<int>, states);
in_cycle.clear_resize(_subgraph.edgeEnd());
edge_queue.clear();
states.clear_resize(__max(_subgraph.edgeEnd(), _subgraph.vertexEnd() + 1));
int i, j, k, bottom;
// Find all subgraph bridges
SpanningTree spt(_subgraph, 0);
in_cycle.zerofill();
spt.markAllEdgesInCycles(in_cycle.ptr(), 1);
// Find the first bridge, put it to the queue 2 times
for (i = _subgraph.edgeBegin(); i < _subgraph.edgeEnd(); i = _subgraph.edgeNext(i))
if (!in_cycle[i] && (cb_can_rotate == 0 || cb_can_rotate(_subgraph, i)))
{
const Edge &edge = _subgraph.getEdge(i);
if (_mapping[edge.beg] < 0 || _mapping[edge.end] < 0)
continue;
edge_queue.push();
edge_queue.top().idx = i;
edge_queue.top().beg = edge.beg;
edge_queue.top().end = edge.end;
edge_queue.push();
edge_queue.top().idx = i;
edge_queue.top().beg = edge.end;
edge_queue.top().end = edge.beg;
break;
}
// If the queue is empty, then we have no bridge
if (edge_queue.size() == 0)
{
GraphAffineMatcher afm(_subgraph, _supergraph, _mapping);
afm.cb_get_xyz = cb_get_xyz;
return afm.match(rms_threshold);
}
float scale = 1.f;
// detect scaling factor by average bond length
if (equalize_edges)
{
float sum_sub = 0.f, sum_super = 0.f;
for (i = _subgraph.edgeBegin(); i < _subgraph.edgeEnd(); i = _subgraph.edgeNext(i))
{
const Edge &edge = _subgraph.getEdge(i);
Vec3f beg, end;
cb_get_xyz(_subgraph, edge.beg, beg);
cb_get_xyz(_subgraph, edge.end, end);
sum_sub += Vec3f::dist(beg, end);
}
for (i = _supergraph.edgeBegin(); i < _supergraph.edgeEnd(); i = _supergraph.edgeNext(i))
{
const Edge &edge = _supergraph.getEdge(i);
Vec3f beg, end;
cb_get_xyz(_supergraph, edge.beg, beg);
cb_get_xyz(_supergraph, edge.end, end);
sum_super += Vec3f::dist(beg, end);
}
if (sum_sub > EPSILON && sum_super > EPSILON)
{
sum_sub /= _subgraph.edgeCount();
sum_super /= _supergraph.edgeCount();
scale = sum_super / sum_sub;
}
}
// save vertex positions
QS_DEF(Array<Vec3f>, xyz_sub);
QS_DEF(Array<Vec3f>, xyz_super);
QS_DEF(Array<int>, xyzmap);
xyzmap.clear_resize(_supergraph.vertexEnd());
xyz_sub.clear();
xyz_super.clear();
for (i = _subgraph.vertexBegin(); i != _subgraph.vertexEnd();
i = _subgraph.vertexNext(i))
{
if (_mapping[i] < 0)
continue;
Vec3f &pos_sub = xyz_sub.push();
Vec3f &pos_super = xyz_super.push();
cb_get_xyz(_subgraph, i, pos_sub);
cb_get_xyz(_supergraph, _mapping[i], pos_super);
pos_sub.scale(scale);
xyzmap[_mapping[i]] = xyz_sub.size() - 1;
}
// Make queue of edges
states.zerofill();
bottom = 0;
while (edge_queue.size() != bottom)
{
// extract edge from queue
int edge_end = edge_queue[bottom].end;
int edge_idx = edge_queue[bottom].idx;
bottom++;
// mark it as 'completed'
states[edge_idx] = 2;
// look for neighbors
const Vertex &end_vertex = _subgraph.getVertex(edge_end);
for (i = end_vertex.neiBegin(); i != end_vertex.neiEnd(); i = end_vertex.neiNext(i))
{
int nei_edge_idx = end_vertex.neiEdge(i);
// check that neighbor have 'untouched' status
if (states[nei_edge_idx] != 0)
continue;
const Edge &nei_edge = _subgraph.getEdge(nei_edge_idx);
int other_end = nei_edge.findOtherEnd(edge_end);
if (_mapping[other_end] < 0)
continue;
// set status 'in process'
states[nei_edge_idx] = 1;
// push the neighbor edge to the queue
edge_queue.push();
edge_queue.top().idx = nei_edge_idx;
edge_queue.top().beg = edge_end;
edge_queue.top().end = other_end;
}
}
// do initial transform (impose first subgraph edge in the queue on corresponding one in the graph)
int beg2 = edge_queue[0].beg;
int end2 = edge_queue[0].end;
int beg1 = _mapping[beg2];
int end1 = _mapping[end2];
Vec3f g1_v1, g1_v2, g2_v1, g2_v2, diff1, diff2;
Transform3f matr;
cb_get_xyz(_supergraph, beg1, g1_v1);
cb_get_xyz(_supergraph, end1, g1_v2);
cb_get_xyz(_subgraph, beg2, g2_v1);
cb_get_xyz(_subgraph, end2, g2_v2);
g2_v1.scale(scale);
g2_v2.scale(scale);
diff1.diff(g1_v2, g1_v1);
diff2.diff(g2_v2, g2_v1);
matr.identity();
if (!matr.rotationVecVec(diff2, diff1))
throw Error("error calling RotationVecVec()");
matr.translateLocal(-g2_v1.x, -g2_v1.y, -g2_v1.z);
matr.translate(g1_v1);
for (k = 0; k < xyz_sub.size(); k++)
xyz_sub[k].transformPoint(matr);
// for all edges in queue that are subject to rotate...
for (i = 0; i < edge_queue.size(); i++)
{
int edge_beg = edge_queue[i].beg;
int edge_end = edge_queue[i].end;
int edge_idx = edge_queue[i].idx;
if (in_cycle[edge_idx])
continue;
if (cb_can_rotate != 0 && !cb_can_rotate(_subgraph, edge_idx))
continue;
// start BFS from the end of the edge
states.zerofill();
states[edge_end] = 1;
vertex_queue.clear();
vertex_queue.push(edge_end);
bottom = 0;
while (vertex_queue.size() != bottom)
{
// extract vertex from queue
const Vertex &vertex = _subgraph.getVertex(vertex_queue[bottom]);
states[vertex_queue[bottom]] = 2;
bottom++;
// look over neighbors
for (int j = vertex.neiBegin(); j != vertex.neiEnd(); j = vertex.neiNext(j))
{
int nei_idx = vertex.neiVertex(j);
if (nei_idx == edge_beg)
continue;
if (states[nei_idx] != 0)
continue;
states[nei_idx] = 1;
vertex_queue.push(nei_idx);
}
}
// now states[j] == 0 if j-th vertex shound not be moved
Vec3f edge_beg_pos, edge_end_pos, rot_axis;
// get rotation axis
edge_beg_pos.copy(xyz_sub[xyzmap[_mapping[edge_beg]]]);
edge_end_pos.copy(xyz_sub[xyzmap[_mapping[edge_end]]]);
rot_axis.diff(edge_end_pos, edge_beg_pos);
if (!rot_axis.normalize())
continue;
const Vertex &edge_end_vertex = _subgraph.getVertex(edge_end);
float max_sum_len = -1;
for (j = edge_end_vertex.neiBegin(); j != edge_end_vertex.neiEnd();
j = edge_end_vertex.neiNext(j))
{
int nei_idx_2 = edge_end_vertex.neiVertex(j);
int nei_idx_1 = _mapping[nei_idx_2];
if (nei_idx_2 == edge_beg)
continue;
if (nei_idx_1 == -1)
continue;
Vec3f nei1_pos;
Vec3f nei2_pos;
nei1_pos.copy(xyz_super[xyzmap[nei_idx_1]]);
nei2_pos.copy(xyz_sub[xyzmap[_mapping[nei_idx_2]]]);
nei1_pos.sub(edge_end_pos);
nei2_pos.sub(edge_end_pos);
float dot1 = Vec3f::dot(nei1_pos, rot_axis);
float dot2 = Vec3f::dot(nei2_pos, rot_axis);
nei1_pos.addScaled(rot_axis, -dot1);
nei2_pos.addScaled(rot_axis, -dot2);
if (max_sum_len > nei1_pos.length() + nei1_pos.length())
continue;
max_sum_len = nei1_pos.length() + nei1_pos.length();
if (!nei1_pos.normalize() || !nei2_pos.normalize())
continue;
double dp = Vec3f::dot(nei1_pos, nei2_pos);
if (dp > 1 - EPSILON)
dp = 1 - EPSILON;
if (dp < -1 + EPSILON)
dp = -1 + EPSILON;
double ang = acos(dp);
Vec3f cross;
cross.cross(nei1_pos, nei2_pos);
if (Vec3f::dot(cross, rot_axis) < 0)
ang = -ang;
matr.rotation(rot_axis.x, rot_axis.y, rot_axis.z, (float)ang);
matr.translateLocalInv(edge_end_pos);
matr.translate(edge_end_pos);
}
if (max_sum_len > 0)
{
for (j = _subgraph.vertexBegin(); j < _subgraph.vertexEnd(); j = _subgraph.vertexNext(j))
if (_mapping[j] >= 0 && states[j] != 0)
xyz_sub[xyzmap[_mapping[j]]].transformPoint(matr);
}
}
float sqsum = 0;
for (k = 0; k < xyz_sub.size(); k++)
sqsum += Vec3f::distSqr(xyz_sub[k], xyz_super[k]);
sqsum = sqrt(sqsum / xyz_sub.size());
if (sqsum > rms_threshold + eps)
return false;
return true;
}
bool collide_front_list_Test(const Transform3f& tf1, const Transform3f& tf2,
const std::vector<Vec3f>& vertices1, const std::vector<Triangle>& triangles1,
const std::vector<Vec3f>& vertices2, const std::vector<Triangle>& triangles2,
SplitMethodType split_method,
bool refit_bottomup, bool verbose)
{
BVHModel<BV> m1;
BVHModel<BV> m2;
m1.bv_splitter.reset(new BVSplitter<BV>(split_method));
m2.bv_splitter.reset(new BVSplitter<BV>(split_method));
BVHFrontList front_list;
std::vector<Vec3f> vertices1_new(vertices1.size());
for(std::size_t i = 0; i < vertices1_new.size(); ++i)
{
vertices1_new[i] = tf1.transform(vertices1[i]);
}
m1.beginModel();
m1.addSubModel(vertices1_new, triangles1);
m1.endModel();
m2.beginModel();
m2.addSubModel(vertices2, triangles2);
m2.endModel();
Transform3f pose1, pose2;
CollisionResult local_result;
MeshCollisionTraversalNode<BV> node;
if(!initialize<BV>(node, m1, pose1, m2, pose2,
CollisionRequest(std::numeric_limits<int>::max(), false), local_result))
std::cout << "initialize error" << std::endl;
node.enable_statistics = verbose;
collide(&node, &front_list);
if(verbose) std::cout << "front list size " << front_list.size() << std::endl;
// update the mesh
for(std::size_t i = 0; i < vertices1.size(); ++i)
{
vertices1_new[i] = tf2.transform(vertices1[i]);
}
m1.beginReplaceModel();
m1.replaceSubModel(vertices1_new);
m1.endReplaceModel(true, refit_bottomup);
m2.beginReplaceModel();
m2.replaceSubModel(vertices2);
m2.endReplaceModel(true, refit_bottomup);
local_result.clear();
collide(&node, &front_list);
if(local_result.numContacts() > 0)
return true;
else
return false;
}