static void apply(Alpha a, const V1 &x, const V2 &y, Beta b, V3 &z)
 {
     if (!math::is_zero(b))
         z.array() = a * x.array() * y.array() + b * z.array();
     else
         z.array() = a * x.array() * y.array();
 }

void frenet(const V3& d1, const V3& d2, V3& t, V3& n, V3& b){	
	b = cross(d2, d1);
	n = cross(d1, b);
	t = d1 * 1./sqrt((d1.magSqr()));
	b *= 1./sqrt(b.magSqr());
	n *= 1./sqrt(n.magSqr());
}

void Canopy::growHexSegment(treeNode *rootOfTree, BranchBase *pCanopyBranch,aabb TreeBoundingBox, LevelDetail *grammar, V3 startHeading)
{
    theOverseer = observer::Instance();
    rootOfTree->tree->m_CanopyCount++;
    V3 root(rootOfTree->pbranch->segments[0].m_tipPointList[0]);
	V3 CanopySegmentRoot(pCanopyBranch->tipPoint);// canopySegmentRoot
	V3 CanopyTop(root);
    CanopyTop.y = root.y + TreeBoundingBox.yMax;
	V3 CanopyFulcrum(CanopyTop);
	CanopyFulcrum.y = CanopyFulcrum.y * 0.57f; // 4/7
    if(pCanopyBranch->tipPoint.y < CanopyFulcrum.y){
        CanopyFulcrum.y = pCanopyBranch->tipPoint.y * 0.714f;// 5/7
    }
	V3 CanopyHeading = CanopySegmentRoot-CanopyFulcrum;
	CanopyHeading.Normalize();
	V3 CanopyArbitrary(CanopySegmentRoot-root);
	CanopyArbitrary.Normalize();
	V3 Left = CrossProduct(CanopyArbitrary,CanopyHeading);
	V3 Right = CrossProduct(CanopyHeading,CanopyArbitrary);
	Left.Normalize();
	Right.Normalize();
	V3 Down = CrossProduct(Left,CanopyHeading);
	V3 Up = CrossProduct(CanopyHeading,Left);
    V3 perturb = CanopyHeading;
	Down.Normalize();
	Up.Normalize();
    int shift = -((int)floor(m_width/2.0f));
    float nudge =0.0f;

    growPatchSegment(rootOfTree, pCanopyBranch,TreeBoundingBox, grammar,startHeading);
}

// Professor's implemetation is more elegant and compact
V3 V3::rotateThisPointAboutAxis(V3 Oa, V3 aDir, float theta) {
    // build aux coordinate system with axis as one of its principal axes
    V3 auxAxis;
    if (fabsf(aDir[0]) > fabsf(aDir[1])) {
        auxAxis = V3(0.0f, 1.0f, 0.0f);
    }
    else {
        auxAxis = V3(1.0f, 0.0f, 0.0f);
    }

    V3 yl = aDir;
    V3 zl = (aDir ^ auxAxis);
    zl.normalize();
    V3 xl = (yl ^ zl);
    xl.normalize();
    M33 lcs;
    lcs[0] = xl;
    lcs[1] = yl;
    lcs[2] = zl;

    // transform to aux coordinate system
    V3 &p = *this;
    V3 p1 = lcs*(p - Oa);

    // rotate about principal axis
    M33 roty;
    roty.setRotationAboutY(theta);
    V3 p2 = roty * p1;

    // transform back to old world
    V3 ret = lcs.getInverted()*p2 + Oa;
    return ret;
}

Matrix<POSE_T, 3, 1> find_opt_cc(NormalAOPoseAdapter<POSE_T, POINT_T>& adapter)
{
	//the R has been fixed, we need to find optimal cc, camera center, given n pairs of 2-3 correspondences
	//Slabaugh, G., Schafer, R., & Livingston, M. (2001). Optimal Ray Intersection For Computing 3D Points From N -View Correspondences.
	typedef Matrix<POSE_T, 3, 1> V3;
	typedef Matrix<POSE_T, 3, 3> M3;

	M3 Rwc = adapter.getRcw().inverse().matrix();
	M3 AA; AA.setZero();
	V3 bb; bb.setZero();
	for (int i = 0; i < adapter.getNumberCorrespondences(); i++)
	{
		if (adapter.isInlier23(i)){
			V3 vr_w = Rwc * adapter.getBearingVector(i).template cast<POSE_T>();
			M3 A;
			A(0,0) = 1 - vr_w(0)*vr_w(0);
			A(1,0) = A(0,1) = - vr_w(0)*vr_w(1);
			A(2,0) = A(0,2) = - vr_w(0)*vr_w(2);
			A(1,1) = 1 - vr_w(1)*vr_w(1);
			A(2,1) = A(1,2) = - vr_w(1)*vr_w(2);
			A(2,2) = 1 - vr_w(2)*vr_w(2);
			V3 b = A * adapter.getPointGlob(i).template cast<POSE_T>();
			AA += A;
			bb += b;
		}
	}
	V3 c_w;
	if (fabs(AA.determinant()) < POSE_T(0.0001))
		c_w = V3(numeric_limits<POSE_T>::quiet_NaN(), numeric_limits<POSE_T>::quiet_NaN(), numeric_limits<POSE_T>::quiet_NaN());
	else
		c_w = AA.jacobiSvd(ComputeFullU | ComputeFullV).solve(bb);
	return c_w;
}

