Esempio n. 1
0
int Gelatin::setNormals(int curNorIndex, int index, int adj[4]) {
    Vector3d x = particles[index]->x;
    Vector3d nor(0.0, 0.0, 0.0);

    for (int k = 0; k < 3; k++) {
        if (adj[k] != -1 && adj[k+1] != -1) {
            Vector3d first = (particles[adj[k]]->x - x).normalized();
            Vector3d second = (particles[adj[k+1]]->x - x).normalized();

            Vector3d res = first.cross(second).normalized();
            nor += res;
        }
    }

    if (adj[3] != -1 && adj[0] != -1) {
        Vector3d first = (particles[adj[3]]->x - x).normalized();
        Vector3d second = (particles[adj[0]]->x - x).normalized();

        Vector3d res = first.cross(second).normalized();
        nor += res;
    }

    nor = nor.normalized();

    norBuf[curNorIndex++] = nor(0);
    norBuf[curNorIndex++] = nor(1);
    norBuf[curNorIndex++] = nor(2);

    return curNorIndex;
}
Esempio n. 2
0
Vector3d Track::getUp(double t) const
{	
	//Vector3d vup = getNonNormalizedNormalVector(t);
	//assert (vup.x==vup.x && vup.y==vup.y && vup.z==vup.z); // Ensure is not NAN
	//if (vup.length() < VECTORLENGHTMINIMUM) return Vector3d(0,0,0);
	//return vup;

	 // Find out in which interval we are on the spline
    int p = (int)(t / delta_t);
	double lt = (t - delta_t*(double)p) / delta_t;
	#define BOUNDS2(pp) { if (pp < 0) pp = 0; else if (pp >= (int)rotations.size()-2) pp = rotations.size() - 2; }
	BOUNDS2(p);
	//double angle0 = rotations[p];
	//double angle1 = rotations[p+1];
	//double angleinterpolated = (angle1-angle0)*lt;

	// TODO: if y axis and tangent is almost parallel, we get gimbal lock. FIX!
	Vector3d yaxis(0,1,0);
	Vector3d tangent = getTangentVector(t);
	
	// Find the unit right/binormal vector in the right-hand system (tangent, (0,1,0), right).
	// Note that this will lead to problems if the tangent vector is very close to +- (0,1,0)
	Vector3d right = tangent.cross(yaxis).normalizedCopy();
	Vector3d up = -tangent.cross(right).normalizedCopy();
	
	// Interpolate the angle linearly between this and next point
	double angleInterpolated = rotations[p] + (rotations[p+1]-rotations[p]) * lt;
	up = cos(angleInterpolated)*up + sin(angleInterpolated)*right;
	return up.normalizedCopy();

}
Vector3d Utils::getHorizonLine(QList<Line*> paralellLines)
{
    Line *firstLine = paralellLines.at(0);
    Line *secondLine = paralellLines.at(1);

    Vector3d a, b;
    a << firstLine->getA()->x(), firstLine->getA()->y(), 1;
    b << firstLine->getB()->x(), firstLine->getB()->y(), 1;
    Vector3d firstLineInHomogeneousCoordinates = a.cross(b);

    a << secondLine->getA()->x(), secondLine->getA()->y(), 1;
    b << secondLine->getB()->x(), secondLine->getB()->y(), 1;
    Vector3d secondLineInHomogeneousCoordinates = a.cross(b);

    Vector3d firstPointHorizonLineInHomogeneousCoordinates = firstLineInHomogeneousCoordinates.cross(secondLineInHomogeneousCoordinates);


    firstLine = paralellLines.at(2);
    secondLine = paralellLines.at(3);

    a << firstLine->getA()->x(), firstLine->getA()->y(), 1;
    b << firstLine->getB()->x(), firstLine->getB()->y(), 1;
    firstLineInHomogeneousCoordinates = a.cross(b);

    a << secondLine->getA()->x(), secondLine->getA()->y(), 1;
    b << secondLine->getB()->x(), secondLine->getB()->y(), 1;
    secondLineInHomogeneousCoordinates = a.cross(b);

    Vector3d secondPointHorizonLineInHomogeneousCoordinates = firstLineInHomogeneousCoordinates.cross(secondLineInHomogeneousCoordinates);

    return firstPointHorizonLineInHomogeneousCoordinates.cross(secondPointHorizonLineInHomogeneousCoordinates);

}
void
basic_ins_qkf::obs_vector(const Vector3d& ref,
		const Vector3d& obs,
		double error)
{
#ifdef TIME_OPS
	timer clock;
	clock.start();
#endif
#define DEBUG_VECTOR_OBS 0

	Vector3d obs_ref = avg_state.orientation.conjugate()*obs;
	Vector3d v_residual = log<double>(Quaterniond().setFromTwoVectors(ref, obs_ref));

	Matrix<double, 3, 2> h_trans;
	h_trans.col(0) = ref.cross(
		(abs(ref.dot(obs_ref)) < 0.9994) ? obs_ref :
			(abs(ref.dot(Vector3d::UnitX())) < 0.707)
				? Vector3d::UnitX() : Vector3d::UnitY()).normalized();
	h_trans.col(1) = -ref.cross(h_trans.col(0));
	assert(!hasNaN(h_trans));
	assert(h_trans.isUnitary());

#ifdef RANK_ONE_UPDATES
	// Running a rank-one update here is a strict win.
	Matrix<double, 12, 1> update = Matrix<double, 12, 1>::Zero();
	for (int i = 0; i < 2; ++i) {
		double obs_error = error;
		double obs_cov = (h_trans.col(i).transpose() * cov.block<3, 3>(3, 3) * h_trans.col(i))[0];
		Matrix<double, 12, 1> gain = cov.block<12, 3>(0, 3) * h_trans.col(i) / (obs_error + obs_cov);
		update += gain * h_trans.col(i).transpose() * v_residual;
		// TODO: Get Eigen to treat cov as self-adjoint
		cov -= gain * h_trans.col(i).transpose() * cov.block<3, 12>(3, 0);
	}
#else
	// block-wise form.  This is much less efficient.
	Vector2d innovation = h_trans.transpose() * v_residual;
	Matrix<double, 12, 2> kalman_gain = cov.block<12, 3>(0, 3) * h_trans
			* (h_trans.transpose() * cov.block<3, 3>(3, 3) * h_trans
				+ (Vector2d() << error, error).finished().asDiagonal()).inverse();
	// TODO: Get Eigen to treat cov as self-adjoint
	cov -= kalman_gain * h_trans.transpose() * cov.block<3, 12>(3, 0);
	Matrix<double, 12, 1> update = (kalman_gain * innovation);
#endif


#if DEBUG_VECTOR_OBS
	// std::cout << "projected update: " << (obs_projection * update.segment<3>(3)).transpose() << "\n";
	std::cout << "deprojected update: " << update.segment<3>(3).transpose() << "\n";
#endif
	avg_state.apply_kalman_vec_update(update);

	assert(invariants_met());
#ifdef TIME_OPS
	double time = clock.stop() * 1e6;
	std::cout << "observe_vector(): " << time << "\n";
#endif

}
Esempio n. 5
0
File: gc.cpp Progetto: rgkoo/slslam
Vector4d gc_av_to_asd(Vector6d av) {
  Vector4d asd;

  Vector3d a = av.head(3);
  Vector3d x = av.tail(3);  // v
  Vector3d y = a.cross(x);  // n

//  Vector2d w(y.norm(), x.norm());
//  w /= w.norm();
//  asd(3) = asin(w(1));
  
  double depth = x.norm() / y.norm();
  // double sig_d = log( (2.0*depth + 1.0) / (1.0 - 2.0*depth));

//  double quotient = depth / 0.5;
//  int integer_quotient = (int)quotient;
//  double floating_quotient = quotient - (double)integer_quotient;
//  depth = depth * floating_quotient;
//  double sig_d = log(2.0*depth + 1.0) - log(1.0 - 2.0*depth);

// double sig_d = atan(1.0 / (1.0*depth) );
// double sig_d = atan(depth);
// double sig_d = atan2(1.0, depth);
// double sig_d = atan(depth);

// double sig_d = atan2(1.0, depth) / 4.0;
  double sig_d = 1.0 / exp(-depth);
  asd(3) = sig_d;
  // cout << "sig_d = " << sig_d << endl;

  // asd(3) = depth;

  // double sig_d = tan(1.0/depth);
  // double sig_d = tan(2.0/depth);
  // double sig_d = tan(2.0*depth);
  // double sig_d = (1.0 - exp(-1.0/depth)) / (2.0*(1.0 + exp(-1.0/depth)));
  // double sig_d = (1.0 - exp(-depth)) / (2.0*(1.0 + exp(-depth)));
  // double sig_d = 1.0 / (1.0 + exp(-1.0/depth));
  // double sig_d = 1.0 / (1.0 + exp(-depth));

  x /= x.norm();
  y /= y.norm();
  Vector3d z = x.cross(y);

  Matrix3d R;
  R.col(0) = x;
  R.col(1) = y;
  R.col(2) = z;

  Vector3d aa = gc_Rodriguez(R);

  asd(0) = aa(0);
  asd(1) = aa(1);
  asd(2) = aa(2);

  return asd;
}
Esempio n. 6
0
File: gc.cpp Progetto: rgkoo/slslam
Vector4d gc_av_to_orth(Vector6d av) {
  Vector4d orth;

  Vector3d a = av.head(3);
  Vector3d v = av.tail(3);  // v
  Vector3d n = a.cross(v);  // n

  Vector3d x = n / n.norm();
  Vector3d y = v / v.norm();
  Vector3d z = x.cross(y);

  orth[0] = atan2( y(2), z(2) );
  orth[1] = asin( - x(2) );
  orth[2] = atan2( x(1), x(0) );

  Vector2d w( n.norm(), v.norm() );
  w = w / w.norm();

  orth[3] = asin( w(1) );

//   MatrixXd A(3,2), Q, R;
// 
//   A.col(0) = n;
//   A.col(1) = v;
// 
//   Eigen::FullPivHouseholderQR<MatrixXd> qr(A);
//   // Q = qr.householderQ();
//   Q = qr.matrixQ();
//   R = qr.matrixQR().triangularView<Upper>();
//   // std::cout << Q << "\n\n" << R << "\n\n" << Q * R - A << "\n";
// 
// //  double sigma1 = R(0,0);
// //  double sigma2 = R(1,1);
// 
// //  cout << "\ntheta from sigma1 = " << acos(sigma1) << endl;
// //  cout << "theta from sigma2 = " << asin(sigma2) << endl;
// 
// // cout << "\nsigma1 = " << sigma1<< endl;
// // cout << "sigma2 = " << sigma2<< endl;
// 
// // sigma2 /= sqrt(sigma1*sigma1 + sigma2*sigma2);
// 
//   Vector3d x = Q.col(0);
//   Vector3d y = Q.col(1);
//   Vector3d z = Q.col(2);
// 
//   orth[0] = atan2( y(2), z(2) );
//   orth[1] = asin( - x(2) );
//   orth[2] = atan2( x(1), x(0) );
//   // orth[3] = asin(sigma2);
// 
//   Vector2d w( n.norm(), v.norm() );
//   w = w / w.norm();
//   orth[3] = asin( w(1) );

  return orth;
}
Esempio n. 7
0
 UpErrorCalculator(const Vector3d& dir_local, const Vector3d& goal_dir_world, RobotAndDOFPtr manip, KinBody::LinkPtr link) :
   dir_local_(dir_local),
   goal_dir_world_(goal_dir_world),
   manip_(manip),
   link_(link)
 {
   Vector3d perp0 = goal_dir_world_.cross(Vector3d::Random()).normalized();
   Vector3d perp1 = goal_dir_world_.cross(perp0);
   perp_basis_.resize(2,3);
   perp_basis_.row(0) = perp0.transpose();
   perp_basis_.row(1) = perp1.transpose();
 }
