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;
}
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
}
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;
}
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;
}
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();
}
/* ********************************************************************************************** */
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;
}
}
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";
}
}
/// 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;
}
}
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;
}
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();
}
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();
}
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;
}
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();
}
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;
}
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);
}
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);
}
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;
}
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;
}
}
/// 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);
}
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);
}
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();
}
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;
}
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);
}
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();
}
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;
}
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;
}