void PPC::positionRelativeToPoint(
	const V3 & P, 
	const V3 & vd, 
	const V3 & up, 
	float distance)
{
	// assumes: up and vd are normalized
	// quit early if assumptions are not met
	if (!((fabs(vd.length() - 1.0f)) < epsilonNormalizedError) ||
		!((fabs(up.length() - 1.0f)) < epsilonNormalizedError)) {
		cerr << "ERROR: Up or vd vectors are not normal vectors. Camera positioning aborted..." << endl;
		return;
	}

	V3 newa, newb, newc, newC;
	// compute new C, a, and b
	newC = P - (vd * distance);
	newa = (vd ^ up).getNormalized() * a.length();
	newb = (vd ^ newa).getNormalized() * b.length();

	V3 principalPoint = getPrincipalPoint();
	float PPu = principalPoint.getX();
	float PPv = principalPoint.getY();
	// compute new c
	newc = vd*getFocalLength() - (PPu * newa) - (PPv * newb);

	// commit new values
	C = newC;
	a = newa;
	b = newb;
	c = newc;

	// update projection matrix
	buildProjM();
}

double angle(const V3 & u, const V3 & v) 
{
	double dot = u * v;
	double nu = u.normL2();
	double nv = v.normL2();
	return remainder(acos(dot/(nu*nv)),2*M_PI);
}

    V3 bounce(Ray ray, V3 normal) {
        // cout << "chrome bounce\n";
        double theta1 = fabs(ray.direction.dot(normal));
        double internalIndex, externalIndex;
        if (theta1 >= 0.0L) {
            internalIndex = ior;
            externalIndex = 1.0L;
        } else {
            internalIndex = 1.0L;
            externalIndex = ior;
        }

        double eta = externalIndex/internalIndex;
        double theta2 = sqrt(1.0L - (eta * eta) * (1.0L - (theta1 * theta1)));
        double rs = (externalIndex * theta1 - internalIndex * theta2) / (externalIndex*theta1 + internalIndex * theta2);
        double rp = (internalIndex * theta1 - externalIndex * theta2) / (internalIndex*theta1 + externalIndex * theta2);
        double reflectance = (rs*rs + rp*rp);

        //reflection
        if(unifRand() < reflectance+reflection) {
            return ray.direction.add(normal.muls(theta1*2.0L));
        }

        // refraction
        return (ray.direction.add(normal.muls(theta1)).muls(eta) \
            .add(normal.muls(-theta2)));
    }

void PPC::setByInterpolation(PPC  &ppc0, PPC &ppc1, int i, int n)
{
	// assumption is that ppc0 and ppc1 have the same internal parameters
	float t = (float)i / (float)(n - 1);
	// Ci
	C = ppc0.C + (ppc1.C - ppc0.C) * t;
	// vdi
	V3 vd0 = ppc0.getViewDir();
	V3 vd1 = ppc1.getViewDir();
	V3 vdi = vd0 + (vd1 - vd0) * t;
	// ai
	a = ppc0.a + (ppc1.a - ppc0.a) * t;
	// in order to calculate b and c we use the camera positioning 
	// formulation. These formulas require us to know internal
	// camera parameters such as PPu, PPv, f, a.length and b.length.
	// But since we are assuming both endpoint cameras have the same
	// internals we can use camera zero as reference
	float PPu = ppc0.getPrincipalPoint().getX();
	float PPv = ppc0.getPrincipalPoint().getY();
	float f = ppc0.getFocalLength();
    // we could assume this is always one in our case but are calculated
	// here anyways just to follow the slides which apply for more general cameras
	float bLength = ppc0.b.length();
	// bi
	V3 orthoNormalVector = vdi ^ a;
	orthoNormalVector.normalize();
	b = orthoNormalVector * bLength;
	// ci
	c = -1.0f * (PPu * a) - (PPv * b) +
		vdi*f;
	// update projection matrix
	buildProjM();
}

inline void bi::exp_vector(V2 x, const V3& is) {
  BOOST_AUTO(iter, is.begin());
  BOOST_AUTO(end, is.end());
  for (; iter != end; ++iter) {
    BOOST_AUTO(elem, subrange(x, *iter, 1));
    exp_elements(elem, elem);
  }
}