Esempio n. 8
0
/* ********************************************************************************************** */
void anglesFromRotationMatrix(double &theta1, double &theta2, double &theta3, const Vector3d &n1, 
		const Vector3d &n2, const Vector3d &n3, const Matrix3d &A) {
	
	double lambda = std::atan2(n3.cross(n2).dot(n1), n3.dot(n1));
	double temp = n1.transpose() * A * n3;
	theta2 = -lambda - acos(temp);
	theta3 = -std::atan2(n1.transpose() * A * n2, -n1.transpose() * A * n3.cross(n2));
	theta1 = -std::atan2(n2.transpose() * A * n3, -n2.cross(n1).transpose() * A * n3);

	if(theta2 < -M_PI) {
		theta2 += 2.0 * M_PI;
	}
}
Esempio n. 9
0
  void POVPainter::drawMultiCylinder (const Vector3d &end1, const Vector3d &end2,
                           double radius, int order, double)
  {
    // Just render single bonds with the standard drawCylinder function
    if (order == 1)
    {
      drawCylinder(end1, end2, radius);
      return;
    }

    // Find the bond axis
    Vector3d axis = end2 - end1;
    double axisNorm = axis.norm();
    if( axisNorm < 1.0e-5 ) return;
    Vector3d axisNormalized = axis / axisNorm;

    // Use the plane normal vector for the molecule to draw multicylinders along
    Vector3d ortho1 = axisNormalized.cross(d->planeNormalVector);
    double ortho1Norm = ortho1.norm();
    if( ortho1Norm > 0.001 ) ortho1 /= ortho1Norm;
    else ortho1 = axisNormalized.unitOrthogonal();
    // This number seems to work well for drawing the multiCylinder inside
    ortho1 *= radius*1.5;
    Vector3d ortho2 = axisNormalized.cross(ortho1);
    // Use an angle offset of zero for double bonds, 90 for triple and 22.5 for higher order
    double angleOffset = 0.0;
    if( order >= 3 )
    {
      if( order == 3 ) angleOffset = 90.0;
      else angleOffset = 22.5;
    }
    // Actually draw the cylinders
    for( int i = 0; i < order; i++)
    {
      double alpha = angleOffset / 180.0 * M_PI + 2.0 * M_PI * i / order;
      Vector3d displacement = cos(alpha) * ortho1 + sin(alpha) * ortho2;
      Vector3d displacedEnd1 = end1 + displacement;
      Vector3d displacedEnd2 = end2 + displacement;
      // Write out a POVRay cylinder for rendering
      *(d->output) << "cylinder {\n"
        << "\t<" << displacedEnd1.x() << ", "
                 << displacedEnd1.y() << ", "
                 << displacedEnd1.z() << ">, "
        << "\t<" << displacedEnd2.x() << ", "
                 << displacedEnd2.y() << ", "
                 << displacedEnd2.z() << ">, " << radius
        << "\n\tpigment { rgbt <" << d->color.red() << ", " << d->color.green() << ", "
        << d->color.blue() << ", " << 1.0 - d->color.alpha() << "> }\n}\n";

    }
  }
