INS INSInitializer::initializeINS() const {
// Average up all accel and mag samples
Eigen::Vector3d y_a = mean(Eigen::Vector3d::Zero(), y_a_log.begin(), y_a_log.end());
Eigen::Vector3d y_m = mean(Eigen::Vector3d::Zero(), y_m_log.begin(), y_m_log.end());
// Use TRIAD to compute a rotation matrix
Eigen::Vector3d a = -config.g_nav.normalized();
Eigen::Vector3d a_hat = y_a.normalized();
Eigen::Vector3d m = a.cross(config.m_nav).normalized();
Eigen::Vector3d m_hat = a_hat.cross(y_m).normalized();
Eigen::Matrix3d R_imu2nav =
(Eigen::Matrix3d() << a, m, a.cross(m)).finished() *
(Eigen::Matrix3d() << a_hat, m_hat, a_hat.cross(m_hat)).finished().transpose();
// Sum up all the DVL readings
Eigen::Vector4d y_d = mean(Eigen::Vector4d::Zero(), y_d_log.begin(), y_d_log.end());
// Use least squares with the beam matrix to determine our velocity
Eigen::Vector3d v_imu_init =
(config.beam_mat.transpose()*config.beam_mat).lu().solve( // TODO fix guide
config.beam_mat.transpose() * y_d);
// Use the depth sensor to determine our z position
double p_nav_z = -mean(0, y_z_log.begin(), y_z_log.end())
- (R_imu2nav*config.r_imu2depth)[2];
// Initialize an INS
return INS(Eigen::Quaterniond(R_imu2nav),
R_imu2nav * v_imu_init,
Eigen::Vector3d(0, 0, p_nav_z),
config.g_nav);
}
/*! This function calculates the torsion angle of three vectors, represented
by four points A--B--C--D, i.e. B and C are vertexes, but none of A--B,
B--C, and C--D are colinear. A "torsion angle" is the amount of "twist"
or torsion needed around the B--C axis to bring A--B into the same plane
as B--C--D. The torsion is measured by "looking down" the vector B--C so
that B is superimposed on C, then noting how far you'd have to rotate
A--B to superimpose A over D. Angles are + in theanticlockwise
direction. The operation is symmetrical in that if you reverse the image
(look from C to B and rotate D over A), you get the same answer.
*/
OBAPI double VectorTorsion(const Eigen::Vector3d& a, const Eigen::Vector3d& b,
const Eigen::Vector3d& c, const Eigen::Vector3d& d)
{
// Bond vectors of the three atoms
Eigen::Vector3d ab = b - a;
Eigen::Vector3d bc = c - b;
Eigen::Vector3d cd = d - c;
// length of the three bonds
const double l_ab = ab.norm();
const double l_bc = bc.norm();
const double l_cd = cd.norm();
if (IsNearZero(l_ab) || IsNearZero(l_bc) || IsNearZero(l_cd) ) {
return 0.0;
}
// normalize the bond vectors:
ab *= (1.0 / l_ab);
bc *= (1.0 / l_bc);
cd *= (1.0 / l_cd);
const Eigen::Vector3d ca = ab.cross(bc);
const Eigen::Vector3d cb = bc.cross(cd);
const Eigen::Vector3d cc = ca.cross(cb);
const double d1 = cc.dot(bc);
const double d2 = ca.dot(cb);
const double tor = RAD_TO_DEG * atan2(d1, d2);
return tor;
}
std::pair<Eigen::Vector3d, double> resolveCenterOfPressure(Eigen::Vector3d torque, Eigen::Vector3d force, Eigen::Vector3d normal, Eigen::Vector3d point_on_contact_plane)
{
// TODO: implement multi-column version
using namespace Eigen;
if (abs(normal.squaredNorm() - 1.0) > 1e-12) {
mexErrMsgIdAndTxt("Drake:resolveCenterOfPressure:BadInputs", "normal should be a unit vector");
}
Vector3d cop;
double normal_torque_at_cop;
double fz = normal.dot(force);
bool cop_exists = abs(fz) > 1e-12;
if (cop_exists) {
auto torque_at_point_on_contact_plane = torque - point_on_contact_plane.cross(force);
double normal_torque_at_point_on_contact_plane = normal.dot(torque_at_point_on_contact_plane);
auto tangential_torque = torque_at_point_on_contact_plane - normal * normal_torque_at_point_on_contact_plane;
cop = normal.cross(tangential_torque) / fz + point_on_contact_plane;
auto torque_at_cop = torque - cop.cross(force);
normal_torque_at_cop = normal.dot(torque_at_cop);
}
else {
cop.setConstant(std::numeric_limits<double>::quiet_NaN());
normal_torque_at_cop = std::numeric_limits<double>::quiet_NaN();
}
return std::pair<Vector3d, double>(cop, normal_torque_at_cop);
}
Eigen::Vector3d ManipTool::update_translation_est(Eigen::Vector3d lv,Eigen::Vector3d rv,\
Eigen::Matrix3d robot_eef_rm, MyrmexTac *myrtac){
Eigen::Matrix3d omiga_skmatrix;
Eigen::Vector3d temp_lv;
Eigen::Vector3d r_hat;
r_hat.setZero();
temp_lv.setZero();
omiga_skmatrix.setZero();
omiga_skmatrix = vectortoskew(rv);
temp_lv(1) = myrtac->ctc_vel(0);
temp_lv(0) = myrtac->ctc_vel(1);
//get the vector from center of tactile sensor to the contact point
r_hat(0) = myrtac->cog_y -7.5;
r_hat(1) = myrtac->cog_x -7.5;
L_r_dot = (-1)*beta_r*L_r-omiga_skmatrix*omiga_skmatrix;
//compute the ve, comparing with Yannis's paper, I am using -v as the Tao.
//because tao = r /cross (f), and v = omega /cross (r)
c_r_dot = (-1)*beta_r*c_r+omiga_skmatrix*(-1)*((ts.tac_sensor_cfm_local*((-1)*temp_lv*0.005/0.004)-\
rv.cross(ts.tac_sensor_cfm_local*r_hat*0.005)) - lv);
est_trans_dot = (-1)*Gama_r*(L_r*est_trans-c_r);
L_r = L_r + L_r_dot;
c_r = c_r + c_r_dot;
est_trans = est_trans + est_trans_dot;
return ((ts.tac_sensor_cfm_local*(temp_lv*0.005/0.004)-\
rv.cross(ts.tac_sensor_cfm_local*r_hat*0.005)) - lv);
}
void SubMoleculeTest::rotate()
{
SubMolecule *sub = m_source_h2o->getRandomSubMolecule();
// Rotate into xy-plane: Align the cross product of the bond vectors
// with the z-axis
Q_ASSERT(sub->numBonds() == 2);
const Eigen::Vector3d b1= *sub->bond(0)->beginPos()-*sub->bond(0)->endPos();
const Eigen::Vector3d b2= *sub->bond(1)->beginPos()-*sub->bond(1)->endPos();
const Eigen::Vector3d cross = b1.cross(b2).normalized();
// Axis is the cross-product of cross with zhat:
const Eigen::Vector3d axis = cross.cross(Eigen::Vector3d::UnitZ()).normalized();
// Angle is the angle between cross and jhat:
const double angle = acos(cross.dot(Eigen::Vector3d::UnitZ()));
// Rotate the submolecule
sub->rotate(angle, axis);
// Verify that the molecule is in the xy-plane
QVERIFY(fabs(sub->atom(0)->pos()->z()) < 1e-2);
QVERIFY(fabs(sub->atom(1)->pos()->z()) < 1e-2);
QVERIFY(fabs(sub->atom(2)->pos()->z()) < 1e-2);
delete sub;
}
bool ConvexPolygon::isInside(const Eigen::Vector3d& p)
{
Eigen::Vector3d A0 = vertices_[0];
Eigen::Vector3d B0 = vertices_[0 + 1];
Eigen::Vector3d direction0 = (B0 - A0).normalized();
Eigen::Vector3d direction20 = (p - A0).normalized();
bool direction_way = direction0.cross(direction20).dot(normal_) > 0;
for (size_t i = 1; i < vertices_.size() - 1; i++) {
Eigen::Vector3d A = vertices_[i];
Eigen::Vector3d B = vertices_[i + 1];
Eigen::Vector3d direction = (B - A).normalized();
Eigen::Vector3d direction2 = (p - A).normalized();
if (direction_way) {
if (direction.cross(direction2).dot(normal_) >= 0) {
continue;
}
else {
return false;
}
}
else {
if (direction.cross(direction2).dot(normal_) <= 0) {
continue;
}
else {
return false;
}
}
}
return true;
}
void Optimizer::eachCloudPair(CloudPair &pair)
{
int cloud0 = pair.corresIdx.first;
int cloud1 = pair.corresIdx.second;
size_t matrix_size = m_pointClouds.size() * 6;
TriContainer mat_elem;
mat_elem.reserve(matrix_size * matrix_size / 5);
SparseMatFiller filler(mat_elem);
for (int i = 0; i < 6; ++i) {
filler.add(i, i, 1.0);
}
Vec atb(matrix_size);
Mat ata(matrix_size, matrix_size);
atb.setZero(), ata.setZero();
double score = 0.0;
{
//pcl::ScopeTime time("calculate LSE matrix");
#pragma unroll 8
for (size_t point_count = 0; point_count < pair.corresPointIdx.size(); ++point_count) {
int point_p = pair.corresPointIdx[point_count].first;
int point_q = pair.corresPointIdx[point_count].second;
PointType P = m_pointClouds[cloud0]->points[point_p];
PointType Q = m_pointClouds[cloud1]->points[point_q];
Eigen::Vector3d p = P.getVector3fMap().cast<double>();
Eigen::Vector3d q = Q.getVector3fMap().cast<double>();
Eigen::Vector3d Np = P.getNormalVector3fMap().cast<double>();
double b = -(p - q).dot(Np);
score += b * b;
Eigen::Matrix<double, 6, 1> A_p, A_q;
A_p.block<3, 1>(0, 0) = p.cross(Np);
A_p.block<3, 1>(3, 0) = Np;
A_q.block<3, 1>(0, 0) = -q.cross(Np);
A_q.block<3, 1>(3, 0) = -Np;
filler.fill(cloud0, cloud1, A_p, A_q);
atb.block<6, 1>(cloud0 * 6, 0) += A_p * b;
atb.block<6, 1>(cloud1 * 6, 0) += A_q * b;
}
ata.setFromTriplets(mat_elem.begin(), mat_elem.end());
}
{
//pcl::ScopeTime time("Fill sparse matrix");
boost::mutex::scoped_lock lock(m_cloudPairMutex);
//std::cout << "\tcurrent thread : " << boost::this_thread::get_id() << std::endl;
//PCL_INFO("\tPair <%d, %d> alignment Score : %.6f\n", cloud0, cloud1, score);
ATA += ata;
ATb += atb;
align_error += score;
}
}
Eigen::Vector3d MetricRectification::normalizeLine(Eigen::Vector3d p0, Eigen::Vector3d p1)
{
Eigen::Vector3d l = p0.cross(p1);
l.x() = l.x() / l.z();
l.y() = l.y() / l.z();
l.z() = 1.0;
//return l;
return p0.cross(p1).normalized();
}
OBAPI double VectorOOP(const Eigen::Vector3d &a, const Eigen::Vector3d &b,
const Eigen::Vector3d &c,const Eigen::Vector3d &d)
{
// This is adapted from http://scidok.sulb.uni-saarland.de/volltexte/2007/1325/pdf/Dissertation_1544_Moll_Andr_2007.pdf
// Many thanks to Andreas Moll and the BALLView developers for this
// calculate normalized bond vectors from central atom to outer atoms:
Eigen::Vector3d ab = a - b;
// store length of this bond:
const double length_ab = ab.norm();
if (IsNearZero(length_ab)) {
return 0.0;
}
// store the normalized bond vector from central atom to outer atoms:
// normalize the bond vector:
ab /= length_ab;
Eigen::Vector3d cb = c - b;
const double length_cb = cb.norm();
if (IsNearZero(length_cb)) {
return 0.0;
}
cb /= length_cb;
Eigen::Vector3d db = d - b;
const double length_db = db.norm();
if (IsNearZero(length_db)) {
return 0.0;
}
db /= length_db;
// the normal vectors of the three planes:
const Eigen::Vector3d an = ab.cross(cb);
const Eigen::Vector3d bn = cb.cross(db);
const Eigen::Vector3d cn = db.cross(ab);
// Bond angle ji to jk
const double cos_theta = ab.dot(cb);
#ifdef USE_ACOS_LOOKUP_TABLE
const double theta = acosLookup(cos_theta);
#else
const double theta = acos(cos_theta);
#endif
// If theta equals 180 degree or 0 degree
if (IsNearZero(theta) || IsNearZero(fabs(theta - M_PI))) {
return 0.0;
}
const double sin_theta = sin(theta);
const double sin_dl = an.dot(db) / sin_theta;
// the wilson angle:
const double dl = asin(sin_dl);
return RAD_TO_DEG * dl;
}
/*! This method calculates the angle between two vectors
\warning If length() of any of the two vectors is == 0.0,
this method will divide by zero. If the product of the
length() of the two vectors is very close to 0.0, but not ==
0.0, this method may behave in unexpected ways and return
almost random results; details may depend on your particular
floating point implementation. The use of this method is
therefore highly discouraged, unless you are certain that the
length()es are in a reasonable range, away from 0.0 (Stefan
Kebekus)
\deprecated This method will probably replaced by a safer
algorithm in the future.
\todo Replace this method with a more fool-proof version.
@returns the angle in degrees (0-360)
*/
OBAPI double VectorAngle (const Eigen::Vector3d& ab, const Eigen::Vector3d& bc)
{
// length of the two bonds
const double l_ab = ab.norm();
const double l_bc = bc.norm();
if (IsNearZero(l_ab) || IsNearZero(l_bc)) {
return 0.0;
}
// Calculate the cross product of v1 and v2, test if it has length unequal 0
const Eigen::Vector3d c1 = ab.cross(bc);
if (IsNearZero(c1.norm())) {
return 0.0;
}
// Calculate the cos of theta and then theta
const double dp = ab.dot(bc) / (l_ab * l_bc);
if (dp > 1.0) {
return 0.0;
} else if (dp < -1.0) {
return 180.0;
} else {
#ifdef USE_ACOS_LOOKUP_TABLE
return (RAD_TO_DEG * acosLookup(dp));
#else
return (RAD_TO_DEG * acos(dp));
#endif
}
return 0.0;
}
void TetrahedronMesh::InitMesh()
{
UpdateMesh();
// Find min tet volume
double minVol = std::numeric_limits<double>::max();
for(int t=0;t<Tetrahedra->rows();t++)
{
Eigen::Vector3d A = InitalVertices->row(Tetrahedra->coeff(t,0)).cast<double>();
Eigen::Vector3d B = InitalVertices->row(Tetrahedra->coeff(t,1)).cast<double>();
Eigen::Vector3d C = InitalVertices->row(Tetrahedra->coeff(t,2)).cast<double>();
Eigen::Vector3d D = InitalVertices->row(Tetrahedra->coeff(t,3)).cast<double>();
Eigen::Vector3d a = A-D;
Eigen::Vector3d b = B-D;
Eigen::Vector3d c = C-D;
double vol = a.dot(c.cross(b));
if(vol < minVol)
minVol = vol;
}
EPS1 = 10e-5;
EPS3 = minVol*EPS1;
}
// - line has starting point (x0, y0, z0) and ending point (x1, y1, z1)
bool LineIntersect(Eigen::Vector3d& line_start, Eigen::Vector3d& line_end,
Eigen::Vector3d& intersection) {
// Solution : http://www.gamedev.net/community/forums/topic.asp?topic_id=467789
Eigen::Vector3d line_dir = (line_end - line_start).normalized();
Eigen::Vector3d A = this->node1->curPos;
Eigen::Vector3d B = this->node2->curPos;
Eigen::Vector3d nan_bias1(1e-10, -1e-10, 1e-10);
Eigen::Vector3d nan_bias2(-1e-10, 1e-10, -1e-10);
Eigen::Vector3d AB = (B - A)+nan_bias1;
Eigen::Vector3d AO = (line_start - A)+nan_bias2;
Eigen::Vector3d AOxAB = AO.cross(AB);
Eigen::Vector3d VxAB = line_dir.cross(AB);
double ab2 = AB.dot(AB);
double a = VxAB.dot(VxAB);
double b = 2 * VxAB.dot(AOxAB);
double c = AOxAB.dot(AOxAB) - (r*r * ab2);
double d = b*b - 4*a*c;
if (d < 0)
return false;
double t = (-b - sqrt(d)) / (2 * a);
if (t < 0)
return false;
intersection = line_start + line_dir*t; /// intersection point
Eigen::Vector3d projection = A + (AB.dot(intersection - A) / ab2) * AB; /// intersection projected onto cylinder axis
if ((projection - A).norm() + (B - projection).norm() > AB.norm() + 1e-5)
return false;
return true;
}
Eigen::Vector3d interpolate_3x3(const Eigen::Vector3d &this_vec, const double &time_now, const Eigen::Vector3d &next_vec, const double &time_other, const double &time_between)
{
Eigen::Quaternion<double> quat(1.0, 0.0, 0.0, 0.0);
double this_norm = this_vec.norm();
double next_norm = next_vec.norm();
Eigen::Vector3d this_uv = unit_vec(this_vec);
Eigen::Vector3d next_uv = unit_vec(next_vec);
double x = this_uv.dot(next_uv);
double y = limit(x,-1.0, 1.0);
double theta = acos(y);
double adjust = (time_between - time_now) / (time_other - time_now);
double factor = ((next_norm - this_norm) * adjust + this_norm) / this_norm;
theta = theta * adjust;
double sto2 = sin(theta / 2.0);
Eigen::Vector3d ax = next_vec.cross(this_vec);
Eigen::Vector3d ax_uv = unit_vec(ax);
double qx, qy, qz, qw;
qx = ax_uv(0) * sto2;
qy = ax_uv(1) * sto2;
qz = ax_uv(2) * sto2;
qw = cos(theta/2.0);
quat = Eigen::Quaternion<double>(qw,qx,qy,qz);
Eigen::Vector3d z = this_vec * factor;
Eigen::Quaternion<double> z_q(0.0, z(0), z(1), z(2));
Eigen::Quaternion<double> q1 = z_q * quat;
Eigen::Quaternion<double> retaq = quat.inverse();
Eigen::Quaternion<double> q2 = retaq * q1;
return q2.vec();
}
/*
* Creates a rotation matrix which describes how a point in an auxiliary
* coordinate system, whose x axis is desbibed by vec_along_x_axis and has
* a point on its xz-plane vec_on_xz_plane, rotates into the real coordinate
* system.
*/
void Cornea::createRotationMatrix(const Eigen::Vector3d &vec_along_x_axis,
const Eigen::Vector3d &vec_on_xz_plane,
Eigen::Matrix3d &R) {
// normalise pw
Eigen::Vector3d vec_on_xz_plane_n = vec_on_xz_plane.normalized();
// define helper variables x, y and z
// x
Eigen::Vector3d xn = vec_along_x_axis.normalized();
// y
Eigen::Vector3d tmp = vec_on_xz_plane_n.cross(xn);
Eigen::Vector3d yn = tmp.normalized();
// z
tmp = xn.cross(yn);
Eigen::Vector3d zn = tmp.normalized();
// create the rotation matrix
R.col(0) << xn(0), xn(1), xn(2);
R.col(1) << yn(0), yn(1), yn(2);
R.col(2) << zn(0), zn(1), zn(2);
}
//line triangle intersections
Eigen::Vector3d Triangle::shortestDistanceTo(Eigen::Vector3d line_segment_start, Eigen::Vector3d line_segment_end, Eigen::Vector3d& mesh_closest_point)
{
Eigen::Vector3d shortest_dist(BIG_DOUBLE,BIG_DOUBLE,BIG_DOUBLE);
//line intersection with all of the edges
Eigen::Vector3d normal = ((points[1]-points[0]).cross(points[2]-points[1])).normalized();
float d = points[1].dot(normal);
Eigen::Vector3d p = line_segment_start - (line_segment_start.dot(normal) -d)*normal;
Eigen::Vector3d r = (line_segment_end - (line_segment_end.dot(normal)-d)*normal) - p;
for (int i = 0; i < 3; ++i)
{
Eigen::Vector3d q = points[i];
Eigen::Vector3d s = points[(i+1)%3] - q;
double u = ((q-p).cross(r)).norm()/(r.cross(s)).norm();
double t = ((q-p).cross(s)).norm()/(r.cross(s)).norm();
if (u > -EPISILON && u < 1+ EPISILON && t > -EPISILON && t < 1+EPISILON)
{
Eigen::Vector3d closest_point = q + u*s;
Eigen::Vector3d mesh_close_point = (line_segment_start*(1-t) + line_segment_end*t);
Eigen::Vector3d short_distance = mesh_close_point - closest_point;
if (short_distance.norm() < shortest_dist.norm())
{
shortest_dist = short_distance;
mesh_closest_point = mesh_close_point;
}
}
}
return shortest_dist;
}
void tangent_and_bitangent(const Eigen::Vector3d & n_,
Eigen::Vector3d & t_, Eigen::Vector3d & b_)
{
double rmin = 0.99;
double n0 = n_(0), n1 = n_(1), n2 = n_(2);
if (std::abs(n0) <= rmin) {
rmin = std::abs(n0);
t_(0) = 0.0;
t_(1) = - n2 / std::sqrt(1.0 - std::pow(n0, 2));
t_(2) = n1 / std::sqrt(1.0 - std::pow(n0, 2));
}
if (std::abs(n1) <= rmin) {
rmin = std::abs(n1);
t_(0) = n2 / std::sqrt(1.0 - std::pow(n1, 2));
t_(1) = 0.0;
t_(2) = - n0 / std::sqrt(1.0 - std::pow(n1, 2));
}
if (std::abs(n2) <= rmin) {
rmin = std::abs(n2);
t_(0) = n1 / std::sqrt(1.0 - std::pow(n2, 2));
t_(1) = -n0 / std::sqrt(1.0 - std::pow(n2, 2));
t_(2) = 0.0;
}
b_ = n_.cross(t_);
// Check that the calculated Frenet-Serret frame is left-handed (levogiro)
// by checking that the determinant of the matrix whose columns are the normal,
// tangent and bitangent vectors has determinant 1 (the system is orthonormal!)
Eigen::Matrix3d M;
M.col(0) = n_;
M.col(1) = t_;
M.col(2) = b_;
if (boost::math::sign(M.determinant()) != 1) {
PCMSOLVER_ERROR("Frenet-Serret local frame is not left-handed!", BOOST_CURRENT_FUNCTION);
}
}
void Mesh::computeFaceGradients(Eigen::MatrixXd& gradients, const Eigen::VectorXd& u) const
{
for (FaceCIter f = faces.begin(); f != faces.end(); f++) {
Eigen::Vector3d gradient;
gradient.setZero();
Eigen::Vector3d normal = f->normal();
normal.normalize();
HalfEdgeCIter he = f->he;
do {
double ui = u(he->next->next->vertex->index);
Eigen::Vector3d ei = he->next->vertex->position - he->vertex->position;
gradient += ui * normal.cross(ei);
he = he->next;
} while (he != f->he);
gradient /= (2.0 * f->area());
gradient.normalize();
gradients.row(f->index) = -gradient;
}
}
void function(const Eigen::Ref<const Eigen::VectorXd> &x, Eigen::Ref<Eigen::VectorXd> out) const override
{
if (chains_ == 2)
{
out[0] = (getTip(x, 0) - getTip(x, 1)).norm() - radius_ * 2;
return;
}
unsigned int idx = 0;
Eigen::Vector3d centroid = Eigen::Vector3d::Zero();
for (unsigned int i = 0; i < chains_; ++i)
centroid += getTip(x, i);
centroid /= chains_;
for (unsigned int i = 0; i < chains_; ++i)
out[idx++] = (centroid - getTip(x, i)).norm() - radius_;
for (unsigned int i = 0; i < chains_ - 3; ++i)
{
const Eigen::Vector3d ab = getTip(x, i + 1) - getTip(x, i);
const Eigen::Vector3d ac = getTip(x, i + 2) - getTip(x, i);
const Eigen::Vector3d ad = getTip(x, i + 3) - getTip(x, i);
out[idx++] = ad.dot(ab.cross(ac));
}
}
void Camera::pan(Eigen::Vector2i oldPosition, Eigen::Vector2i newPosition){
const Eigen::Vector2d delta =
(newPosition - oldPosition).eval().template cast<double>();
const Eigen::Vector3d forwardVector = lookAt - position;
const Eigen::Vector3d rightVector = forwardVector.cross(up);
const Eigen::Vector3d upVector = rightVector.cross(forwardVector);
const double scale = 0.0005;
position += scale*(-delta.x()*rightVector +
delta.y()*upVector);
}
//==============================================================================
Eigen::Isometry3d State::getCOMFrame() const
{
Eigen::Isometry3d T = Eigen::Isometry3d::Identity();
// Y-axis
Eigen::Vector3d yAxis = Eigen::Vector3d::UnitY();
// X-axis
Eigen::Vector3d xAxis = mPelvis->getTransform().linear().col(0);
Eigen::Vector3d pelvisXAxis = mPelvis->getTransform().linear().col(0);
double mag = yAxis.dot(pelvisXAxis);
pelvisXAxis -= mag * yAxis;
xAxis = pelvisXAxis.normalized();
// Z-axis
Eigen::Vector3d zAxis = xAxis.cross(yAxis);
T.translation() = getCOM();
T.linear().col(0) = xAxis;
T.linear().col(1) = yAxis;
T.linear().col(2) = zAxis;
return T;
}
//! Determine moment coefficients from pressure coefficients.
Eigen::Vector3d HypersonicLocalInclinationAnalysis::calculateMomentCoefficients(
const int partNumber )
{
// Declare moment coefficient vector and intialize to zeros.
Eigen::Vector3d momentCoefficients = Eigen::Vector3d::Zero( );
// Declare moment arm for panel moment determination.
Eigen::Vector3d referenceDistance ;
// Loop over all panels and add moments due pressures.
for ( int i = 0 ; i < vehicleParts_[ partNumber ]->getNumberOfLines( ) - 1 ; i++ )
{
for ( int j = 0 ; j < vehicleParts_[ partNumber ]->getNumberOfPoints( ) - 1 ; j++ )
{
// Determine moment arm for given panel centroid.
referenceDistance = ( vehicleParts_[ partNumber ]->
getPanelCentroid( i, j ) - momentReferencePoint_ );
momentCoefficients -=
pressureCoefficient_[ partNumber ][ i ][ j ] *
vehicleParts_[ partNumber ]->getPanelArea( i, j ) *
( referenceDistance.cross( vehicleParts_[ partNumber ]->
getPanelSurfaceNormal( i, j ) ) );
}
}
// Scale result by reference length and area.
momentCoefficients /= ( referenceLength_ * referenceArea_ );
return momentCoefficients;
}
void RigidBody::operator()(const RigidBody::InternalState &x, RigidBody::InternalState &dxdt, const double /* t */)
{
Eigen::Vector3d x_dot, v_dot, omega_dot;
Eigen::Vector4d q_dot;
Eigen::Vector4d q(attitude.x(), attitude.y(), attitude.z(), attitude.w());
Eigen::Vector3d omega = angularVelocity;
Eigen::Matrix4d omegaMat = Omega(omega);
x_dot = velocity;
v_dot = force/mass;
q_dot = 0.5*omegaMat*q;
omega_dot = inertia.inverse() *
(torque - omega.cross(inertia*omega));
dxdt[0] = x_dot(0);
dxdt[1] = x_dot(1);
dxdt[2] = x_dot(2);
dxdt[3] = v_dot(0);
dxdt[4] = v_dot(1);
dxdt[5] = v_dot(2);
dxdt[6] = q_dot(0);
dxdt[7] = q_dot(1);
dxdt[8] = q_dot(2);
dxdt[9] = q_dot(3);
dxdt[10] = omega_dot(0);
dxdt[11] = omega_dot(1);
dxdt[12] = omega_dot(2);
}
bool
Triangulation::isInside(const ON_NurbsCurve &curve, const pcl::PointXYZ &v)
{
Eigen::Vector2d vp (v.x, v.y);
Eigen::Vector3d a0, a1;
pcl::on_nurbs::NurbsTools::computeBoundingBox (curve, a0, a1);
double rScale = 1.0 / pcl::on_nurbs::NurbsTools::computeRScale (a0, a1);
Eigen::Vector2d pc, tc;
double err, param;
if (curve.Order () == 2)
param = pcl::on_nurbs::FittingCurve2dAPDM::inverseMappingO2 (curve, vp, err, pc, tc);
else
{
param = pcl::on_nurbs::FittingCurve2dAPDM::findClosestElementMidPoint (curve, vp);
param = pcl::on_nurbs::FittingCurve2dAPDM::inverseMapping (curve, vp, param, err, pc, tc, rScale);
}
Eigen::Vector3d a (vp (0) - pc (0), vp (1) - pc (1), 0.0);
Eigen::Vector3d b (tc (0), tc (1), 0.0);
Eigen::Vector3d z = a.cross (b);
return (z (2) >= 0.0);
}
double Face::distance(const Eigen::Vector3d& origin, const Eigen::Vector3d& direction) const
{
// check for false intersection with the outside of the mesh
if (!sameDirection(direction, normal().normalized())) {
return INFINITY;
}
// Möller–Trumbore intersection algorithm
const Eigen::Vector3d& p1(he->vertex->position);
const Eigen::Vector3d& p2(he->next->vertex->position);
const Eigen::Vector3d& p3(he->next->next->vertex->position);
Eigen::Vector3d e1 = p2 - p1;
Eigen::Vector3d e2 = p3 - p1;
Eigen::Vector3d n = direction.cross(e2);
double det = e1.dot(n);
// ray does not lie in the plane
if (det > -EPSILON && det < EPSILON) {
return INFINITY;
}
double invDet = 1.0 / det;
Eigen::Vector3d t = origin - p1;
double u = t.dot(n) * invDet;
// ray lies outside triangle
if (u < 0.0 || u > 1.0) {
return INFINITY;
}
Eigen::Vector3d q = t.cross(e1);
double v = direction.dot(q) * invDet;
// ray lies outside the triangle
if (v < 0.0 || v + u > 1.0) {
return INFINITY;
}
double s = e2.dot(q) * invDet;
// intersection
if (s > EPSILON) {
return s;
}
// no hit
return INFINITY;
}
Eigen::MatrixXd cIKSolver::BuildConsPosJacob(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& pose, const tConsDesc& cons_desc)
{
assert(static_cast<int>(cons_desc(eConsDescType)) == eConsTypePos);
int num_joints = static_cast<int>(joint_mat.rows());
int parent_id = static_cast<int>(cons_desc(eConsDescParam0));
tVector attach_pt = tVector(cons_desc(eConsDescParam1), cons_desc(eConsDescParam2), 0.f, 0.f);
tVector end_pos = cKinTree::LocalToWorldPos(joint_mat, pose, parent_id, attach_pt);
const Eigen::Vector3d rot_axis = Eigen::Vector3d(0, 0, 1);
int num_dof = cKinTree::GetNumDof(joint_mat);
Eigen::MatrixXd J = Eigen::MatrixXd(gPosDims, num_dof);
J.setZero();
for (int i = 0; i < gPosDims; ++i)
{
J(i, i) = 1;
}
int curr_id = parent_id;
while (true)
{
tVector joint_pos = cKinTree::CalcJointWorldPos(joint_mat, pose, curr_id);
tVector delta = end_pos - joint_pos;
Eigen::Vector3d tangent = rot_axis.cross(Eigen::Vector3d(delta(0), delta(1), delta(2)));
int curr_parent_id = cKinTree::GetParent(joint_mat, curr_id);
for (int i = 0; i < gPosDims; ++i)
{
J(i, gPosDims + curr_id) = tangent(i);
}
if (curr_parent_id == cKinTree::gInvalidJointID)
{
// no scaling for root
break;
}
else
{
#if !defined(DISABLE_LINK_SCALE)
double attach_x = joint_desc(curr_id, cKinTree::eJointDescAttachX);
double attach_y = joint_desc(curr_id, cKinTree::eJointDescAttachY);
double attach_z = joint_desc(curr_id, cKinTree::eJointDescAttachZ);
double parent_world_theta = cKinTree::CalcJointWorldTheta(joint_desc, curr_parent_id);
double world_attach_x = std::cos(parent_world_theta) * attach_x - std::sin(parent_world_theta) * attach_y;
double world_attach_y = std::sin(parent_world_theta) * attach_x + std::cos(parent_world_theta) * attach_y;
double world_attach_z = attach_z; // hack ignoring z, do this properly
J(0, gPosDims + num_joints + curr_id) = world_attach_x;
J(1, gPosDims + num_joints + curr_id) = world_attach_y;
#endif
curr_id = curr_parent_id;
}
}
return J;
}
static void qprod(const Eigen::Vector4d & q, const Eigen::Vector4d & p, Eigen::Vector4d & prod_result) {
double a=q(0);
Eigen::Vector3d v = q.segment(1, 3); //coefficients q
double x=p(0);
Eigen::Vector3d u = p.segment(1, 3); //coefficients p
prod_result << a*x-v.transpose()*u, (a*u+x*v) + v.cross(u);
}
/* Calculate the distance of point a to the plane determined by b,c,d */
double Point2Plane(const Eigen::Vector3d& a, const Eigen::Vector3d& b,
const Eigen::Vector3d& c, const Eigen::Vector3d& d)
{
Eigen::Vector3d ab = a - b;
Eigen::Vector3d bc = c - b;
Eigen::Vector3d bd = d - b;
Eigen::Vector3d normal = bc.cross(bd);
return fabs( normal.dot( ab ) / normal.norm() );
}
void taskwrenchEllipsoid::sampleComEllipsoid()
{
//sampleParametricForm, might make more sense to sample more in one direction
//also sample over mass from m-delta -> m=delta
int iMax=10;
int jMax=10;
int lMax=4;
int k=0;
for(int i=0; i < iMax; i++){
for(int j=0; j < jMax; j++){
for(int l=0; l<lMax;l++){
//u von -Pi/2 bis Pi/2
//v von -Pi bis Pi
double u=-M_PI/2 + ((double) i / (double) iMax)*M_PI;
double v=-M_PI + ((double) j / (double) jMax)*2*M_PI;
double w=mass_ - sigmaMass_ + ((double) l / (double) lMax)*2*sigmaMass_;
Eigen::Vector3d currentCom; //parametrized in PCA frame
Eigen::Vector3d force;
Eigen::Vector3d torque;
Eigen::Vector3d comInRotFrame;
currentCom(0)=sigmaCom_(0)*cos(u)*cos(v);
currentCom(1)=sigmaCom_(1)*cos(u)*sin(v);
currentCom(2)=sigmaCom_(2)*sin(u);
//std::cout << currentCom << std::endl;
//std::cout << u << std::endl;
//std::cout << v << std::endl;
force=-9.81*w*gripperRot_*gravityNormal_;//use SI Units for now, transform into hand frame, not sure about pose!
//comInRotFrame=comRotFrame_*currentCom; // sampling done in the pca frame!
if(l==0){force_min=force;}
if(l==lMax-1){force_max=force;}
torque=currentCom.cross(force);
comEllipse_.col(k)=comInRotFrame;
sampledPointsEllipse_.col(k)=torque;
k++;
//
}
}
}
}
void NuTo::CollidableWallBase::VisualizationStatic(NuTo::Visualize::UnstructuredGrid& rVisualizer) const
{
double size = 1.;
if (*mBoxes.begin() == mOutsideBox)
size = 2.;
Eigen::Matrix<double, 4, 3> corners;
// get some vector != mDirection
Eigen::Vector3d random;
random << 1, 0, 0;
if (std::abs(random.dot(mDirection)) == 1)
{
random << 0, 1, 0;
}
Eigen::Vector3d transversal = random.cross(mDirection);
Eigen::Vector3d transversal2 = transversal.cross(mDirection);
// normalize to size/2;
transversal.normalize();
transversal2.normalize();
transversal *= size / 2;
transversal2 *= size / 2;
corners.row(0) = (mPosition + transversal + transversal2).transpose();
corners.row(1) = (mPosition + transversal - transversal2).transpose();
corners.row(2) = (mPosition - transversal - transversal2).transpose();
corners.row(3) = (mPosition - transversal + transversal2).transpose();
std::vector<int> cornerIndex(4);
for (int i = 0; i < 4; ++i)
{
cornerIndex[i] = rVisualizer.AddPoint(corners.row(i));
}
int insertIndex = rVisualizer.AddCell(cornerIndex, eCellTypes::QUAD);
rVisualizer.SetCellData(insertIndex, "Direction", mDirection);
}
drag::drag(const Eigen::Quaterniond& q, const Eigen::Vector3d& v, const Eigen::Vector3d& w, const Eigen::Vector3d& x, const Eigen::Vector3d& n, double c, double a)
: q(q)
, v(v)
, w(w)
, x(x)
, n(n)
, t(x.cross(n))
, c(c)
, a(a)
{
}