inline void frenet(const V3& d1, const V3& d2, V3& t, V3& n, V3& b){	
	b = cross(d2, d1);
	n = cross(d1, b);
	t = d1;
	t.normalize();
	b.normalize();
	n.normalize();
}

    V5() {
	thunk_OK = this;
	if( bar() < 1000 ) {
	    V2 *p = this;
	    p->foo( bar() );
	    V3 *q = this;
	    q->foo( bar() );
	}
    }

/**
 * Draw axis aligned rectangle.
 *
 * @param p0	the top left corner of the rectangle
 * @param p1	the bottom right corner of the rectangle
 * @param c		the color
 */
void FrameBuffer::DrawRectangle(const V3 &p0, const V3 &p1, const V3 &c) {
	V3 p2(p0.x(), p1.y(), 0.0f);
	V3 p3(p1.x(), p0.y(), 0.0f);

	Draw2DSegment(p0, c, p2, c);
	Draw2DSegment(p2, c, p1, c);
	Draw2DSegment(p1, c, p3, c);
	Draw2DSegment(p3, c, p0, c);
}

    Camera(V3 iorigin, V3 itopleft, V3 itopright, V3 ibottomleft) {
        origin = iorigin;
        topleft = itopleft;
        topright = itopright;
        bottomleft = ibottomleft;

        xd = topright.sub(topleft);
        yd = bottomleft.sub(topleft);
    }

real bi::det_vector(const V2 x, const V3& is) {
  BOOST_AUTO(iter, is.begin());
  BOOST_AUTO(end, is.end());
  real det = 1.0;
  for (; iter != end; ++iter) {
    det *= *(x.begin() + *iter);
  }
  return det;
}

/**
 * Setup this camera such that it looks at the specified point with the specified view direction, up direction, and the distance from the point.
 *
 * @param p		the target point that this camera will look at
 * @param vd	the view direction
 * @param up	the up direction
 * @param d		the distance between the center of this camera and the target
 */
void PPC::LookAt(const V3 &p, const V3 &vd, const V3 &up, float d) {
	float f = GetFocalLength();

	C = p - vd.UnitVector() * d;
	a = (vd ^ up).UnitVector() * a.Length();
	b = (vd ^ a).UnitVector() * b.Length();
	c = vd.UnitVector() * f - a * ((float)w / 2.0f) - b * ((float)h / 2.0f);

	SetPMat();
}

V3 PPC::unproject(const V3 & projP) const
{
	// From projection of point formula we know
	// P = C + (au + bv + c) * w
	// but since our project function above returns 1/w
	// P = C + (au + bv + c) * (1/w)
	V3 ret = C + (a*projP.getX() + b*projP.getY() + c) / projP.getZ();

	return ret;
}

void format_type(Formatter &f, const V3 &v)
{
	string fmt = f.front();

	     if(fmt == "%(abs)") { f.out << abs(v); }
	else if(fmt == "%(polar())") {
		f.out << io::format("[r=%f theta=%f phi=%f]") << abs(v) << deg(v.theta()) << deg(v.phi());
	}
	else if(fmt == "%(x)") { f.out << v.x; }
	else if(fmt == "%(y)") { f.out << v.y; }
	else if(fmt == "%(z)") { f.out << v.z; }
	else f.out << v;
}

/**
 * Scale this mesh by placing the centroid at givin position and scaling to given AABB size.
 *
 * @param centroid		the given position for the new centroid
 * @param size			the given AABB size
 */
void TMesh::Scale(const V3 &centroid, const V3 &size) {
	AABB aabb;
	ComputeAABB(aabb);

	V3 c = GetCentroid();
	
	V3 scale(size.x() / aabb.Size().x(), size.y() / aabb.Size().y(), size.z() / aabb.Size().z());

	for (int i = 0; i < vertsN; i++) {
		verts[i].v[0] = (verts[i].v.x() - c.x()) * scale.x() + centroid.x();
		verts[i].v[1] = (verts[i].v.y() - c.y()) * scale.y() + centroid.y();
		verts[i].v[2] = (verts[i].v.z() - c.z()) * scale.z() + centroid.z();
	}
}

void PPC::zoom(float zoomFactor)
{
	// calculate new focal length
	float newFocalLength = getFocalLength() * zoomFactor;
	// calculate new c
	V3 principalPoint = getPrincipalPoint();
	V3 viewDirection = getViewDir();
	float PPu = principalPoint.getX();
	float PPv = principalPoint.getY();
	c = -1.0f * (PPu * a) - (PPv * b) +
		viewDirection*newFocalLength;
	// update projection matrix
	buildProjM();
}