/*
* 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);
}
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);
}
}
void PlanarJoint::setPlane(const Eigen::Vector3d& _rotAxis,
const Eigen::Vector3d& _tranAxis1)
{
mPlaneType = PT_ARBITRARY;
// Rotational axis
mRotAxis = _rotAxis.normalized();
// Translational axes
assert(_rotAxis == _tranAxis1);
Eigen::Vector3d unitTA1 = _tranAxis1.normalized();
mTranAxis1 = (unitTA1 - unitTA1.dot(mRotAxis) * mRotAxis).normalized();
mTranAxis2 = (mRotAxis.cross(mTranAxis1)).normalized();
}
//==============================================================================
ScrewJointUniqueProperties::ScrewJointUniqueProperties(
const Eigen::Vector3d& _axis, double _pitch)
: mAxis(_axis.normalized()),
mPitch(_pitch)
{
// Do nothing
}
//==============================================================================
PlaneShape::PlaneShape(const Eigen::Vector3d& _normal,
const Eigen::Vector3d& _point)
: Shape(),
mNormal(_normal.normalized()),
mOffset(mNormal.dot(_point))
{
}
/** handleRayPick is called to test whether a visualizer is intersected
* by a pick ray. It should be overridden by any pickable visualizer.
*
* \param pickOrigin origin of the pick ray in local coordinates
* \param pickDirection the normalized pick ray direction, in local coordinates
* \param pixelAngle angle in radians subtended by one pixel of the viewport
*/
bool
Visualizer::handleRayPick(const Eigen::Vector3d& pickOrigin,
const Eigen::Vector3d& pickDirection,
double pixelAngle) const
{
if (!m_geometry.isNull())
{
// For geometry with a fixed apparent size, test to see if the pick ray is
// within apparent size / 2 pixels of the center.
if (m_geometry->hasFixedApparentSize())
{
double cosAngle = pickDirection.dot(-pickOrigin.normalized());
if (cosAngle > 0.0)
{
if (cosAngle >= 1.0 || acos(cosAngle) < m_geometry->apparentSize() / 2.0 * pixelAngle)
{
return true;
}
}
}
}
return false;
}
//==============================================================================
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;
}
mesh_core::Plane::Plane(
const Eigen::Vector3d& normal,
const Eigen::Vector3d& point_on_plane)
: normal_(normal.normalized())
{
d_ = -point_on_plane.dot(normal_);
}
// return closest point on line segment to the given point, and the distance
// betweeen them.
double mesh_core::closestPointOnLine(
const Eigen::Vector3d& line_a,
const Eigen::Vector3d& line_b,
const Eigen::Vector3d& point,
Eigen::Vector3d& closest_point)
{
Eigen::Vector3d ab = line_b - line_a;
Eigen::Vector3d ab_norm = ab.normalized();
Eigen::Vector3d ap = point - line_a;
Eigen::Vector3d closest_point_rel = ab_norm.dot(ap) * ab_norm;
double dp = ab.dot(closest_point_rel);
if (dp < 0.0)
{
closest_point = line_a;
}
else if (dp > ab.squaredNorm())
{
closest_point = line_b;
}
else
{
closest_point = line_a + closest_point_rel;
}
return (closest_point - point).norm();
}
void pointAndLineTransform(const Eigen::Vector3d& lineVector,
const Eigen::Vector3d& linePoint,
Eigen::Matrix4d& transform)
{
//define a plane containing the line point and with normal vector equal to the line vector
pcl::ModelCoefficientsPtr planeCoefficients( new pcl::ModelCoefficients );
planeCoefficients->values.resize(4);
Eigen::Vector3d normal;
normal = lineVector.normalized();
planeCoefficients->values[0] = normal(0,0);
planeCoefficients->values[1] = normal(1,0);
planeCoefficients->values[2] = normal(2,0);
// Provided the plane explicit equation A·X + B·Y + C·Z + D = 0, define D by
// evaluating the equation with the line point:
planeCoefficients->values[3] = -( normal(0,0)*linePoint(0,0) +
normal(1,0)*linePoint(1,0) +
normal(2,0)*linePoint(2,0) );
Eigen::Vector3d origin;
origin = linePoint;
pal::planeTransform(planeCoefficients,
&origin,
transform);
}
void Camera::updatePrincipleRay() {
// XXX Does lens distortion play a role here...?
const Eigen::Vector3d tcol = K_.col(2);
const Eigen::Vector3d dir = Kinv_ * (tcol / tcol[2]);
principleRay_.setSource(C_);
principleRay_.setDirection(Rinv_ * dir.normalized());
}
double constraints() {
Eigen::Vector3d vec = ((*s2).curPos - (*s1).curPos);
double magnitude = vec.norm();
double ext_dist = magnitude - const_dist;
// lighter objects move further
double weight1 = fmax(1.0 - (*s1).mass/((*s1).mass + (*s2).mass), 1e-5);
double weight2 = fmax(1.0 - (*s2).mass/((*s1).mass + (*s2).mass), 1e-5);
if (abs(ext_dist) > 1e-5) {
Eigen::Vector3d s1s2 = vec.normalized();
Eigen::Vector3d s2s1 = -(vec.normalized());
(*s1).curPos = (*s1).curPos + weight1*ext_dist*s1s2;
(*s2).curPos = (*s2).curPos + weight2*ext_dist*s2s1;
(*s1).oldPos = (*s1).oldPos + 0.6*weight1*ext_dist*s1s2;
(*s2).oldPos = (*s2).oldPos + 0.6*weight2*ext_dist*s2s1;
}
return ext_dist;
}
UniversalJoint::UniversalJoint(const Eigen::Vector3d& _axis0,
const Eigen::Vector3d& _axis1,
const std::string& _name)
: Joint(_name)
{
mGenCoords.push_back(&mCoordinate[0]);
mGenCoords.push_back(&mCoordinate[1]);
mS = Eigen::Matrix<double, 6, 2>::Zero();
mdS = Eigen::Matrix<double, 6, 2>::Zero();
mSpringStiffness.resize(2, 0.0);
mDampingCoefficient.resize(2, 0.0);
mRestPosition.resize(2, 0.0);
mAxis[0] = _axis0.normalized();
mAxis[1] = _axis1.normalized();
}
// Set this to a random transformation with bounded rotation and translation.
inline void Transformation::setRandom(double translationMaxMeters,
double rotationMaxRadians) {
// Create a random unit-length axis.
Eigen::Vector3d axis = rotationMaxRadians * Eigen::Vector3d::Random();
// Create a random rotation angle in radians.
Eigen::Vector3d r = translationMaxMeters * Eigen::Vector3d::Random();
r_ = r;
q_ = Eigen::AngleAxisd(axis.norm(), axis.normalized());
updateC();
}
void StateEstimatorKinematic::integrate_angular_velocity(Eigen::Vector3d xyz) {
Eigen::Quaterniond m;
// m = Eigen::AngleAxisd(timestep,xyz);
m = Eigen::AngleAxisd(xyz.norm()*timestep,xyz.normalized());
_q = _q*m;
}
/**
* @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>();
}
//==============================================================================
void TranslationalJoint2DUniqueProperties::setArbitraryPlane(
const Eigen::Vector3d& transAxis1, const Eigen::Vector3d& transAxis2)
{
// Set plane type as arbitrary plane
mPlaneType = PlaneType::ARBITRARY;
// First translational axis
mTransAxes.col(0) = transAxis1.normalized();
// Second translational axis
mTransAxes.col(1) = transAxis2.normalized();
// Orthogonalize translational axes
const double dotProduct = mTransAxes.col(0).dot(mTransAxes.col(1));
assert(std::abs(dotProduct) < 1.0 - 1e-6);
if (std::abs(dotProduct) > 1e-6)
mTransAxes.col(1)
= (mTransAxes.col(1) - dotProduct * mTransAxes.col(0)).normalized();
}
//==============================================================================
void PrismaticJoint::setAxis(const Eigen::Vector3d& _axis)
{
if(_axis == mAspectProperties.mAxis)
return;
mAspectProperties.mAxis = _axis.normalized();
Joint::notifyPositionUpdate();
updateLocalJacobian();
Joint::incrementVersion();
}
UniversalJoint::UniversalJoint(BodyNode* _parent, BodyNode* _child,
const Eigen::Vector3d& _axis0,
const Eigen::Vector3d& _axis1,
const std::string& _name)
: Joint(_parent, _child, _name)
{
mJointType = UNIVERSAL;
mGenCoords.push_back(&mCoordinate[0]);
mGenCoords.push_back(&mCoordinate[1]);
mS = Eigen::Matrix<double,6,2>::Zero();
mdS = Eigen::Matrix<double,6,2>::Zero();
mDampingCoefficient.resize(2, 0);
mAxis[0] = _axis0.normalized();
mAxis[1] = _axis1.normalized();
}
/**
* @function getPlaneTransform
* @brief Assumes plane coefficients are of the form ax+by+cz+d=0, normalized
*/
Eigen::Matrix4d getPlaneTransform ( pcl::ModelCoefficients &coeffs,
double up_direction,
bool flatten_plane ) {
Eigen::Matrix4d tf = Eigen::Matrix4d::Identity();
if( coeffs.values.size() <= 3 ) {
std::cout << "[ERROR] Coefficient size less than 3. I will output nonsense values"<<std::endl;
return tf;
}
double a = coeffs.values[0];
double b = coeffs.values[1];
double c = coeffs.values[2];
double d = coeffs.values[3];
//asume plane coefficients are normalized
Eigen::Vector3d position(-a*d, -b*d, -c*d);
Eigen::Vector3d z(a, b, c);
//if we are flattening the plane, make z just be (0,0,up_direction)
if(flatten_plane) {
z << 0, 0, up_direction;
}
else {
//make sure z points "up" (the plane normal and the table must have a > 90 angle, hence the cosine must be negative)
if ( z.dot( Eigen::Vector3d(0, 0, up_direction) ) > 0) {
z = -1.0 * z;
printf("Changing sign \n");
coeffs.values[0]*= -1; coeffs.values[1]*= -1; coeffs.values[2]*= -1; coeffs.values[3]*= -1;
}
}
//try to align the x axis with the x axis of the original frame
//or the y axis if z and x are too close too each other
Eigen::Vector3d x; x << 1, 0, 0;
if ( fabs(z.dot(x)) > 1.0 - 1.0e-4) x = Eigen::Vector3d(0, 1, 0);
Eigen::Vector3d y = z.cross(x); y.normalized();
x = y.cross(z); x.normalized();
Eigen::Matrix3d rotation;
rotation.block(0,0,3,1) = x;
rotation.block(0,1,3,1) = y;
rotation.block(0,2,3,1) = z;
Eigen::Quaterniond orientation( rotation );
tf.block(0,0,3,3) = orientation.matrix();
tf.block(0,3,3,1) = position;
return tf;
}
Eigen::Matrix3d Utility::g2R(const Eigen::Vector3d &g)
{
Eigen::Matrix3d R0;
Eigen::Vector3d ng1 = g.normalized();
Eigen::Vector3d ng2{0, 0, 1.0};
R0 = Eigen::Quaterniond::FromTwoVectors(ng1, ng2).toRotationMatrix();
double yaw = Utility::R2ypr(R0).x();
R0 = Utility::ypr2R(Eigen::Vector3d{-yaw, 0, 0}) * R0;
// R0 = Utility::ypr2R(Eigen::Vector3d{-90, 0, 0}) * R0;
return R0;
}
//==============================================================================
void PlanarJoint::UniqueProperties::setArbitraryPlane(
const Eigen::Vector3d& _transAxis1,
const Eigen::Vector3d& _transAxis2)
{
// Set plane type as arbitrary plane
mPlaneType = PT_ARBITRARY;
// First translational axis
mTransAxis1 = _transAxis1.normalized();
// Second translational axis
mTransAxis2 = _transAxis2.normalized();
// Orthogonalize translational axese
double dotProduct = mTransAxis1.dot(mTransAxis2);
assert(std::abs(dotProduct) < 1.0 - 1e-6);
if (std::abs(dotProduct) > 1e-6)
mTransAxis2 = (mTransAxis2 - dotProduct * mTransAxis1).normalized();
// Rotational axis
mRotAxis = (mTransAxis1.cross(mTransAxis2)).normalized();
}
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()));
}
RevoluteJoint::RevoluteJoint(const Eigen::Vector3d& axis,
const std::string& _name)
: Joint(REVOLUTE, _name),
mAxis(axis.normalized()) {
mGenCoords.push_back(&mCoordinate);
mS = Eigen::Matrix<double, 6, 1>::Zero();
mdS = Eigen::Matrix<double, 6, 1>::Zero();
mSpringStiffness.resize(1, 0.0);
mDampingCoefficient.resize(1, 0.0);
mRestPosition.resize(1, 0.0);
}
// Calculate the fitting error of a plane to certain 3D data
double PlaneFit::fitPlaneError(const Points3D& P, const Eigen::Vector3d& normal, Eigen::Vector3d& point)
{
// Normalise the normal vector (to put the resulting error in units of normal distance to plane)
Eigen::Vector3d n = normal.normalized();
// Calculate the average distance to the plane of the data points
double err = 0.0;
size_t N = P.size();
for(size_t i = 0; i < N; i++)
err += fabs(n.dot(P[i] - point))/N;
// Return the calculated error
return err;
}
Eigen::Vector3d Circuit::GetFieldFromPoint(
const Eigen::Vector3d& point,
const Eigen::Vector3d& segmentCenter,
const Eigen::Vector3d& flowDirection,
double segmentCoeff
) {
Eigen::Vector3d r = segmentCenter.array()-point.array();
double rLengthSquared = r.squaredNorm(); // distance squared.
Eigen::Vector3d rUnit = r.normalized();
Eigen::Vector3d Vcrossr = flowDirection.cross(rUnit);
Eigen::Vector3d B = segmentCoeff/rLengthSquared*Vcrossr.array();
return B;
}
RevoluteJoint::RevoluteJoint(BodyNode* _parent, BodyNode* _child,
const Eigen::Vector3d& axis,
const std::string& _name)
: Joint(_parent, _child, _name),
mAxis(axis.normalized())
{
mJointType = REVOLUTE;
mGenCoords.push_back(&mCoordinate);
mS = Eigen::Matrix<double,6,1>::Zero();
mdS = Eigen::Matrix<double,6,1>::Zero();
mDampingCoefficient.resize(1, 0);
}
ScrewJoint::ScrewJoint(const Eigen::Vector3d& axis,
double _pitch,
const std::string& _name)
: Joint(SCREW, _name),
mAxis(axis.normalized()),
mPitch(_pitch) {
mGenCoords.push_back(&mCoordinate);
mS = Eigen::Matrix<double, 6, 1>::Zero();
mdS = Eigen::Matrix<double, 6, 1>::Zero();
mSpringStiffness.resize(1, 0.0);
mDampingCoefficient.resize(1, 0.0);
mRestPosition.resize(1, 0.0);
}
int main(int , char** )
{
for (int k = 0; k < 100000; ++k) {
// create a random rotation matrix by sampling a random 3d vector
// that will be used in axis-angle representation to create the matrix
Eigen::Vector3d rotAxisAngle = Vector3d::Random();
rotAxisAngle += Vector3d::Random();
Eigen::AngleAxisd rotation(rotAxisAngle.norm(), rotAxisAngle.normalized());
Eigen::Matrix3d Re = rotation.toRotationMatrix();
// our analytic function which we want to evaluate
Matrix<double, 3 , 9> dq_dR;
compute_dq_dR (dq_dR,
Re(0,0),Re(1,0),Re(2,0),
Re(0,1),Re(1,1),Re(2,1),
Re(0,2),Re(1,2),Re(2,2));
// compute the Jacobian using AD
Matrix<double, 3 , 9, Eigen::RowMajor> dq_dR_AD;
typedef ceres::internal::AutoDiff<RotationMatrix2QuaternionManifold, double, 9> AutoDiff_Dq_DR;
double *parameters[] = { Re.data() };
double *jacobians[] = { dq_dR_AD.data() };
double value[3];
RotationMatrix2QuaternionManifold rot2quat;
AutoDiff_Dq_DR::Differentiate(rot2quat, parameters, 3, value, jacobians);
// compare the two Jacobians
const double allowedDifference = 1e-6;
for (int i = 0; i < dq_dR.rows(); ++i) {
for (int j = 0; j < dq_dR.cols(); ++j) {
double d = fabs(dq_dR_AD(i,j) - dq_dR(i,j));
if (d > allowedDifference) {
cerr << "\ndetected difference in the Jacobians" << endl;
cerr << PVAR(Re) << endl << endl;
cerr << PVAR(dq_dR_AD) << endl << endl;
cerr << PVAR(dq_dR) << endl << endl;
return 1;
}
}
}
cerr << "+";
}
return 0;
}
//not used, using implicit instead
vector<Eigen::Vector3d> mySpring::calcSpringForce() {// {S_SCALAR,S_VECTOR, ATTR, SPRING};
vector<Eigen::Vector3d> result(2, Eigen::Vector3d(0, 0, 0));
if (restLen != 0) { //spring with damping force
Eigen::Vector3d aMb = (a->getPosition() - b->getPosition());
Eigen::Vector3d lnorm = aMb.normalized();//unitlength vector of sprRestVec
Eigen::Vector3d ldot = (a->getPosition() - a->getPosition(1)); //since using verlet, use this for velocity
double d = ((a->getPosition(1) - b->getPosition(1)).norm());
double KsTerm = Ks * (d - restLen);
double KdTerm = Kd * (ldot.dot(lnorm));
double fp = -1 * (KsTerm + KdTerm);
result[0] = (lnorm * fp);
result[1] = (lnorm * (-1 * fp));
}//only add force if magnitude of distance vector is not 0
else {
cout << "0 restlen spring : " << ID << endl;
}
return result;
}//calcForceOnParticle
