/*! 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;
}
force::force(std::shared_ptr<particle> part1, std::shared_ptr<particle> part2, unsigned int fileId, Eigen::Vector3d pos1, Eigen::Vector3d pos2, Eigen::Vector3d val, double volWater, double distCurr, double distCrit) {
_part1 = part1;
_part2 = part2;
_volWater = volWater;
_distCurr = distCurr;
_distCrit = distCrit;
if (fabs(pos1.norm()/part1->c().norm() - 1.0) > 0.0001 or fabs(pos2.norm()/part2->c().norm() -1.0 ) > 0.0001 ) {
std::cerr<<"Particle positions in force and particle files are not the same!"<<std::endl;
std::cerr<<"pid1 "<<part1->id()<<": ("<<pos1[0]<<" "<<pos1[1]<<" "<<pos1[2]<< ") != (" << part1->c()[0]<<" "<<part1->c()[1]<<" "<<part1->c()[2];
std::cerr<<"pid2 "<<part2->id()<<": ("<<pos2[0]<<" "<<pos2[1]<<" "<<pos2[2]<< ") != (" << part2->c()[0]<<" "<<part2->c()[1]<<" "<<part2->c()[2]<<"); "<<std::endl;
std::cerr<<"dif1 "<<pos1.norm()/part1->c().norm()<<"; dif2 "<<pos2.norm()/part2->c().norm()<<std::endl<<std::endl;
}
//Updating particle positions as they are more accurate in fstat
_part1->c(pos1);
_part2->c(pos2);
_cPZ = Eigen::Vector3d::Zero();
_val = val;
_fileId = fileId;
_calculateStressTensor = false;
_cP = (pos1-pos2)/2.0 + pos1;
_axisMatrix = _axisMatrix.Zero();
};
void Mesh::computeIntegratedDivergence(Eigen::VectorXd& integratedDivs,
const Eigen::MatrixXd& gradients) const
{
for (VertexCIter v = vertices.begin(); v != vertices.end(); v++) {
double integratedDiv = 0.0;
Eigen::Vector3d p = v->position;
HalfEdgeCIter he = v->he;
do {
Eigen::Vector3d gradient = gradients.row(he->face->index);
Eigen::Vector3d p1 = he->next->vertex->position;
Eigen::Vector3d p2 = he->next->next->vertex->position;
Eigen::Vector3d e1 = p1 - p;
Eigen::Vector3d e2 = p2 - p;
Eigen::Vector3d ei = p2 - p1;
double theta1 = acos((-e2).dot(-ei) / (e2.norm() * ei.norm()));
double cot1 = 1.0 / tan(theta1);
double theta2 = acos((-e1).dot(ei) / (e1.norm() * ei.norm()));
double cot2 = 1.0 / tan(theta2);
integratedDiv += e1.dot(gradient) * cot1 + e2.dot(gradient) * cot2;
he = he->flip->next;
} while (he != v->he);
integratedDivs[v->index] = 0.5 * integratedDiv;
}
}
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();
}
/*! 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;
}
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;
}
void ManipTool::update_nv(Eigen::Vector3d lv,Eigen::Vector3d& n_hat,Eigen::Vector3d& nv_dot_fb){
P_bar = Eigen::Matrix3d::Identity()-n_hat*n_hat.transpose();
nv_dot = -1*Gama_n*P_bar*L_n*n_hat;
nv_dot_fb = nv_dot;
L_n_dot = -beta_n*L_n+(1/(1+pow(lv.norm(),2)))*lv*lv.transpose();
n_hat = n_hat+nv_dot;
n_hat = n_hat/n_hat.norm();
L_n = L_n + L_n_dot;
}
/**
* @brief set pose based on compact scaled-axis representation
*
* @param v compact 6 vector [r t], where r is a 3 vector representing
* the rotation in scaled axis form, and t is the translation 3 vector.
*/
void fromVector6d( const Vector6d &v )
{
const Eigen::Vector3d saxis = v.head<3>();
if( saxis.norm() > 1e-9 )
orientation = Eigen::AngleAxisd( saxis.norm(), saxis.normalized() );
else
orientation = Eigen::Quaterniond::Identity();
position = v.tail<3>();
}
Eigen::Vector3d Triangle::shortestDistanceTo(Triangle* other, Eigen::Vector3d& closest_point){
Eigen::Vector3d distance = shortestDistanceTo(other->points[0]);
closest_point = distance + other->points[0];
//project other triangle vertices onto us
for (int i = 1; i < 3; ++i)
{
Eigen::Vector3d new_dist = shortestDistanceTo(other->points[i]);
if (new_dist.norm() < distance.norm())
{
distance = new_dist;
closest_point = distance + other->points[i];
}
}
//project all vertices onto other triangle
for (int i = 0; i < 3; ++i)
{
Eigen::Vector3d new_dist = -other->shortestDistanceTo(points[i]);
if (new_dist.norm() < distance.norm())
{
distance = new_dist;
closest_point = points[i];
}
}
//project edges onto other triangle, do line intersections
for (int i = 0; i < 3; ++i)
{
Eigen::Vector3d close_point;
Eigen::Vector3d new_dist = -shortestDistanceTo(other->points[i], other->points[(i+1)%3], close_point);
if (new_dist.norm() < distance.norm())
{
distance = new_dist;
closest_point = close_point;
}
}
for (int i = 0; i < 3; ++i)
{
Eigen::Vector3d close_point;
Eigen::Vector3d new_dist = other->shortestDistanceTo(points[i],points[(i+1)%3], close_point);
if (new_dist.norm() < distance.norm())
{
distance = new_dist;
closest_point = points[i];
}
}
return distance;
}
void randomize_transform(Eigen::Isometry3d& tf,
double translation_limit=100,
double rotation_limit=100)
{
Eigen::Vector3d r = random_vec<3>(translation_limit);
Eigen::Vector3d theta = random_vec<3>(rotation_limit);
tf.setIdentity();
tf.translate(r);
if(theta.norm()>0)
tf.rotate(Eigen::AngleAxisd(theta.norm(), theta.normalized()));
}
// -------------------------------------------------------------------------------
Eigen::Vector3d* ComputeTrixelMidpoints(trixel* trixel)
{
static Eigen::Vector3d tmp;
Eigen::Vector3d* midPoints = new Eigen::Vector3d[3];
tmp = trixel->_vertices[1] + trixel->_vertices[2];
midPoints[0] = tmp / tmp.norm();
tmp = trixel->_vertices[0] + trixel->_vertices[2];
midPoints[1] = tmp / tmp.norm();
tmp = trixel->_vertices[0] + trixel->_vertices[1];
midPoints[2] = tmp / tmp.norm();
return midPoints;
}
Eigen::Vector3d getForcePoint(Eigen::Vector3d & c_current, Eigen::Vector3d & c_previous, const double & ro)
{
if(c_current.norm()<ro)
{
Eigen::Vector3d f=kp_mat*(ro-c_current.norm())*c_current.normalized()
-kd_mat*(c_current.norm()-c_previous.norm())*c_current.normalized();
return f;
}
else
{
return Eigen::Vector3d(0,0,0);
}
}
Quaternion::Quaternion(const Eigen::Vector3d& axis, double angle)
{
if(axis.norm() == 0.0)
{
throw Exception("Quaternion::Quaternion", "Norm of axis is zero.");
}
double inv_norm = 1.0 / axis.norm();
double sin_val = sin(0.5 * angle);
x_ = axis.coeff(0) * inv_norm * sin_val;
y_ = axis.coeff(1) * inv_norm * sin_val;
z_ = axis.coeff(2) * inv_norm * sin_val;
w_ = cos(0.5 * angle);
}
int main( )
{
// Use Boost library to make a random number generator.
boost::mt19937 randomNumbergenerator( time( 0 ) );
boost::random::uniform_real_distribution< > uniformDistribution( 0.0, 10.0 );
boost::variate_generator< boost::mt19937&, boost::random::uniform_real_distribution < > >
generateRandomNumbers( randomNumbergenerator, uniformDistribution );
// Generate random altitude value between 0 and 10 km.
const double altitudeKilometers = generateRandomNumbers( );
// Use the Tudat Core library to convert km to m.
const double altitudeMeters = tudat::unit_conversions::convertKilometersToMeters(
altitudeKilometers );
// Use the Eigen library to create position vectors.
Eigen::Vector3d positionOfBodySubjectToAcceleration;
positionOfBodySubjectToAcceleration << 1737.1e3 + altitudeMeters, 0.0, 0.0;
const Eigen::Vector3d positionOfBodyExertingAcceleration = Eigen::Vector3d::Zero( );
// Use the Tudat library to compute the acceleration vector.
const Eigen::Vector3d gravitationalAcceleration =
tudat::gravitation::computeGravitationalAcceleration(
positionOfBodySubjectToAcceleration, 4.9e12,
positionOfBodyExertingAcceleration );
// Print the altitude and norm of the acceleration vector.
std::cout << "Hello world!" << std::endl;
std::cout << "I am floating " << altitudeKilometers << " km above the Moon's surface." <<
std::endl;
std::cout << "The gravitational acceleration here is " <<
gravitationalAcceleration.norm() << " m/s^2." << std::endl;
return 0;
}
Eigen::Vector3d R2AxisAngle(Eigen::Matrix3d const & C){
// Sometimes, because of roundoff error, the value of tr ends up outside
// the valid range of arccos. Truncate to the valid range.
double tr = std::max(-1.0, std::min( (C(0,0) + C(1,1) + C(2,2) - 1.0) * 0.5, 1.0));
double a = acos( tr ) ;
Eigen::Vector3d axis;
if(fabs(a) < 1e-10) {
return Eigen::Vector3d::Zero();
}
axis[0] = (C(2,1) - C(1,2));
axis[1] = (C(0,2) - C(2,0));
axis[2] = (C(1,0) - C(0,1));
double n2 = axis.norm();
if(fabs(n2) < 1e-10)
return Eigen::Vector3d::Zero();
double scale = -a/n2;
axis = scale * axis;
return axis;
}
inline Eigen::Vector3d unit_vec(const Eigen::Vector3d &vec)
{
Eigen::Vector3d retval;
double nrm = vec.norm();
retval = vec / nrm;
return retval;
}
//! Function to convert a Cartesian to a spherical orbital state.
basic_mathematics::Vector6d convertCartesianToSphericalOrbitalState(
const basic_mathematics::Vector6d& bodyFixedCartesianState )
{
basic_mathematics::Vector6d sphericalOrbitalState;
// Compute and set spherical position
Eigen::Vector3d sphericalPosition = coordinate_conversions::convertCartesianToSpherical(
bodyFixedCartesianState.segment( 0, 3 ) );
sphericalOrbitalState( radiusIndex ) = sphericalPosition( 0 );
sphericalOrbitalState( latitudeIndex ) = mathematical_constants::PI / 2.0 - sphericalPosition( 1 );
sphericalOrbitalState( longitudeIndex ) = sphericalPosition( 2 );
// Convert velocity to vertical frame.
Eigen::Vector3d verticalFrameVelocity =
reference_frames::getRotatingPlanetocentricToLocalVerticalFrameTransformationQuaternion(
sphericalOrbitalState( longitudeIndex ), sphericalOrbitalState( latitudeIndex ) ) *
bodyFixedCartesianState.segment( 3, 3 );
// Set velocity in spherical orbital state.
sphericalOrbitalState( speedIndex ) = verticalFrameVelocity.norm( );
sphericalOrbitalState( flightPathIndex ) = calculateFlightPathAngle( verticalFrameVelocity );
sphericalOrbitalState( headingAngleIndex ) = calculateHeadingAngle( verticalFrameVelocity );
return sphericalOrbitalState;
}
inline Eigen::Vector3d reflect_from_surface(const Eigen::Vector3d& v,
const Eigen::Vector3d& surface_norm)
{
assert(fabs(surface_norm.norm() - 1.0) < 1e-6);
const double mag = 2.0*v.dot(surface_norm);
return v - surface_norm * mag;
}
//==============================================================================
Eigen::MatrixXd ContactConstraint::getTangentBasisMatrixODE(
const Eigen::Vector3d& _n)
{
// TODO(JS): Use mNumFrictionConeBases
// Check if the number of bases is even number.
// bool isEvenNumBases = mNumFrictionConeBases % 2 ? true : false;
Eigen::MatrixXd T(Eigen::MatrixXd::Zero(3, 2));
// Pick an arbitrary vector to take the cross product of (in this case,
// Z-axis)
Eigen::Vector3d tangent = mFirstFrictionalDirection.cross(_n);
// TODO(JS): Modify following lines once _updateFirstFrictionalDirection() is
// implemented.
// If they're too close, pick another tangent (use X-axis as arbitrary vector)
if (tangent.norm() < DART_CONTACT_CONSTRAINT_EPSILON)
tangent = Eigen::Vector3d::UnitX().cross(_n);
tangent.normalize();
// Rotate the tangent around the normal to compute bases.
// Note: a possible speedup is in place for mNumDir % 2 = 0
// Each basis and its opposite belong in the matrix, so we iterate half as
// many times
T.col(0) = tangent;
T.col(1) = Eigen::Quaterniond(Eigen::AngleAxisd(DART_PI_HALF, _n)) * tangent;
return T;
}
inline bool ICP::error(Eigen::VectorXd xopt_ant , Eigen::VectorXd xopt){
double dr;
double q0;
Eigen::Vector3d dt;
dt(0) = xopt(4) - xopt_ant(4);
dt(1) = xopt(5) - xopt_ant(5);
dt(2) = xopt(6) - xopt_ant(6);
if (xopt_ant(3)>1) {xopt_ant(3) = 1.0;}
if (xopt_ant(3)<-1){xopt_ant(3) =-1.0;}
if (xopt(3)>1) {xopt(3) = 1.0;}
if (xopt(3)<-1) {xopt(3) =-1.0;}
//q0 = xopt * xopt_ant.inverse()
q0 = -(-xopt_ant(0))*xopt(0)
-(-xopt_ant(1))*xopt(1)
-(-xopt_ant(2))*xopt(2)
+xopt_ant(3) *xopt(3);
dr = 2 * acos(q0);
//std::cout << fabs(dr) << " " << dt.norm() << " " << THRESH_dr << " " << THRESH_dt <<" "<< (fabs(dr) <= THRESH_dr) << " "<< (dt.norm() <= THRESH_dt) << "\n";
if ((fabs(dr) <= THRESH_dr) && ( dt.norm() <= THRESH_dt)) return true;
return false;
}
bool check_result(
const double initial_energy,
const double initial_angle,
const size_t step_divisor,
const double nrj_error,
const double mom_error,
const Eigen::Vector3d& position
) {
if (nrj_error > 1e-12) {
return false;
}
if (position.norm() > 7 * earth_radius) {
if (initial_angle == 90) {
if (step_divisor >= 512) {
return false;
}
} else {
if (step_divisor >= 64) {
return false;
}
}
}
if (initial_energy == 1e4) {
if (initial_angle == 90) {
if (step_divisor >= 128 and mom_error > 2e-2) {
return false;
}
return true;
} else {
if (step_divisor >= 128 and mom_error > 4e-2) {
return false;
}
return true;
}
} else {
if (initial_angle == 90) {
if (step_divisor >= 1024 and mom_error > 0.6) {
return false;
}
return true;
} else {
if (step_divisor >= 512 and mom_error > 2.1) {
return false;
}
return true;
}
}
return true;
}
InterpolatedPtr Herb::planToEndEffectorOffset(MetaSkeletonStateSpacePtr _space,
ConstJacobianNodePtr _endEffector,
const Eigen::Vector3d &_direction,
double _distance,
double _timelimit) const {
auto startState = _space->getScopedStateFromMetaSkeleton();
Eigen::VectorXd positions(7);
_space->convertStateToPositions(startState, positions);
std::random_device randomDevice;
auto seedEngine = RNGWrapper<std::default_random_engine>(randomDevice());
auto engines = splitEngine(seedEngine, 2);
// Generate the constraint TSR
// Frame w set such that the z-axis is pointing along the direction to move
double epsilon = 0.01;
TSRPtr constraint = std::make_shared<TSR>();
constraint->mT0_w = dart::math::computeTransform(_direction / _direction.norm(),
_endEffector->getTransform().translation(),
dart::math::AxisType::AXIS_Z);
constraint->mTw_e = constraint->mT0_w.inverse() * _endEffector->getTransform();
constraint->mBw << -epsilon, epsilon,
-epsilon, epsilon, std::min(0., _distance), std::max(0., _distance),
-epsilon, epsilon, -epsilon, epsilon, -epsilon, epsilon;
std::cout << constraint->mT0_w.matrix() << std::endl;
TSRPtr goalRegion = std::make_shared<TSR>();
auto offset = Eigen::Isometry3d::Identity();
offset(2,3) = _distance;
goalRegion->mT0_w = constraint->mT0_w * offset;
goalRegion->mTw_e = constraint->mTw_e;
goalRegion->mBw << -epsilon, epsilon, -epsilon, epsilon, -epsilon, epsilon,
-epsilon, epsilon, -epsilon, epsilon, -epsilon, epsilon;
auto untimedTrajectory = planCRRTConnect(
startState,
std::make_shared<FrameTestable>(_space, _endEffector, goalRegion),
std::make_shared<InverseKinematicsSampleable>(
_space,
goalRegion,
createSampleableBounds(_space, std::move(engines[0])),
mRightIk,
10),
std::make_shared<NewtonsMethodProjectable>(
std::make_shared<FrameDifferentiable>(
_space, _endEffector, constraint),
std::vector<double>(6, 1e-4)),
_space,
std::make_shared<GeodesicInterpolator>(_space),
createDistanceMetric(_space),
createSampleableBounds(_space, std::move(engines[1])),
getSelfCollisionConstraint(_space),
createTestableBounds(_space),
createProjectableBounds(_space),
_timelimit, std::numeric_limits<double>::infinity(),
mCollisionResolution, mCollisionResolution*0.5, mCollisionResolution);
return untimedTrajectory;
}
// - 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;
}
void ParticleManagerTinkerToy::addConstraint( double x, double y, bool fBeadConstraint )
{
FixedDistanceConstraint* pConstraint = NULL;
if ( !fBeadConstraint )
{
int indexClosestParticle = findClosestParticleIndex( x, y, true );
Eigen::Vector3d distVector = m_particles[indexClosestParticle]->mPosition - m_particles[m_particles.size() - 1]->mPosition;
double distance = distVector.norm();
pConstraint = new FixedDistanceConstraint( m_particles[indexClosestParticle], m_particles[m_particles.size() -1 ], indexClosestParticle, m_particles.size() - 1, distance );
m_pConstraintManager->addConstraint( pConstraint );
}
else
{
Eigen::Vector3d positionCurrent( x, y, 0 );
if ( abs(positionCurrent.norm() - 0.2) < 0.02 )
{
double distance = 0.2;
pConstraint = new FixedDistanceConstraint( m_particles[m_particles.size() -1 ], NULL, m_particles.size() - 1, -1, distance );
m_particles[m_particles.size() - 1]->mColor[0] = 0.0;
m_particles[m_particles.size() - 1]->mColor[2] = 1.0;
m_particles[m_particles.size() - 1]->mPosition.normalize();
m_particles[m_particles.size() - 1]->mPosition *= 0.2;
m_pConstraintManager->addConstraint( pConstraint );
}
}
}
void Model::calForcesHelper(int i, int j, Eigen::Vector3d &F) {
double dist;
Eigen::Vector3d r;
dist = 0.0;
F.fill(0);
r = particles[j]->r - particles[i]->r;
dist = r.norm();
if (dist < 2.0) {
std::cerr << "overlap " << i << "\t" << j << "\t"<< this->timeCounter << "dist: " << dist << "\t" << this->timeCounter <<std::endl;
#ifdef OPENMP
std::cerr << "report from thread: " << omp_get_thread_num() << std::endl;
std::cerr << "number of threads: " << omp_get_num_threads() << std::endl;
#endif
dist = 2.06;
}
if (dist < cutoff) {
// the unit of force is kg m s^-2
// kappa here is kappa*a a non-dimensional number
double Fpp = -4.0/3.0*
Os_pressure*M_PI*(-3.0/4.0*pow(combinedSize,2.0)+3.0*dist*dist/16.0*radius_nm*radius_nm);
Fpp = -Bpp * Kappa * exp(-Kappa*(dist-2.0));
// Fpp += -9e-13 * exp(-kappa* (dist - 2.0));
F = Fpp*r/dist;
}
}
/* ------------------------------------------------------------------------------------------ */
void run () {
// Create the interface to the Kinect
char buf [256];
sprintf(buf, "#%d", id);
pcl::Grabber* interface = new pcl::OpenNIGrabber(buf);
boost::function<void (const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr&)> f =
boost::bind (&SimpleOpenNIViewer::cloud_cb_, this, _1);
// Start the interface
interface->registerCallback (f);
interface->start ();
// Set the callback function to get the points
cv::Vec3s coords (0, 0, -1);
cv::namedWindow("image", CV_WINDOW_AUTOSIZE);
cv::setMouseCallback("image", interact, (void*) (&coords));
bool update = true;
// Wait
int counter = 0;
int lastMouseInput = -1;
Eigen::Vector3d sumCenter (0,0,0);
while (true) { //(!viewer.wasStopped()) {
// Create the image from cloud data
pthread_mutex_lock(&lock);
if(update) makeImage();
pthread_mutex_unlock(&lock);
// Get the user input if it exists and grow a region around it
pthread_mutex_lock(&lock2);
if((coords[2] > 0)) {
growRegion(coords[0], coords[1]);
if(coords[2] > lastMouseInput) {
counter = 0;
sumCenter = Eigen::Vector3d(0,0,0);
cout << "----------- reset" << endl;
lastMouseInput = coords[2];
}
// coords[2] = -1;
if((!(center(0) != center(0))) && center.norm() > 1e-2) {
counter++;
sumCenter += center;
}
if(counter >= 100) {
counter = 0;
cout << "mean center: " << (sumCenter/100).transpose() << endl;
sumCenter = Eigen::Vector3d(0,0,0);
}
// update = false;
}
pthread_mutex_unlock(&lock2);
usleep(1e4);
}
// Stop the interface
interface->stop ();
}
void
LaserSensorModel::setObstacle(const Eigen::Vector3d& pObstacle)
{
m_r_p = pObstacle.norm();
m_pObstacle = pObstacle;
m_validRange = m_pObstacle.norm() + 2.0 * m_sigma;
}
Eigen::Quaterniond ExponentialMapToQuaternion(const Eigen::Vector3d& exponential_rotation)
{
double angle = 0.5 * exponential_rotation.norm();
Eigen::Quaterniond quaternion;
quaternion.w() = std::cos(angle);
quaternion.vec() = 0.5 * boost::math::sinc_pi(angle) * exponential_rotation;
return quaternion;
}
inline void
RangeImageSpherical::getImagePoint (const Eigen::Vector3d& point, double& image_x, double& image_y, double& range) const
{
Eigen::Vector3d transformedPoint = to_range_image_system_ * point;
range = transformedPoint.norm ();
double angle_x = atan2LookUp (transformedPoint[0], transformedPoint[2]),
angle_y = asinLookUp (transformedPoint[1]/range);
getImagePointFromAngles (angle_x, angle_y, image_x, image_y);
}
Eigen::Vector3d get_earth_dipole(const Eigen::Vector3d& position)
{
const double distance = position.norm();
const Eigen::Vector3d
direction = position / distance,
projected_dip_mom = dipole_moment.dot(direction) * direction;
return 1e-7 * (3 * projected_dip_mom - dipole_moment) / std::pow(distance, 3);
}
