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;
}