Vector4d Matrix4d::operator* (Vector4d& other){
Vector4d result;
for (int i = 0; i < 4; i++){
result.set(i, other[0] * m[0][i] + other[1] * m[1][i] + other[2] * m[2][i] + other[3] * m[3][i]);
}
return result;
}
ColorBank::Colorf ColorBank::colorf(DotPath const &path) const
{
if (path.isEmpty()) return Colorf();
Vector4d clamped = data(path).as<Impl::ColorData>().color;
clamped = clamped.max(Vector4d(0, 0, 0, 0)).min(Vector4d(1, 1, 1, 1));
return Colorf(float(clamped.x), float(clamped.y), float(clamped.z), float(clamped.w));
}
Vector PickObjectTool::GetWorldCoordinates(BaseDraw* bd, const Matrix4d& m, Float x, Float y, Float z)
{
// pick object returns the view-projection matrix. This transforms a point in camera space into clip space.
Int32 l, t, r, b, w, h;
Vector4d pos;
Vector posWorld;
bd->GetFrame(&l, &t, &r, &b);
if (l == r || b == t)
return Vector(0.0);
w = r - l;
h = b - t;
// first, transform the points into clip space
pos.x = (x - Float(l)) / Float(w);
pos.y = (y - Float(t)) / Float(h);
pos.z = z;
pos.w = 1.0;
pos = pos * 2.0f - Vector4d(1.0f);
pos.y = -pos.y;
// apply the inverse view transform
Matrix4d im = !m;
pos = im * pos;
pos.MakeVector3();
// convert it into a 3-tupel
posWorld = bd->GetMg() * GetVector3(pos);
return posWorld;
}
/**
* operating with 3-vectors here because there are a lot of cross products
*/
double Triangle::intersection(const ray &viewRay)const {
Vector4d edge1 = p2 - p1;
Vector4d edge2 = p3 - p1;
Vector4d h = cross(viewRay.dir, edge2);
double a = edge1.dot(h);
if (a > -EPSILON && a < EPSILON)
return -1 * std::numeric_limits<double>::max();
double f = 1/a;
Vector4d s = viewRay.orig - p1;
double u = f * s.dot(h);
if (u < 0.0 || u > 1.0)
return -1 * std::numeric_limits<double>::max();
Vector4d q = cross(s, edge1);
double v = f * q.dot(viewRay.dir);
if (v < 0.0 || u + v > 1.0)
return -1 * std::numeric_limits<double>::max();
double t = f * edge2.dot(q);
return t;
}
int addPointAndProjection(SysSBA& sba, vector<Point, Eigen::aligned_allocator<Point> >& points, int ndi)
{
// Define dimensions of the image.
int maxx = 640;
int maxy = 480;
// Project points into nodes.
for (int i = 0; i < points.size(); i++)
{
double pointnoise = 0.1;
// Add points into the system, and add noise.
// Add up to .5 pixels of noise.
Vector4d temppoint = points[i];
temppoint.x() += pointnoise*(drand48() - 0.5);
temppoint.y() += pointnoise*(drand48() - 0.5);
temppoint.z() += pointnoise*(drand48() - 0.5);
int index = sba.addPoint(temppoint);
Vector3d proj;
calculateProj(sba, points[i], ndi, proj);
// If valid (within the range of the image size), add the stereo
// projection to SBA.
//if (proj.x() > 0 && proj.x() < maxx && proj.y() > 0 && proj.y() < maxy)
//{
sba.addStereoProj(ndi, index, proj);
//}
}
return sba.tracks.size() - points.size();
}
Vector6d gc_pipi_plk( Vector4d pi1, Vector4d pi2){
Vector6d plk;
Matrix4d dp = pi1 * pi2.transpose() - pi2 * pi1.transpose();
plk << dp(0,3), dp(1,3), dp(2,3), - dp(1,2), dp(0,2), - dp(0,1);
return plk;
}
void rigidBodyConstraintParseQuat(const mxArray* pm, Vector4d &quat)
{
if(!mxIsNumeric(pm))
{
mexErrMsgIdAndTxt("Drake:rigidBodyConstraintParseQuat:BadInputs","The input argument 1 should be a 4x1 double vector");
}
if(!(mxGetM(pm) == 4 && mxGetN(pm) == 1))
{
mexErrMsgIdAndTxt("Drake:rigidBodyConstraintParseQuat:BadInputs","The input argument 1 should be of size 4x1");
}
memcpy(quat.data(),mxGetPr(pm),sizeof(double)*4);
for(int i = 0;i<4;i++)
{
if((mxIsInf(quat(i)))||(mxIsNaN(quat(i))))
{
mexErrMsgIdAndTxt("Drake:rigidBodyConstraintParseQuat:BadInputs","The input argument 1 cannot have entry equal to NaN or Inf");
}
}
double quat_norm = quat.norm();
if(quat_norm==0.0)
{
mexErrMsgIdAndTxt("Drake:rigidBodyConstraintParseQuat:BadInputs","The Input argument 1 must be a nonzero vector");
}
quat = quat/quat_norm;
}
Vector4d Triangle::normal_vector(const Vector4d &surface) const {
double u, v;
std::tie(u, v) = uvCoords(surface);
Vector4d normal = (1 - u - v) * n1 + u * n2 + v * n3;
normal.normalize();
return normal;
}
void
addnode(SysSPA &spa, int n,
// node translation
std::vector< Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > ntrans,
// node rotation
std::vector< Eigen::Vector4d, Eigen::aligned_allocator<Eigen::Vector4d> > nqrot,
// constraint indices
std::vector< Eigen::Vector2i, Eigen::aligned_allocator<Eigen::Vector2i> > cind,
// constraint local translation
std::vector< Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > ctrans,
// constraint local rotation as quaternion
std::vector< Eigen::Vector4d, Eigen::aligned_allocator<Eigen::Vector4d> > cqrot,
// constraint covariance
std::vector< Eigen::Matrix<double,6,6>, Eigen::aligned_allocator<Eigen::Matrix<double,6,6> > > cvar)
{
Node nd;
// rotation
Quaternion<double> frq;
frq.coeffs() = nqrot[n];
frq.normalize();
if (frq.w() <= 0.0) frq.coeffs() = -frq.coeffs();
nd.qrot = frq.coeffs();
// translation
Vector4d v;
v.head(3) = ntrans[n];
v(3) = 1.0;
nd.trans = v;
nd.setTransform(); // set up world2node transform
nd.setDr(true);
// add to system
spa.nodes.push_back(nd);
// add in constraints
for (int i=0; i<(int)ctrans.size(); ++i)
{
ConP2 con;
con.ndr = cind[i].x();
con.nd1 = cind[i].y();
if ((con.ndr == n && con.nd1 <= n-1) ||
(con.nd1 == n && con.ndr <= n-1))
{
con.tmean = ctrans[i];
Quaternion<double> qr;
qr.coeffs() = cqrot[i];
qr.normalize();
con.qpmean = qr.inverse(); // inverse of the rotation measurement
con.prec = cvar[i]; // ??? should this be inverted ???
// need a boost for noise-offset system
//con.prec.block<3,3>(3,3) *= 10.0;
spa.p2cons.push_back(con);
}
}
}
void Model::RotateObject(Shape* shape, TreeObject* object, Vector4d rotate)
{
if (!shape)
return;
Vector3d rot(rotate.x(), rotate.y(), rotate.z());
shape->Rotate(rot, rotate.w());
ModelChanged();
}
Vector3d transformPoint(Vector3d& point,const MatrixXd& transform)
{
Vector4d tmp;
tmp.head(3)=point;
tmp(3)=1;
tmp.head(3)=transform*tmp;
return tmp.head(3);
}
void CSimuVertexRingObj::_computeElasticForcesNew(
const unsigned int timeid, const bool isStatic, const bool needjacobian)
{
//compute the rotation for each vertex
const bool bRecompRot = (timeid%2 == 1);
if (timeid<=1 || bRecompRot){
const bool needquat = false;
updateRotationQuaternionForAllElements(timeid, needquat);
}
else{
//use central difference to update the quaternions
for (int i=0; i<m_nVRingElementCount; i++){
CVertexRingElement &e = m_pVRingElement[i];
Vector4d quat = e.m_quat + e.m_quat - e.m_quat0;
quat.normalize();
e.m_quat0 = e.m_quat;
e.m_quat = quat;
//convert quat to matrix
Quaternion *pquat = (Quaternion*)&e.m_quat.x;
typedef double M33[3][3];
pquat->getRotationMatrix(*((M33*)&e.m_R.x));
}
}
//call the old alg.
const int SKIPSTEP = m_nRotationSkipStep;
m_nRotationSkipStep = INT_MAX-1;
_computeElasticForces(timeid, isStatic, needjacobian);
m_nRotationSkipStep = SKIPSTEP;
/*
//compute forces using the rotation
Vector3d force;
double3x3 jacobian, *pjac=NULL;
const bool FASTSTIFF = (this->m_mtl.getMaterialType()==0);
if (needjacobian) pjac = &jacobian;
for (int i=0; i<m_nEdge; i++){
CSimuEdgeInput& edge = m_pEdge[i];
const int v0=edge.v0, v1=edge.v1;
if (bRecompRot){
//computer averaged rotation of the rod, then save it
const Quaternion *q0 = (const Quaternion *)(&m_pVRingElement[v0].m_quat);
const Quaternion *q1 = (const Quaternion *)(&m_pVRingElement[v1].m_quat);
const Quaternion q = Quaternion::slerp_midpoint(*q0, *q1);
q.getRotationMatrix(*((double(*)[3][3])(&edge.mat)));
}
//apply the rotation for the rod element
const Vector3d& p0 = m_pVertInfo[v0].m_pos;
const Vector3d& p1 = m_pVertInfo[v1].m_pos;
m_pGyrod[i].computeNodalForce(p0, p1, edge.mat, *edge.pMaterial, force, pjac);
//accumulate the force and stiffness
m_pVertInfo[v0].m_force+=force;
m_pVertInfo[v1].m_force-=force;
if (needjacobian)
saveVertSitffMatrixIntoSparseMatrix(v0, v1, *pjac, FASTSTIFF);
}
*/
}
inline bool cullSphere(const Vector4d& center,
double radius) const
{
return (center.z() - radius > m_nearZ ||
center.z() + radius < m_farZ ||
center.dot(m_planeNormals[0]) < -radius ||
center.dot(m_planeNormals[1]) < -radius ||
center.dot(m_planeNormals[2]) < -radius ||
center.dot(m_planeNormals[3]) < -radius);
}
Vector4d rigidBodyDynamics::mrp2quaternion(Vector3d mrp) const{
Vector4d dq;
dq << 8*mrp / (16 + mrp.transpose() * mrp), (16 - mrp.transpose() * mrp) / (16+mrp.transpose() * mrp);
// std::cout << "MRP: " << mrp.transpose() << std::endl;
// std::cout << "dq_prenorm: " << dq.transpose() << std::endl;
dq /=dq.norm();
// std::cout << "dq_postnorm: " << dq.transpose() << std::endl;
return dq;
}
double geometric(Vector4d s1, Vector4d n1, Vector4d s2, Vector4d n2) {
ASSERT(isUnitVector(n1), "assumes the normals are normalized");
ASSERT(isUnitVector(n2), "assumes the normals are normalized");
Vector4d segment = s1 - s2;
Vector4d seg_dir = segment.normalized();
double cos1 = n1.dot(seg_dir);
double cos2 = n2.dot(-1 * seg_dir);
return std::abs(cos1 * cos2) / segment.squaredNorm();
}
// Project a 3D point into this Pose.
Vector2d Pose::ProjectTo2D(const Vector3d& pt3d) {
Vector4d pt3d_h = Vector4d::Constant(1.0);
pt3d_h.head(3) = pt3d;
const Vector4d proj_h = Rt_ * pt3d_h;
Vector2d proj = proj_h.head(2);
proj /= proj_h(2);
return proj;
}
Vector4d CProxyCamera::intersect(const Vector4d &coord, const Vector4d& abcd) const
{
Vector4d ray = m_center - coord;
const double A = coord.dot(abcd);
const double B = ray.dot(abcd);
if (fabs(B)<1e-8)
return Vector4d(0.0f, 0.0f, 0.0f, -1.0f);
else
return coord - A / B * ray;
}
// Test a point to see if it lies within the frustum defined by
// planes z=nearZ, z=farZ, and the four side planes with specified
// normals.
static inline bool frustumCull(const Vector4d& curvePoint,
double curveBoundingRadius,
double nearZ, double farZ,
const Vector4d viewFrustumPlaneNormals[])
{
return (curvePoint.z() - curveBoundingRadius > nearZ ||
curvePoint.z() + curveBoundingRadius < farZ ||
curvePoint.dot(viewFrustumPlaneNormals[0]) < -curveBoundingRadius ||
curvePoint.dot(viewFrustumPlaneNormals[1]) < -curveBoundingRadius ||
curvePoint.dot(viewFrustumPlaneNormals[2]) < -curveBoundingRadius ||
curvePoint.dot(viewFrustumPlaneNormals[3]) < -curveBoundingRadius);
}
/** Add a new sample to the path. If the sample time is less than the first time,
* it is added at the end. If it is greater than the last time, it is appended
* to the path. The sample is ignored if it has a time in between the first and
* last times of the path.
*/
void
CurvePlot::addSample(const CurvePlotSample& sample)
{
bool addToBack = false;
if (m_samples.empty() || sample.t > m_samples.back().t)
{
addToBack = true;
}
else if (sample.t < m_samples.front().t)
{
addToBack = false;
}
else
{
// Sample falls within range of current samples; discard it
return;
}
if (addToBack)
m_samples.push_back(sample);
else
m_samples.push_front(sample);
if (m_samples.size() > 1)
{
// Calculate a bounding radius for this segment. No point on the curve will
// be further from the start point than the bounding radius.
if (addToBack)
{
const CurvePlotSample& lastSample = m_samples[m_samples.size() - 2];
double dt = sample.t - lastSample.t;
Matrix4d coeff = cubicHermiteCoefficients(zeroExtend(lastSample.position),
zeroExtend(sample.position),
zeroExtend(lastSample.velocity * dt),
zeroExtend(sample.velocity * dt));
Vector4d extents = coeff.cwiseAbs() * Vector4d(0.0, 1.0, 1.0, 1.0);
m_samples[m_samples.size() - 1].boundingRadius = extents.norm();
}
else
{
const CurvePlotSample& nextSample = m_samples[1];
double dt = nextSample.t - sample.t;
Matrix4d coeff = cubicHermiteCoefficients(zeroExtend(sample.position),
zeroExtend(nextSample.position),
zeroExtend(sample.velocity * dt),
zeroExtend(nextSample.velocity * dt));
Vector4d extents = coeff.cwiseAbs() * Vector4d(0.0, 1.0, 1.0, 1.0);
m_samples[1].boundingRadius = extents.norm();
}
}
}
Vector4d rigidBodyDynamics::quaternionFromRot(Matrix3d& R) const{
Vector4d q;
double div1, div2, div3, div4;
double numerical_limit = 1.0e-4;
if (abs(R.determinant()-1) > numerical_limit ) {
std::cerr << "R does not have a determinant of +1" << std::endl;
} else {
div1 = 0.5*sqrt(1+R(0,0)+R(1,1)+R(2,2));
div2 = 0.5*sqrt(1+R(0,0)-R(1,1)-R(2,2));
div3 = 0.5*sqrt(1-R(0,0)-R(1,1)+R(2,2));
div4 = 0.5*sqrt(1-R(0,0)+R(1,1)-R(2,2));
//if (div1 > div2 && div1 > div3 && div1 > div4) {
if (fabs(div1) > numerical_limit) {
q(3) = div1;
q(0) = 0.25*(R(1,2)-R(2,1))/q(3);
q(1) = 0.25*(R(2,0)-R(0,2))/q(3);
q(2) = 0.25*(R(0,1)-R(1,0))/q(3);
} else if (fabs(div2) > numerical_limit) {
//} else if (div2 > div1 && div2 > div3 && div2 > div4) {
q(0) = div2;
q(1) = 0.25*(R(0,1)+R(1,0))/q(0);
q(2) = 0.25*(R(0,2)+R(2,0))/q(0);
q(3) = 0.25*(R(1,2)+R(2,1))/q(0);
} else if (fabs(div3) > numerical_limit) {
//} else if (div3 > div1 && div3 > div2 && div3 > div4) {
q(2) = div3;
q(0) = 0.25*(R(0,2)+R(2,0))/q(2);
q(1) = 0.25*(R(1,2)+R(2,1))/q(2);
q(3) = 0.25*(R(0,1)-R(1,0))/q(2);
//} else {
} else if (fabs(div4) > numerical_limit) {
q(1) = div4;
q(0) = 0.25*(R(0,1)+R(1,0))/q(1);
q(2) = 0.25*(R(1,2)+R(2,1))/q(1);
q(3) = 0.25*(R(2,0)-R(0,2))/q(1);
} else {
std::cerr << "quaternionFromRot didn't convert: [" << div1 << ", " << div2 << ", " << div3 << ", " << div4 << std::endl;
std::cerr << "Rotation Matrix: " << R << std::endl;
}
}
/*
if (q(3) < 0) {
q *= -1;
}
*/
q /=q.norm();
return q;
}
Vector4d rigidBodyDynamics::quaternionMultiplication(Vector4d& q1, Vector4d& q2) const {
//q1 \mult q2
Matrix4d qm;
Vector4d result;
qm << q1(3), q1(2), -q1(1), q1(0),
-q1(2), q1(3), q1(0), q1(1),
q1(1), -q1(0), q1(3), q1(2),
-q1(0), -q1(1), -q1(2), q1(3);
result = qm*q2;
result /= result.norm();
return result;
}
void mexFunction(int nlhs, mxArray* plhs[], int nrhs, const mxArray* prhs[])
{
if(nrhs != 5)
{
mexErrMsgIdAndTxt("Drake:testQuatmex:BadInputs","Usage [r,dr,e,ed,quat,dquat,q3,dq3,w,dw] = testQuatmex(q1,q2,axis,u,v)");
}
Vector4d q1;
Vector4d q2;
memcpy(q1.data(),mxGetPr(prhs[0]),sizeof(double)*4);
memcpy(q2.data(),mxGetPr(prhs[1]),sizeof(double)*4);
Vector4d r = quatDiff(q1,q2);
Matrix<double,4,8> dr = dquatDiff(q1,q2);
plhs[0] = mxCreateDoubleMatrix(4,1,mxREAL);
plhs[1] = mxCreateDoubleMatrix(4,8,mxREAL);
memcpy(mxGetPr(plhs[0]),r.data(),sizeof(double)*4);
memcpy(mxGetPr(plhs[1]),dr.data(),sizeof(double)*4*8);
Vector3d axis;
memcpy(axis.data(),mxGetPr(prhs[2]),sizeof(double)*3);
double e = quatDiffAxisInvar(q1,q2,axis);
Matrix<double,1,11> de = dquatDiffAxisInvar(q1,q2,axis);
plhs[2] = mxCreateDoubleScalar(e);
plhs[3] = mxCreateDoubleMatrix(1,11,mxREAL);
memcpy(mxGetPr(plhs[3]),de.data(),sizeof(double)*11);
Vector3d u,v;
Vector4d quat;
Matrix<double,4,6> dquat;
memcpy(u.data(),mxGetPr(prhs[3]),sizeof(double)*3);
memcpy(v.data(),mxGetPr(prhs[4]),sizeof(double)*3);
Vector4d q3 = quatProduct(q1,q2);
Matrix<double,4,8> dq3 = dquatProduct(q1,q2);
plhs[4] = mxCreateDoubleMatrix(4,1,mxREAL);
plhs[5] = mxCreateDoubleMatrix(4,8,mxREAL);
memcpy(mxGetPr(plhs[4]),q3.data(),sizeof(double)*4);
memcpy(mxGetPr(plhs[5]),dq3.data(),sizeof(double)*4*8);
Vector3d w = quatRotateVec(q1,u);
Matrix<double,3,7> dw = dquatRotateVec(q1,u);
plhs[6] = mxCreateDoubleMatrix(3,1,mxREAL);
plhs[7] = mxCreateDoubleMatrix(3,7,mxREAL);
memcpy(mxGetPr(plhs[6]),w.data(),sizeof(double)*3);
memcpy(mxGetPr(plhs[7]),dw.data(),sizeof(double)*3*7);
}
Eigen::Matrix<typename Derived::Scalar, 4, 1> rpy2quat(const Eigen::MatrixBase<Derived>& rpy)
{
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Eigen::MatrixBase<Derived>, 3);
auto rpy_2 = (rpy / 2.0).array();
auto s = rpy_2.sin();
auto c = rpy_2.cos();
Vector4d q;
q << c(0)*c(1)*c(2) + s(0)*s(1)*s(2),
s(0)*c(1)*c(2) - c(0)*s(1)*s(2),
c(0)*s(1)*c(2) + s(0)*c(1)*s(2),
c(0)*c(1)*s(2) - s(0)*s(1)*c(2);
q /= q.norm() + std::numeric_limits<typename Derived::Scalar>::epsilon();
return q;
}
Vector6d gc_aid_to_av(Vector4d aid) {
Vector6d av;
Vector3d aa = aid.head(3);
double d = 1.0 / aid(3);
Matrix3d R = gc_Rodriguez(aa);
av.head(3) = R.col(2) * d;
av.tail(3) = R.col(0);
// Vector6d av;
// double a = aid[0];
// double b = aid[1];
// double g = aid[2];
// double t = aid[3];
//
// double s1 = sin(a);
// double c1 = cos(a);
// double s2 = sin(b);
// double c2 = cos(b);
// double s3 = sin(g);
// double c3 = cos(g);
//
// Matrix3d R;
// R <<
// c2 * c3, s1 * s2 * c3 - c1 * s3, c1 * s2 * c3 + s1 * s3,
// c2 * s3, s1 * s2 * s3 + c1 * c3, c1 * s2 * s3 - s1 * c3,
// -s2, s1 * c2, c1 * c2;
//
// double d = 1.0 / t;
// av.head(3) = -R.col(2) * d;
// av.tail(3) = R.col(1);
return av;
}
Vector6d gc_asd_to_av(Vector4d asd) {
Vector6d av;
Vector3d aa = asd.head(3);
// double d_inv = asd(3);
// double sig_d_inv = (1.0 - exp(-asd(3))) / (2.0 * (1.0 + exp(-asd(3))));
// double sig_d_inv = -log(1.0/asd(3) - 1.0);
// double sig_d_inv = log( (2.0 * asd(3) + 1.0) / (1.0 - 2.0*asd(3)) );
// double sig_d_inv = atan(asd(3)) / 2.0;
// double sig_d_inv = atan2(asd(3), 1.0) / 2.0;
// double sig_d_inv = atan2(asd(3), 1.0);
// double sig_d_inv = atan2(asd(3), 1.0) * 1.0;
// double sig_d_inv = tan(4.0 * asd(3));
double sig_d_inv = log(asd(3));
// cout << "sig_d_inv = " << sig_d_inv << endl;
// double sig_d_inv = cos(asd(3)) / sin(asd(3));
// double sig_d_inv = sin(asd(3)) / cos(asd(3));
// double sig_d_inv = sin(asd(3)) / cos(asd(3));
Matrix3d R = gc_Rodriguez(aa);
// av.head(3) = R.col(2) / sig_d_inv;
av.head(3) = R.col(2) * sig_d_inv;
av.tail(3) = R.col(0);
return av;
}
// 6. Total measurement update
void Ekf::measurementUpdateDecaWave(Vector4d z){
// Determine h and H
Vector4d h;
h = hDecaWave(state_, DecaWaveBeaconLoc_, DecaWaveOffset_);
Matrix46 H;
H = HDecaWave(state_, DecaWaveBeaconLoc_, DecaWaveOffset_);
// Find Kalman Gain
// First check if any z values are -1 (i.e. a bad measurement). If so, make
// the corresponding entry in RDecaWave_ very high for this iteration. This
// effectively cancels out any influence by that measurement on the update.
Matrix4d RDecaWaveTemp;
for (int i = 0; i < z.rows(); i++) {
if (z(i) < 0) {
RDecaWaveTemp(i,i) = 1e20;
}
else {
RDecaWaveTemp(i,i) = RDecaWave_(i,i);
}
}
// Now go on to calculate the Kalman gain with the new R value.
Matrix64 K;
K = KDecaWave(cov_, H, RDecaWaveTemp);
// Find new state
state_ = stateUpdateDecaWave(state_, K, z, h);
// Find new covariance
cov_ = covUpdateDecaWave(cov_, K, H);
cov_ = zeroOutBiasXYThetaCov(cov_);
}
std::tuple<double, double> Triangle::uvCoords(const Vector4d &point) const {
Vector4d edge1 = p2 - p1;
Vector4d edge2 = p3 - p1;
Vector4d h = cross(trueNormal, edge2);
double a = edge1.dot(h);
double f = 1/a;
Vector4d s = point - p1;
double u = f * s.dot(h);
Vector4d q = cross(s, edge1);
double v = f * q.dot(trueNormal);
return std::make_tuple(u, v);
}
void rigidBodyDynamics::setState(VectorXd x, Vector4d q) {
_r = x.segment<3>(0);
_v = x.segment<3>(3);
_a = x.segment<3>(6);
_w = x.segment<3>(9);
_qref = q / q.norm();
}
bool Rotation3::FromQuaternion(const Vector4d& quat) {
if (fabs(quat.norm() - 1.0) > 1e-6) {
std::cout << "Rotation3::FromQuaternion: invalid quaternion" << std::endl;
return false;
}
quat_ = quat;
return true;
}
ClothoidPtr ClothoidFitter::getCurveWithZeroCurvature(double param) const
{
Matrix<double, 5, 5> lhs;
Matrix<double, 5, 1> rhs;
Vector4d constraint;
constraint << 6 * param, 2, 0, 0; // second derivative of ax^3+bx^2+cx+d is 6ax+2b
//For constrained least squares,
//lhs is now [A^T A C]
// [ C^T 0]
lhs << _getLhs(_totalLength), constraint,
constraint.transpose(), 0;
rhs << _rhs, 0;
Vector4d abcd = (lhs.inverse() * rhs).head<4>();
return getClothoidWithParams(abcd);
}