Esempio n. 10
0
/// Returns the transformation between the proximal and distal shoulder frames (3.2.3)
void getT1 (const Transform<double, 3, Affine>& relGoal, 
		const Transform<double, 3, Affine>& Ty, const Vector3d& elbow, 
		Transform<double, 3, Affine>& T1) {

	const bool debug = 0;

	// First, compute the transformation between distal shoulder and 
	// proximal wrist frames, and then, its translation is the position of
	// the wrist in the distal shoulder (world) frame
	Transform<double, 3, Affine> temp (A * Ty.matrix() * B);
	Vector3d w = temp.translation();
	
	// Define the axes for the distal shoulder frame assuming T_1, the 
	// transformation between proximal and distal shoulder frames is 
	// identity (the rest configuration). 
	Vector3d x = A.topRightCorner<3,1>().normalized();
	Vector3d y = (w - w.dot(x) * x);
	
	y = y.normalized();
	Vector3d z = x.cross(y);

	// First, compute the wrist location wrt to the distal shoulder
	// in the goal configuration
	Vector3d wg = relGoal.translation();

	// Then, define the axes for the distal shoulder frame in the
	// goal configuration (T_1 is not I anymore). 
	Vector3d xg = elbow.normalized();
	Vector3d yg = (wg - wg.dot(xg) * xg).normalized();
	Vector3d zg = xg.cross(yg);

	// Create the two rotation matrices from the frames above and
	// compute T1.
	Matrix3d R1r, R1g;
	R1r.col(0) = x; R1r.col(1) = y; R1r.col(2) = z;
	R1g.col(0) = xg; R1g.col(1) = yg; R1g.col(2) = zg;
	T1 = Transform<double, 3, Affine>(R1g * R1r.transpose());

	if(0 && debug) {
		cout << "Ty:\n" << Ty.matrix() << endl;
		cout << "A * Ty * B: \n" << temp.matrix() << endl << endl;
		cout << "w: " << w.transpose() << endl;
		cout << "y: " << y.transpose() << "\n" << endl;
		cout << "x: " << x.transpose() << endl;
		cout << "y: " << y.transpose() << endl;
		cout << "z: " << z.transpose() << endl;
		cout << "R1r: " << R1r << endl;
		cout << "R1g: " << R1g << endl;
	}
}
Esempio n. 11
0
MatrixXd pointPlaneDistJac(Vector3d y11, Vector3d y21, Vector3d y22, Vector3d y23)
{
  Vector3d a(y22 - y21);
  Vector3d b(y23 - y21);
  Vector3d c(y11 - y21);
  Vector3d f(a.cross(b));
  Matrix3d da_dy21(-Matrix3d::Identity());
  Matrix3d da_dy22(Matrix3d::Identity());
  if(f.dot(c)<0)
  {
    a = y21-y22;
    f = a.cross(b);
    da_dy21 = Matrix3d::Identity();
    da_dy22 = -Matrix3d::Identity();
  }
  const Matrix3d db_dy21(-Matrix3d::Identity());
  const Matrix3d db_dy23(Matrix3d::Identity());
  const Matrix3d dc_dy21(-Matrix3d::Identity());
  const Matrix3d dc_dy11(Matrix3d::Identity());
  const double norm_f = f.norm();

  const Vector3d dd_df((Matrix3d::Identity()/norm_f - f*f.transpose()/pow(norm_f,3))*c);

  const RowVector3d dd_da(-dd_df.cross(b));
  const RowVector3d dd_db(dd_df.cross(a));
  const RowVector3d dd_dc(f/norm_f);

  MatrixXd Jd(1,12);
  Jd << dd_dc*dc_dy11, dd_da*da_dy21 + dd_db*db_dy21 + dd_dc*dc_dy21, 
        dd_da*da_dy22, dd_db*db_dy23;

  //DEBUG
  //cout << "simpleClosestPointFunctions::pointPlaneDistJac: Values:" << endl;
  //cout << "y11 = [" << y11.transpose() << "]';" << endl;
  //cout << "y21 = [" << y21.transpose() << "]';" << endl;
  //cout << "y22 = [" << y22.transpose() << "]';" << endl;
  //cout << "y23 = [" << y23.transpose() << "]';" << endl;
  //cout << "a = [" << a.transpose() << "]';" << endl;
  //cout << "b = [" << b.transpose() << "]';" << endl;
  //cout << "c = [" << c.transpose() << "]';" << endl;
  //cout << "f = [" << f.transpose() << "]';" << endl;
  //cout << "dd_df = [" << dd_df.transpose() << "]';" << endl;
  //cout << "dd_da = [" << dd_da << "]';" << endl;
  //cout << "dd_db = [" << dd_da << "]';" << endl;
  //cout << "dd_dc = [" << dd_da << "]';" << endl;
  //cout << "Jd = [" << Jd << "]';" << endl;
  //cout << "simpleClosestPointFunctions::lineLineDistJac: End Values:" << endl;
  //END_DEBUG
  return Jd;
}
Esempio n. 12
0
GLCamera::GLCamera(const Vector3d &pos, const Vector3d &target, const Vector3d &up)
{
    m_pos = pos;
    m_target = target;
    m_up = up;
    n = Vector3d( pos.x-target.x, pos.y-target.y, pos.z-target.z);
    u = Vector3d(up.cross(n).x, up.cross(n).y, up.cross(n).z);
    v = Vector3d(n.cross(u).x,n.cross(u).y,n.cross(u).z);

    n.normalize();
    u.normalize();
    v.normalize();

    setModelViewMatrix();
}
Esempio n. 13
0
void MyTorus::torusNormal(double phi, double theta, Vector3d &normal){
    Vector3d dT_dTheta (-(R+r*cos(phi))*sin(theta),(R+r*cos(phi))*cos(theta),0);
    Vector3d dT_dPhi (-r*sin(phi)*cos(theta),-r*sin(phi)*sin(theta),r*cos(phi));
    normal.cross(dT_dTheta,dT_dPhi);
    normal.normalize();

}
Esempio n. 14
0
void Camera::computeMatrix(){
	
	Vector3d z = e - d;
	z.normalize();
	Vector3d x = up.cross(z);
	x.normalize();
	Vector3d y = z.cross(x);
	y.normalize();
	Matrix4d r;
	r.identity();
	r.set(0, 0, x[0]);
	r.set(1, 0, x[1]);
	r.set(2, 0, x[2]);

	r.set(0, 1, y[0]);
	r.set(1, 1, y[1]);
	r.set(2, 1, y[2]);

	r.set(0, 2, z[0]);
	r.set(1, 2, z[1]);
	r.set(2, 2, z[2]);
	Matrix4d t;
	t.identity();
	t.makeTranslate(-e[0], -e[1], -e[2]);
	m = r * t;
	
}
Esempio n. 15
0
void Shape::OptimizeRotation()
{
  // CenterAroundXY();
  vector<Vector3d> normals = getMostUsedNormals();
  // cycle through most-used normals?

  Vector3d N;
  Vector3d Z(0,0,-1);
  double angle=0;
  int count = (int)normals.size();
  for (int n=0; n < count; n++) {
    //cerr << n << normals[n] << endl;
    N = normals[n];
    angle = acos(N.dot(Z));
    if (angle>0) {
      Vector3d axis = N.cross(Z);
      if (axis.squared_length()>0.1) {
	Rotate(axis,angle);
	break;
      }
    }
  }
  CalcBBox();
  PlaceOnPlatform();
}
Esempio n. 16
0
unsigned RotateBox(CONFIG& config, int ig)
{
    Vector3d a;
    Vector3d rotv;
    Vector3d rtmp;
    double   angle;
    time_t   time1, time2;
    time(&time1);


    a = config.grain[ig].angle;

    rotv << cos(a(0))*sin(a(1)), sin(a(0))*sin(a(1)), cos(a(1));
    angle = a(2);

    for (int i = 0; i < config.atoms_grain; i++)
    {
        rtmp = config.atom_grain[i].r;
        config.atom_grain[i].r = rtmp * cos(angle) + rotv.cross(rtmp) * sin(angle) + (1 - cos(angle)) * rotv *(rotv.dot(rtmp)) + config.grain[ig].r; //  Rodrigue's rotation formula
    }

    time(&time2);
    if (config.time) cout << "Rotated in " << time2-time1 << " s.";

    return 0;
}
Esempio n. 17
0
std::pair<Eigen::Vector3d, double> resolveCenterOfPressure(const Eigen::MatrixBase<DerivedTorque> & torque, const Eigen::MatrixBase<DerivedForce> & force, const Eigen::MatrixBase<DerivedNormal> & normal, const Eigen::MatrixBase<DerivedPoint> & point_on_contact_plane)
{
    // TODO: implement multi-column version
    using namespace Eigen;

    if (abs(normal.squaredNorm() - 1.0) > 1e-12) {
        throw std::runtime_error("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);
}
Esempio n. 18
0
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);
}
Esempio n. 19
0
vector<Vector3d> getAxes(const Vector3d &r1,const Vector3d &r2,const Vector3d &r3)
{
  vector<Vector3d> l0;
  Vector3d i = (r3 - r2).normalized(), r21 = r1 - r2;
  Vector3d e = (r21 - (r21.dot(i) * i)).normalized();
  l0.push_back(i); l0.push_back(e); l0.push_back(i.cross(e));
  return l0;
}
Esempio n. 20
0
void general_ewald_sum(UnitCell &uc, const Vector3d &a1, const Vector3d &a2, const Vector3d &a3, const Vector3i &Rcutoff)
{
    int N = uc.size();
    const double sigma = M_PI;

    // renormalize electron occupation
    double unrenorm_Ne = 0, Ne = 0;
    for (UnitCell::iterator it = uc.begin(); it < uc.end(); ++it) {
        unrenorm_Ne += it->ne;
        Ne += it->C;
    }
    for (UnitCell::iterator it = uc.begin(); it < uc.end(); ++it) 
        it->ne *= Ne/unrenorm_Ne;

    // get reciprocal vectors
    Vector3d b1, b2, b3;
    double uc_vol = a1.dot(a2.cross(a3));
    b1 = 2*M_PI*a2.cross(a3)/uc_vol;
    b2 = 2*M_PI*a3.cross(a1)/uc_vol;
    b3 = 2*M_PI*a1.cross(a2)/uc_vol;
    
    // Ewald sum
    double Vs, Vl;
    Vector3d k;
    for (int id = 0; id < N; ++id) {
        Vs = 0; Vl = 0;
        for (int m = -Rcutoff[0]; m < Rcutoff[0]; ++m)
            for (int n = -Rcutoff[1]; n < Rcutoff[1]; ++n)
                for (int l = -Rcutoff[2]; l < Rcutoff[2]; ++l) {
                    k = n*b1 + m*b2 + l*b3;
                    double ksquare = k.dot(k);
                    for (int i = 0; i < N; ++i) {
                        double dR = (uc[id].r - (uc[i].r + m*a1 + n*a2 + l*a3)).norm();
                        // for long-range terms
                        if ( !(m == 0 && n == 0 && l == 0) )
                            Vl += 4*M_PI/uc_vol*(uc[i].C-uc[i].ne)/ksquare * cos(k.dot(uc[id].r-uc[i].r)) * exp(-pow(sigma,2)*ksquare/2.);
                        
                        // for short-range terms
                        if ( !(m == 0 && n == 0 && l == 0 && i == id) )
                            Vs += (uc[i].C - uc[i].ne) / dR * erfc(dR/sqrt(2)/sigma);
                    }
                }
        uc[id].V = Vs + Vl - (uc[id].C - uc[id].ne)*sqrt(2/M_PI)/sigma;
    }
}
Esempio n. 21
0
  /// transforms system with z' to regular system
  /// will try to align y' with z, but if that fails will align y' with y
  Transformation Transformation::alignZPrime(const Vector3d& zPrime)
  {
    Vector3d xp;
    Vector3d yp;
    Vector3d zp = zPrime;
    if (!zp.normalize()){
      LOG(Error, "Could not normalize zPrime");
    }

    Vector3d xAxis(1,0,0);
    Vector3d yAxis(0,1,0);
    Vector3d zAxis(0,0,1);
    Vector3d negXAxis(-1,0,0);

    // check if face normal is up or down
    if (fabs(zp.dot(zAxis)) < 0.99){
      // not facing up or down, set yPrime along zAxis
      yp = zAxis - (zp.dot(zAxis)*zp);
      if (!yp.normalize()){
        LOG(Error, "Could not normalize axis");
      }
      xp = yp.cross(zp);
    }else{
      // facing up or down, set xPrime along -xAxis
      xp = negXAxis - (zp.dot(negXAxis)*zp);
      if (!xp.normalize()){
        LOG(Error, "Could not normalize axis");
      }
      yp = zp.cross(xp);
    }

    Matrix storage = identity_matrix<double>(4);
    storage(0,0) = xp.x();
    storage(1,0) = xp.y();
    storage(2,0) = xp.z();
    storage(0,1) = yp.x();
    storage(1,1) = yp.y();
    storage(2,1) = yp.z();
    storage(0,2) = zp.x();
    storage(1,2) = zp.y();
    storage(2,2) = zp.z();

    return Transformation(storage);
  }
Esempio n. 22
0
double lineLineDist(const Vector3d y11, const Vector3d y12, const Vector3d y21, const Vector3d y22)
{
  Vector3d a = y12-y11;
  Vector3d b = y22-y21;
  Vector3d c = y21-y11;
  Vector3d f = a.cross(b);
  double g = std::abs(c.dot(f));
  double d = g/(f.norm());
  return d;
}
/** Get the orientation of the frame at the specified time. The frame's orientation
  * is undefined whenever one or more of the following is true:
  *  - the state of either the primary or secondary object is undefined
  *  - the positions of the primary and secondary object are identical
  *  - the secondary is stationary with respect to the primary
  *  - the position and velocity vectors are exactly aligned
  */
Quaterniond
TwoBodyRotatingFrame::orientation(double t) const
{
    StateVector state = m_secondary->state(t) - m_primary->state(t);
    if (state.position().isZero() || state.velocity().isZero())
    {
        return Quaterniond::Identity();
    }

    // Compute the axes of the two body rotating frame,
    // convert this to a matrix, then derive a quaternion
    // from this matrix.

    // x-axis points in direction from the primary to the secondary
    Vector3d xAxis = state.position().normalized();
    Vector3d v = state.velocity().normalized();

    Vector3d zAxis;
    if (m_velocityAlligned) {
        // z-axis normal to both the x-axis and the velocity vector
        zAxis = xAxis.cross(v);
        if (zAxis.isZero())
        {
            return Quaterniond::Identity();
        }
    }
    else {
        // z-axis normal to both the x-axis and the z axis of the primary
        zAxis = xAxis.cross(m_primary->orientation(t) * Vector3d::UnitX());
        if (zAxis.isZero())
        {
            return Quaterniond::Identity();
        }
    }

    zAxis.normalize();
    Vector3d yAxis = zAxis.cross(xAxis);

    Matrix3d m;
    m << xAxis, yAxis, zAxis;

    return Quaterniond(m);
}
Esempio n. 24
0
void cOrbit::FromPositionAndVelocity(cOrbit &out, const cBody &referenceBody, const Vector3d &position, const Vector3d &velocity, double t) 
{
    double mu = referenceBody.GravitationalParameter();
    double r = position.norm();
    double v = velocity.norm();
    Vector3d specificAngularMomentum = position.cross(velocity);
    Vector3d nodeVector;
    if (specificAngularMomentum(0) != 0.0 || specificAngularMomentum(1) != 0.0) 
    {
        nodeVector = Vector3d(-specificAngularMomentum(1), specificAngularMomentum(0), 0).normalized();
    } else {
        nodeVector = Vector3d(1, 0, 0);
    }
    Vector3d eccentricityVector = ((1 / mu) * velocity.cross(specificAngularMomentum)) - position.normalized();
    double semiMajorAxis = 1 / (2 / r - v * v / mu);
    double eccentricity = eccentricityVector.norm();    
    double inclination = std::acos(specificAngularMomentum[2] / specificAngularMomentum.norm());
    double longAscNode, argPeriaps;
    if (eccentricity == 0.0) {
        argPeriaps = 0;
        longAscNode = 0;
    } else {
        longAscNode = std::acos(nodeVector[0]);
        if (nodeVector[1] < 0) {
            longAscNode = TWO_PI - longAscNode;
        }
        argPeriaps = std::acos(nodeVector.dot(eccentricityVector) / eccentricity);
        if (eccentricityVector[2] < 0) {
            argPeriaps = TWO_PI - argPeriaps;
        }
    }
    double trueAnomaly = std::acos(eccentricityVector.dot(position) / (eccentricity * r));
    if (position.dot(velocity) < 0) 
    {
        trueAnomaly = -trueAnomaly;
    }

    out.Reset(&referenceBody, semiMajorAxis, eccentricity, inclination, longAscNode, argPeriaps, 0, 0);

    out.m_meanAnomalyAtEpoch = out.MeanAnomalyAtTrueAnomaly(trueAnomaly);
    out.m_timeOfPeriapsisPassage = t - out.m_meanAnomalyAtEpoch / out.MeanMotion();
}
Esempio n. 25
0
MatrixXd pointLineDistJac(const Vector3d y11, const Vector3d y21, const Vector3d y22)
{
  const Vector3d a(y22 - y21);
  const Vector3d b(y21 - y11);
  const Vector3d c(a.cross(b));
  const double norm_a = a.norm();
  const double norm_a_squared = a.squaredNorm();
  const double norm_c = c.norm();
  const Matrix3d da_dy21(-Matrix3d::Identity());
  const Matrix3d da_dy22(Matrix3d::Identity());
  const Matrix3d db_dy11(-Matrix3d::Identity());
  const Matrix3d db_dy21(Matrix3d::Identity());

  const RowVector3d dd_da(-( c.cross(b)/(norm_a*norm_c) + 
                          norm_c*a/(norm_a*norm_a_squared)));
  const RowVector3d dd_db(c.cross(a)/(norm_a*norm_c));
  MatrixXd Jd(1,9);
  Jd << dd_db*db_dy11, dd_da*da_dy21 + dd_db*db_dy21, dd_da*da_dy22;
  return Jd;
}
Esempio n. 26
0
MatrixXd lineLineDistJac(Vector3d y11, Vector3d y12, Vector3d y21, Vector3d y22)
{
  Vector3d a(y12 - y11);
  Vector3d b(y22 - y21);
  Vector3d c(y21 - y11);
  Vector3d f(a.cross(b));
  Matrix3d da_dy11(-Matrix3d::Identity());
  Matrix3d da_dy12(Matrix3d::Identity());
  Matrix3d db_dy21(-Matrix3d::Identity());
  Matrix3d db_dy22(Matrix3d::Identity());
  if (f.dot(c) < 0) {
    a = y11 - y12;
    b = y22 - y21;
    f = a.cross(b);
    da_dy11 = Matrix3d::Identity();
    da_dy12 = -Matrix3d::Identity();
    db_dy21 = -Matrix3d::Identity();
    db_dy22 = Matrix3d::Identity();
  }
  const double g = c.dot(f);

  const double norm_f = f.norm();
  const Matrix3d dc_dy11(-Matrix3d::Identity());
  const Matrix3d dc_dy21(Matrix3d::Identity());

  const Vector3d dd_df(-f*abs(g)/pow(norm_f,3));
  const double dd_dg(g/(abs(g)*norm_f));
  const Vector3d& dg_df = c;
  const Vector3d full_dd_df(dd_df + dd_dg*dg_df);

  const RowVector3d dd_da = -full_dd_df.cross(b); 
  const RowVector3d dd_db = full_dd_df.cross(a); 
  const RowVector3d dd_dc = dd_dg*f;
  MatrixXd Jd(1,12);

  Jd << dd_da*da_dy11 + dd_dc*dc_dy11, dd_da*da_dy12, 
        dd_db*db_dy21 + dd_dc*dc_dy21, dd_db*db_dy22;

  return Jd;
}
inline void KinematicUtil::get_head_angles_for_distance_intern(KRobot& robot, const Vector2d& pos, bool left_leg, double y_angle, double x_angle) {
    Affine3d base, inv_base;
    //handle support leg
    if(left_leg) {
        inv_base = robot.get_joint_by_id(JointIds::LFootEndpoint).get_chain_matrix_inverse();
        base = robot.get_joint_by_id(JointIds::LFootEndpoint).get_chain_matrix();
    } else {
        inv_base = robot.get_joint_by_id(JointIds::RFootEndpoint).get_chain_matrix_inverse();
        base = robot.get_joint_by_id(JointIds::RFootEndpoint).get_chain_matrix();
    }
    Vector3d head_pos = (inv_base.matrix() * robot.get_joint_by_id(JointIds::Camera).get_endpoint()).head<3>();
    head_pos.y() = 0;
    //difference vector between head and the point to focus
    Vector3d diff = (Vector3d()<<pos, 0).finished() - head_pos;
    Vector3d target = base.linear() * diff.normalized();
    //handle "special" tasks. Those tasks where another point than the middle point should have the given distance.
    if(additional_parameter > 0) {
        Vector3d y_orthogonal, x_orthogonal;
        //Calculate an orthogonal vector to have a rotation axis when rotating the target vector, so that y-angle has the given distance
        y_orthogonal = diff.cross(Vector3d::UnitZ()).normalized();
        L_DEBUG(cout<<"Orthogonale:\n"<<y_orthogonal.transpose()<<endl);
        //Creating the rotation type
        AngleAxisd y_turn( - y_angle, y_orthogonal);
        L_DEBUG(cout<<"Turn Matrix:\n"<<(Matrix3d()<<(Matrix3d::Identity() * y_turn)).finished()<<endl);
        //rotating
        target = y_turn * target;
        if(additional_parameter > 1) {
            //same as for y-axis now for x-axis, y-axis first because of "dependencies"
            x_orthogonal = diff.cross(y_orthogonal).normalized();
            L_DEBUG(cout<<"Orthogonale:\n"<<x_orthogonal.transpose()<<endl);
            AngleAxisd x_turn(x_angle, x_orthogonal);
            L_DEBUG(cout<<"Turn Matrix:\n"<<(Matrix3d()<<(Matrix3d::Identity() * x_turn)).finished()<<endl);
            target = x_turn * target;
        }
    }
    robot.inverse_chain(ChainIds::HeadChain, target, 1e-3, 100, Kin::KJoint::AxisType::XAxis);
    L_DEBUG(cout<<"angles: y_angle:" << y_angle << " x_angle: "<< x_angle<<endl);
    L_DEBUG(cout<< "Target"<<endl<<target.transpose()<<endl);
    L_DEBUG(cout<<"Head"<<endl<<(robot.get_joint_by_id(JointIds::LFootEndpoint).get_chain_matrix_inverse() * robot.get_joint_by_id(JointIds::Camera).get_chain_matrix()).matrix()<<endl);
}
Esempio n. 28
0
void WalkLink::render() const
{
	Vector3d mid = (m_edge.e1 + m_edge.e2) / 2;
	Vector3d alongEdge = (m_edge.e2 - m_edge.e1);
	Vector3d acrossEdge = alongEdge.cross(Vector3d(0,0,1)).normalize();
	acrossEdge *= 0.2;
	Vector3d s = mid + acrossEdge, d = mid - acrossEdge;
	glBegin(GL_LINES);
		glColor3d(0,1,1);
		glVertex3d(s.x, s.y, s.z);
		glVertex3d(d.x, d.y, d.z);
	glEnd();
}
Esempio n. 29
0
Triangle::Triangle(const Vector3d& inVertexA, const Vector3d& inVertexB, const Vector3d& inVertexC) {
    vertexA = inVertexA;
    vertexB = inVertexB;
    vertexC = inVertexC;

    Vector3d ab = vertexA - vertexB;
    Vector3d ac = vertexA - vertexC;
    Vector3d n = ab.cross(ac);

    normalA = n;
    normalB = n;
    normalC = n;
}
Esempio n. 30
0
File: gc.cpp Progetto: rgkoo/slslam
Vector4d gc_av_to_aid(Vector6d av) {

  Vector4d aid;
  Vector3d a = av.head(3);
  Vector3d x = av.tail(3);  // v
  Vector3d y = a.cross(x);  // n
  aid(3) = x.norm() / y.norm();
  x /= x.norm();
  y /= y.norm();
  Vector3d z = x.cross(y);

  Matrix3d R;
  R.col(0) = x;
  R.col(1) = y;
  R.col(2) = z;

  Vector3d aa = gc_Rodriguez(R);

  aid(0) = aa(0);
  aid(1) = aa(1);
  aid(2) = aa(2);

//  Vector4d aid;
//  Vector3d a = av.head(3);
//  Vector3d v = av.tail(3);  // v
//  Vector3d n = a.cross(v);  // n
//
//  aid(3) = v.norm() / n.norm();
//
//  Vector3d x = n / n.norm();
//  Vector3d y = v / v.norm();
//  Vector3d z = x.cross(y);
//
//  aid[0] = atan2( y(2), z(2) );
//  aid[1] = asin( - x(2) );
//  aid[2] = atan2( x(1), x(0) );

  return aid;
}