TEST_F(CubeMeshTests, IntersectionTest) {
  IObject3D *object = Cube;

  Ray ray(glm::dvec3(3.0, 2.0, -1.0), Z_NORM_VEC);
  IntersectionResult res = object->Intersect(ray);
  ASSERT_TRUE(res);
  ASSERT_DOUBLE_EQ(1.0, res.GetDistance());
  ASSERT_VEC_NEAR(-Z_NORM_VEC, (res.GetNormalRay().GetDirection()), EPS_WEAK);

  // Test ray falling on edge of triangle.
  ray.SetOrigin(glm::dvec3(3.0, 3.0, -1.0));
  res = object->Intersect(ray);
  ASSERT_TRUE(res);
  ASSERT_DOUBLE_EQ(1.0, res.GetDistance());
  ASSERT_VEC_NEAR(-Z_NORM_VEC, (res.GetNormalRay().GetDirection()), EPS_WEAK);

  // Test ray falling on vertex.
  ray.SetOrigin(glm::dvec3(10.0, 5.0, 0.0));
  ray.SetDirection(-X_NORM_VEC);
  res = object->Intersect(ray);
  ASSERT_TRUE(res);
  ASSERT_DOUBLE_EQ(5.0, res.GetDistance());
  ASSERT_VEC_NEAR(X_NORM_VEC, (res.GetNormalRay().GetDirection()), EPS_WEAK);
}

 bool StaticMapTree::getObjectHitPos(const Vector3& pPos1, const Vector3& pPos2, Vector3& pResultHitPos, float pModifyDist) const
 {
     float maxDist = (pPos2 - pPos1).magnitude();
     // valid map coords should *never ever* produce float overflow, but this would produce NaNs too:
     MANGOS_ASSERT(maxDist < std::numeric_limits<float>::max());
     // prevent NaN values which can cause BIH intersection to enter infinite loop
     if (maxDist < 1e-10f)
     {
         pResultHitPos = pPos2;
         return false;
     }
     Vector3 dir = (pPos2 - pPos1) / maxDist;            // direction with length of 1
     G3D::Ray ray(pPos1, dir);
     float dist = maxDist;
     if (getIntersectionTime(ray, dist, false))
     {
         pResultHitPos = pPos1 + dir * dist;
         if (pModifyDist < 0)
         {
             if ((pResultHitPos - pPos1).magnitude() > -pModifyDist)
             {
                 pResultHitPos = pResultHitPos + dir * pModifyDist;
             }
             else
             {
                 pResultHitPos = pPos1;
             }
         }
         else
         {
             pResultHitPos = pResultHitPos + dir * pModifyDist;
         }
     }
     pResultHitPos = pPos2;
     return false;
 }

bool CGraphicView::GetXYPointForDXFTextExport( const CPoint& screenPt, CPoint3D& worldPt )
{
	//get a ray that goes through screenPt from the front of the view frustum to the back of it
	CPoint3D nearPt, farPt;
	if( !Get3DPointFromScreen( screenPt, nearPt, 0.f ) )
		return false;
	if( !Get3DPointFromScreen( screenPt, farPt, 1.f ) )
		return false;
	CLine3D ray( nearPt, farPt );

	//now find out where in 3D space the ray passes through the z=0 plane
	CVector3D xyNormal( 0., 0., 1. );
	CPoint3D ptOnXYPlane( 0., 0., 0. );
	CPlane3D xyPlane( xyNormal, ptOnXYPlane );
	double denom = xyNormal.dot( CVector3D( farPt, nearPt ) );
	if( zero( denom ) ) return false;
	double num = xyNormal.dot( CVector3D( ptOnXYPlane, nearPt  ) );
	double s = num/denom;
	worldPt = ray.offset( s );
	
	if( true/*is world point valid*/ )
		return true;
	else return false;
}

void gkCameraNode::calculateNewPosition(const gkVector3& currentPosition, gkScalar rayLength, gkScalar tick)
{
	gkVector3 oDir = gkVector3::NEGATIVE_UNIT_Z * rayLength;

	gkVector3 tmpPosition = m_center - m_target->getOrientation() * oDir;

	bool newPosSet = false;

	if (GET_SOCKET_VALUE(AVOID_BLOCKING))
	{
		gkVector3 direction = tmpPosition - m_center;

		Ogre::Ray ray(m_center, direction);

		gkSweptTest::AVOID_LIST avoidList;
		avoidList.push_back(m_centerObj->getPhysicsController()->getCollisionObject());

		gkSweptTest sweptTest(avoidList);

		gkScalar blokingRadius = GET_SOCKET_VALUE(BLOCKING_RADIUS);

		if (sweptTest.collides(ray, blokingRadius))
		{
			gkVector3 displacement = (sweptTest.getHitPoint() - currentPosition) * 0.9f;

			m_target->setPosition(currentPosition + (displacement + sweptTest.getSliding()) * tick);

			newPosSet = true;
		}
	}

	if (!newPosSet)
	{
		m_target->setPosition(tmpPosition);
	}
}

/** genRay **/
vector_t genRay(scene_t *scene, int column, int row) {
   vector_t direction;                       // Directior vector
   entity_t    *ent;
   window_t *window;

   assert(scene->magic == SCENE_T);
   ent = scene->window;
   window = ent->entDerived;
   assert(window->magic == WINDOW_T);

   /* Computer the pixel's real scene coordinates */
   direction.x = ((double)(column)/
      (double)(scene->picture->columns-1))*window->windowWidth;
   direction.x -= window->windowWidth/2.0;
   direction.y = ((double)(row)/
      (double)(scene->picture->rows-1))*window->windowHeight;
   direction.y -= window->windowHeight/2.0;
   direction.z = 0;

   /* And now construct a unit vector from the view point to the pixel */
   direction = ray(window->viewPoint, direction);
   direction = unitize(direction);
   return(direction);
} /* End genRay */

  TEST_F(Ray3fTest, MethodIntersectPlane)
  {
    // Intersect.
    Ray3 ray(Vec3f(0, 0, 0), Vec3f(0, 0, 1));
    Plane plane(Vec3f(0, 0, 10), Vec3f(0, 0, -1));
    Vec3f intersection;
    bool intersect = ray.intersect(plane, intersection);
    float result[3];
    result[0] = 0;
    result[1] = 0;
    result[2] = 10;
    ASSERT_TRUE(intersect);
    cmpVec3f(result, intersection);

    // Ray pointing away.
    ray = Ray3(Vec3f(0, 0, 0), Vec3f(0, 0, -1));
    plane = Plane(Vec3f(0, 0, 10), Vec3f(0, 0, -1));
    intersect = ray.intersect(plane, intersection);
    ASSERT_TRUE(!intersect);

    // Normal same direction as ray.
    ray = Ray3(Vec3f(0, 0, 0), Vec3f(0, 0, 1));
    plane = Plane(Vec3f(0, 0, 10), Vec3f(0, 0, 1));
    intersect = ray.intersect(plane, intersection);
    result[0] = 0;
    result[1] = 0;
    result[2] = 10;
    ASSERT_TRUE(intersect);
    cmpVec3f(result, intersection);

    // Parallel.
    ray = Ray3(Vec3f(0, 0, 0), Vec3f(0, 0, 1));
    plane = Plane(Vec3f(0, 0, 10), Vec3f(0, 1, 0));
    intersect = ray.intersect(plane, intersection);
    ASSERT_TRUE(!intersect);
  }

bool BaseTank::isCollision(const Ogre::Vector3& position, const Ogre::Vector3& direction, Ogre::Real RaySize)
{
	//根据初始位置和方向创建一条射线;
	Ogre::Ray ray(position, direction);
	m_pRaySceneQuery->setRay(ray);
	//获取射线查询结果;
	Ogre::RaySceneQueryResult	&result = m_pRaySceneQuery->execute();
	Ogre::RaySceneQueryResult::iterator ite;
	//遍历查询结果，对每个结果做相应的操作;
	for( ite = result.begin(); ite!=result.end(); ite++)
	{
		//如果在指定距离的射线范围内查找到实体，则返回true;
		Ogre::String p=m_pBodyEntity->getName();
		if (ite->movable->getName().compare(m_pBodyEntity->getName()) != 0
			&& ite->distance < RaySize)
		{
			if (ite->movable->getName() != "PlayingCamera")
			{
					return true;
			}
		}
	}
	return false;
}

void Engine::Calculate()
{
    const int max = Width * Height;
    const double cellWidth = 0.005;
    const double cellHeight = 0.005;
    const Vector3 origin(0.0,0.0,-5.0);

    for (int i = 0; i < Width; i++)
    {
        for (int j = 0; j < Height; j++)
        {
            Vector3 a;
            a.x = 1.0 * cellWidth * (i - Width/2);
            a.y = -1.0 * cellHeight * (j - Height/2);
            a.z = 0.0;

            a = a - origin;
            a.Normalize();

            Ray ray(origin, a);
            m_image[i][j] = trace(m_scene, ray, 0);
            m_progress++;

            int part = (m_progress*100/max);
            if (part*max == (m_progress*100))
            {
                if ((part/5)*5 == part)
                {
                    std::ostringstream caption;
                    caption << part << "%";
                    m_screen.SetCaption(caption.str());
                }
            }
        }
    }
}

Point Polygon::FarthestPointAtAngle(real angle) const
{
    // TODO(mraggi): Replace with Binary search implementation
    int n = NumPoints();

    for (int i = 0; i < n; ++i)
    {
        Point p1 = m_vPoints[i];
        Point p2 = m_vPoints[(i + 1)%n];
        real a1 = p1.Angle();
        real a2 = p2.Angle();

        if (isAngleBetweenAngles(angle, a1, a2))
        {
            Point hola;
            Ray ray(Ray(Point(0, 0), angle));
            ray.Intersects(Segment(p1, p2), hola);
            return hola + Position();
        }
    }
    if (NumPoints() > 2)
        std::cerr << "ERROR IN Polygon::FarthestPointAtAngle" << std::endl;
    return {0, 0};
}

 void PathTracingRenderer::Job::kernel(uint32_t threadID) {
     ArenaAllocator &mem = mems[threadID];
     IndependentLightPathSampler &pathSampler = *pathSamplers[threadID];
     for (int ly = 0; ly < numPixelY; ++ly) {
         for (int lx = 0; lx < numPixelX; ++lx) {
             float time = pathSampler.getTimeSample(timeStart, timeEnd);
             PixelPosition p = pathSampler.getPixelPositionSample(basePixelX + lx, basePixelY + ly);
             
             float selectWLPDF;
             WavelengthSamples wls = WavelengthSamples::createWithEqualOffsets(pathSampler.getWavelengthSample(), pathSampler.getWLSelectionSample(), &selectWLPDF);
             
             LensPosQuery lensQuery(time, wls);
             LensPosQueryResult lensResult;
             SampledSpectrum We0 = camera->sample(lensQuery, pathSampler.getLensPosSample(), &lensResult);
             
             IDFSample WeSample(p.x / imageWidth, p.y / imageHeight);
             IDFQueryResult WeResult;
             IDF* idf = camera->createIDF(lensResult.surfPt, wls, mem);
             SampledSpectrum We1 = idf->sample(WeSample, &WeResult);
             
             Ray ray(lensResult.surfPt.p, lensResult.surfPt.shadingFrame.fromLocal(WeResult.dirLocal), time);
             SampledSpectrum C = contribution(*scene, wls, ray, pathSampler, mem);
             SLRAssert(C.hasNaN() == false && C.hasInf() == false && C.hasMinus() == false,
                       "Unexpected value detected: %s\n"
                       "pix: (%f, %f)", C.toString().c_str(), px, py);
             
             SampledSpectrum weight = (We0 * We1) * (absDot(ray.dir, lensResult.surfPt.gNormal) / (lensResult.areaPDF * WeResult.dirPDF * selectWLPDF));
             SLRAssert(weight.hasNaN() == false && weight.hasInf() == false && weight.hasMinus() == false,
                       "Unexpected value detected: %s\n"
                       "pix: (%f, %f)", weight.toString().c_str(), px, py);
             sensor->add(p.x, p.y, wls, weight * C);
             
             mem.reset();
         }
     }
 }

const std::shared_ptr<std::vector<ray>> mesh_light::shed(unsigned long samples) const {
	random_sampler s;
	std::vector<float> face_selector = *s.get_1d_samples(0.0f, 1.0f, samples);
	std::vector<vec2> c = *s.get_2d_samples(0, 1, 0, 1, samples);
	std::shared_ptr<std::vector<ray>> out(new std::vector<ray>());
	for (unsigned long i = 0; i < c.size(); i++) {
		for (unsigned long f = 0; f < cr_areas.size(); f++) {
			if (face_selector.at(i) < cr_areas[f]) {
				std::shared_ptr<triangle> face = faces->at(f);
				std::shared_ptr<vec2> lcoord(new vec2(c[i][0] + c[i][1] > 1 ? 1 - c[i][1] : c[i][0],
													  c[i][0] + c[i][1] > 1 ? 1 - c[i][0] : c[i][1]));
				std::shared_ptr<position> lpos = std::make_shared<position>(object_to_world(
						*face->get_barycentric_position(1 - (*lcoord)[0] - (*lcoord)[1], (*lcoord)[0], (*lcoord)[1]) +
						offset));
				std::array<position, 3> & v = *face->get_vertices();
				normal n = normalise(cross(object_to_world(v[1]-v[0]), object_to_world(v[2]-v[0])));
				if (dot(n, object_to_world(*face->get_avg_normal())) < 0)
					n = -n;
				out->push_back(ray(*lpos, s.get_solid_angle_samples(n, static_cast<float>(M_PI / 2), 1)->at(0)));
			}
		}
	}
	return out;
}

void kdtreebenthin::draw<1>(scene& scene, ray4* r, hit4* hit4)
{

        unsigned int signx = movemask(r->D().x());
        unsigned int signy = movemask(r->D().y());
        unsigned int signz = movemask(r->D().z());

        //If the traversal direction is not same for all rays we 
        //  do a single ray traversal
        if (((signx - 1) < 14)       // sign of x is 0xF or 0
            || ((signy - 1) < 14)    // sign of y is 0xF or 0
            || ((signz - 1) < 14)) { // sign of z is 0xF or 0
                hit hit[4];
                for (int i = 0; i < 4; ++i) {
                        vec3f d(r->D().x()[i], r->D().y()[i], r->D().z()[i]);
                        ray ray(r->O(), d);
                        hit[i].prim = -1;
                        draw(scene, ray, hit[i]);
                }
                hit4->prim = ssei(hit[0].prim, hit[1].prim, hit[2].prim, hit[3].prim);
                hit4->u    = ssef(hit[0].u, hit[1].u, hit[2].u, hit[3].u);
                hit4->v    = ssef(hit[0].v, hit[1].v, hit[2].v, hit[3].v);
                return;
        }

        ssef tnear, tfar;
        _boundingBox.clip(*r, tnear, tfar);
        if (movemask(tnear >= tfar) == 0xF)
                return;

        const unsigned int dir[3][2] = {
                { signx & 1 , 1 - (signx & 1) },
                { signy & 1 , 1 - (signy & 1) },
                { signz & 1 , 1 - (signz & 1) } };

        ssef far[MAX_STACK_SIZE];
        ssef near[MAX_STACK_SIZE];
        int  nodes[MAX_STACK_SIZE];

        //push dummyNode onto stack which will cause us to exit
        nodes[0] = 0;
        far[0]  = BPRAY_INF;

        uint32_t stackptr = 1;
        kdnode* currNode = _nodes + 1;

        int activemask = 0xF;
#if MAILBOX
        static uint64_t rayid = 0;
        __sync_add_and_fetch(&rayid, 1);
#endif
        while (true) {
                if (!currNode->isLeaf()) {
                        const int axis  = currNode->getAxis();
                        const int front = currNode->getLeft() + dir[axis][0];
                        const int back  = currNode->getLeft() + dir[axis][1];
                        const ssef dist = currNode->getSplit() - r->O()[axis];
                        const ssef t    = dist * r->rcpD()[axis];

                        currNode   = _nodes + back;
                        if (!(movemask(tnear <= t) & activemask)) continue;

                        currNode   = _nodes + front;
                        if (!(movemask(tfar >= t) & activemask))  continue;

                        nodes[stackptr] = back;
                        near[stackptr]  = max(tnear, t);
                        far[stackptr]   = tfar;
                        tfar            = min(tfar, t);
                        activemask     &= movemask(tnear <= tfar);
                        ++stackptr;
                } else {
                        int primidx   = currNode->getPrimitiveOffset();
                        int primcount = currNode->getNumPrims();

                        for (int i = 0; i != primcount; ++i) {
                                int t = _prims[primidx + i];

                                //prefetch
                                int t2 = _prims[primidx + i + 1];
                                _mm_prefetch((char*)&scene._accels[t2], _MM_HINT_T0);
#if MAILBOX
                                //mailboxing
                                if (mbox.find(scene, rayid, t)) continue;
#endif
                                scene.intersect(t, *r, *hit4);
#if MAILBOX
                                mbox.add(scene, rayid, t);
#endif
                        }

                        if (movemask(tfar < r->tfar) == 0) return;
                        
                        --stackptr;
                        currNode   = nodes[stackptr] + _nodes;
                        tfar       = far[stackptr];
                        tnear      = near[stackptr];
                        activemask = movemask(tnear <= tfar);
                }
        }
//.........这里部分代码省略.........

dgInt32 dgCollisionConvexPolygon::CalculateContactToConvexHullContinue (dgCollisionParamProxy& proxy, const dgVector& polyInstanceScale, const dgVector& polyInstanceInvScale)
{
    dgAssert (proxy.m_referenceCollision->IsType (dgCollision::dgCollisionConvexShape_RTTI));
    dgAssert (proxy.m_floatingCollision->IsType (dgCollision::dgCollisionConvexPolygon_RTTI));

    const dgCollisionInstance* const hull = proxy.m_referenceCollision;

    dgAssert (this == proxy.m_floatingCollision->GetChildShape());
    dgAssert (m_count);
    dgAssert (m_count < dgInt32 (sizeof (m_localPoly) / sizeof (m_localPoly[0])));

    const dgBody* const floatingBody = proxy.m_floatingBody;
    const dgBody* const referenceBody = proxy.m_referenceBody;

    dgContact* const contactJoint = proxy.m_contactJoint;
    contactJoint->m_closestDistance = dgFloat32 (1.0e10f);

    m_normal = m_normal.CompProduct4(polyInstanceInvScale);
    dgAssert (m_normal.m_w == dgFloat32 (0.0f));
    m_normal = m_normal.CompProduct4(m_normal.DotProduct4(m_normal).InvSqrt());
    const dgVector savedFaceNormal (m_normal);

    for (dgInt32 i = 0; i < m_count; i ++) {
        m_localPoly[i] = polyInstanceScale.CompProduct4(dgVector (&m_vertex[m_vertexIndex[i] * m_stride]));
        dgAssert (m_localPoly[i].m_w == dgFloat32 (0.0f));
    }

    dgVector hullOrigin (proxy.m_matrix.UntransformVector(dgVector (dgFloat32 (0.0f))));
    hullOrigin = (hullOrigin - m_normal.CompProduct4(m_normal.DotProduct4(hullOrigin - m_localPoly[0]))) | dgVector::m_wOne;

    dgMatrix polygonMatrix;
    polygonMatrix[0] = m_localPoly[1] - m_localPoly[0];
    polygonMatrix[0] = polygonMatrix[0].CompProduct4 (polygonMatrix[0].InvMagSqrt());
    polygonMatrix[1] = m_normal;
    polygonMatrix[2] = polygonMatrix[0] * m_normal;
    polygonMatrix[3] = hullOrigin;
    dgAssert (polygonMatrix.TestOrthogonal());

    dgMatrix savedProxyMatrix (proxy.m_matrix);
    proxy.m_matrix = polygonMatrix * proxy.m_matrix;

    dgVector floatingVeloc (floatingBody->m_veloc);
    dgVector referenceVeloc (referenceBody->m_veloc);
    const dgMatrix& hullMatrix = hull->GetGlobalMatrix();
    dgVector hullRelativeVeloc (hullMatrix.UnrotateVector(referenceVeloc - floatingVeloc));
    dgVector polyRelativeVeloc (proxy.m_matrix.UnrotateVector (hullRelativeVeloc));

    dgVector polyBoxP0 (dgFloat32 ( 1.0e15f));
    dgVector polyBoxP1 (dgFloat32 (-1.0e15f));
    m_normal = polygonMatrix.UnrotateVector(m_normal);

    if (m_normal.DotProduct4(polyRelativeVeloc).m_x >= 0.0f) {
        proxy.m_matrix = savedProxyMatrix;
        return 0;
    }
    for (dgInt32 i = 0; i < m_count; i ++) {
        m_localPoly[i] = polygonMatrix.UntransformVector(m_localPoly[i]);
        dgAssert (m_localPoly[i].m_w == dgFloat32 (0.0f));
        polyBoxP0 = polyBoxP0.GetMin (m_localPoly[i]);
        polyBoxP1 = polyBoxP1.GetMax (m_localPoly[i]);
    }
    dgInt32 count = 0;


    dgVector hullBoxP0;
    dgVector hullBoxP1;
    hull->CalcAABB (proxy.m_matrix.Inverse(), hullBoxP0, hullBoxP1);
    dgVector minBox (polyBoxP0 - hullBoxP1);
    dgVector maxBox (polyBoxP1 - hullBoxP0);
    dgFastRayTest ray (dgVector (dgFloat32 (0.0f)), polyRelativeVeloc);
    dgFloat32 distance = ray.BoxIntersect(minBox, maxBox);

    if (distance < dgFloat32 (1.0f)) {

        dgVector boxSize ((hullBoxP1 - hullBoxP0).Scale4 (dgFloat32 (0.5f)));
//		dgVector boxOrigin ((hullBoxP1 + hullBoxP0).Scale4 (dgFloat32 (0.5f)));
//		boxOrigin += polyRelativeVeloc.Scale4 (distance);

        dgVector normalInHull (proxy.m_matrix.RotateVector (m_normal.Scale4 (dgFloat32 (-1.0f))));
        dgVector pointInHull (hull->SupportVertex (normalInHull, NULL));
        dgVector pointInPlane (proxy.m_matrix.UntransformVector (pointInHull));
        dgFloat32 distToPlane = (m_localPoly[0] - pointInPlane) % m_normal;
        dgFloat32 timeToPlane = distToPlane / (polyRelativeVeloc % m_normal);
        dgVector boxOrigin (pointInPlane + polyRelativeVeloc.Scale4(timeToPlane));

        bool inside = true;
        dgInt32 i0 = m_count - 1;
        for (dgInt32 i = 0; i < m_count; i ++) {
            dgVector e (m_localPoly[i] - m_localPoly[i0]);
            dgVector n (m_normal * e);
            dgPlane plane (n, - (m_localPoly[i0] % n));

            dgVector supportDist (plane.Abs().DotProduct4 (boxSize));
            dgFloat32 centerDist = plane.Evalue(boxOrigin);

            if ((centerDist + supportDist.m_x) < dgFloat32 (0.0f)) {
                proxy.m_matrix = savedProxyMatrix;
                return 0;
            }

//.........这里部分代码省略.........

dgInt32 dgCollisionConvexPolygon::CalculateContactToConvexHullContinue(const dgWorld* const world, const dgCollisionInstance* const parentMesh, dgCollisionParamProxy& proxy)
{
	dgAssert(proxy.m_instance0->IsType(dgCollision::dgCollisionConvexShape_RTTI));
	dgAssert(proxy.m_instance1->IsType(dgCollision::dgCollisionConvexPolygon_RTTI));

	dgAssert(this == proxy.m_instance1->GetChildShape());
	dgAssert(m_count);
	dgAssert(m_count < dgInt32(sizeof (m_localPoly) / sizeof (m_localPoly[0])));

	const dgBody* const body0 = proxy.m_body0;
	const dgBody* const body1 = proxy.m_body1;

	dgAssert (proxy.m_instance1->GetGlobalMatrix().TestIdentity());

	dgVector relativeVelocity (body0->m_veloc - body1->m_veloc);
	if (m_normal.DotProduct4(relativeVelocity).GetScalar() >= 0.0f) {
		return 0;
	}
	dgFloat32 den = dgFloat32 (1.0f) / (relativeVelocity % m_normal);
	if (den > dgFloat32 (1.0e-5f)) {
		// this can actually happens
		dgAssert(0);
		return 0;
	}

	dgContact* const contactJoint = proxy.m_contactJoint;
	contactJoint->m_closestDistance = dgFloat32(1.0e10f);

	dgMatrix polygonMatrix;
	dgVector right (m_localPoly[1] - m_localPoly[0]);
	polygonMatrix[0] = right.CompProduct4(right.InvMagSqrt());
	polygonMatrix[1] = m_normal;
	polygonMatrix[2] = polygonMatrix[0] * m_normal;
	polygonMatrix[3] = dgVector::m_wOne;
	dgAssert (polygonMatrix.TestOrthogonal());

	dgVector polyBoxP0(dgFloat32(1.0e15f));
	dgVector polyBoxP1(dgFloat32(-1.0e15f));
	for (dgInt32 i = 0; i < m_count; i++) {
		dgVector point (polygonMatrix.UnrotateVector(m_localPoly[i]));
		polyBoxP0 = polyBoxP0.GetMin(point);
		polyBoxP1 = polyBoxP1.GetMax(point);
	}

	dgVector hullBoxP0;
	dgVector hullBoxP1;
	dgMatrix hullMatrix (polygonMatrix * proxy.m_instance0->m_globalMatrix);
	proxy.m_instance0->CalcAABB(hullMatrix, hullBoxP0, hullBoxP1);
	dgVector minBox(polyBoxP0 - hullBoxP1);
	dgVector maxBox(polyBoxP1 - hullBoxP0);
	dgVector veloc (polygonMatrix.UnrotateVector (relativeVelocity));
	dgFastRayTest ray(dgVector(dgFloat32(0.0f)), veloc);
 	dgFloat32 distance = ray.BoxIntersect(minBox, maxBox);

	dgInt32 count = 0;
	if (distance < dgFloat32(1.0f)) {
		bool inside = false;

		dgVector boxSize((hullBoxP1 - hullBoxP0).CompProduct4(dgVector::m_half));
		dgVector sphereMag2 (boxSize.DotProduct4(boxSize));
		boxSize = sphereMag2.Sqrt();

		dgVector pointInPlane (polygonMatrix.RotateVector(hullBoxP1 + hullBoxP0).CompProduct4(dgVector::m_half));
		dgFloat32 distToPlane = (m_localPoly[0] - pointInPlane) % m_normal;

		dgFloat32 timeToPlane0 = (distToPlane + boxSize.GetScalar()) * den;
		dgFloat32 timeToPlane1 = (distToPlane - boxSize.GetScalar()) * den;

		dgVector boxOrigin0 (pointInPlane + relativeVelocity.Scale4(timeToPlane0));
		dgVector boxOrigin1 (pointInPlane + relativeVelocity.Scale4(timeToPlane1));
		dgVector boxOrigin ((boxOrigin0 + boxOrigin1).CompProduct4(dgVector::m_half)); 
		dgVector boxProjectSize (((boxOrigin0 - boxOrigin1).CompProduct4(dgVector::m_half))); 
		sphereMag2 = boxProjectSize.DotProduct4(boxProjectSize);
		boxSize = sphereMag2.Sqrt();

		dgAssert (boxOrigin.m_w == 0.0f);
		boxOrigin = boxOrigin | dgVector::m_wOne;
		
		if (!proxy.m_intersectionTestOnly) {
			inside = true;
			dgInt32 i0 = m_count - 1;

			for (dgInt32 i = 0; i < m_count; i++) {
				dgVector e(m_localPoly[i] - m_localPoly[i0]);
				dgVector n(m_normal * e & dgVector::m_triplexMask);
				dgFloat32 param = dgSqrt (sphereMag2.GetScalar() / (n.DotProduct4(n)).GetScalar());
				dgPlane plane(n, -(m_localPoly[i0] % n));

				dgVector p0 (boxOrigin + n.Scale4 (param));
				dgVector p1 (boxOrigin - n.Scale4 (param));

				dgFloat32 size0 = (plane.DotProduct4 (p0)).GetScalar();
				dgFloat32 size1 = (plane.DotProduct4 (p1)).GetScalar();

				if ((size0 < 0.0f) && (size1 < 0.0f)) {
					return 0;
				}

				if ((size0 * size1) < 0.0f) {
					inside = false;
//.........这里部分代码省略.........

vec3f RayTracer::traceRay( Scene *scene, const ray& r, 
	const vec3f& thresh, int depth, isect& i, vector<const SceneObject*>& stack )
{
	if( depth>=0
		&& thresh[0] > threshold - RAY_EPSILON && thresh[1] > threshold - RAY_EPSILON && thresh[2] > threshold - RAY_EPSILON
		&& scene->intersect( r, i ) ) {
		// YOUR CODE HERE

		// An intersection occured!  We've got work to do.  For now,
		// this code gets the material for the surface that was intersected,
		// and asks that material to provide a color for the ray.  

		// This is a great place to insert code for recursive ray tracing.
		// Instead of just returning the result of shade(), add some
		// more steps: add in the contributions from reflected and refracted
		// rays.
		
		const Material& m = i.getMaterial();
		vec3f color = m.shade(scene, r, i);
		//calculate the reflected ray
		vec3f d = r.getDirection();
		vec3f position = r.at(i.t);
		vec3f direction = d - 2 * i.N * d.dot(i.N);
		ray newray(position, direction);
		if(!m.kr.iszero()) {
			vec3f reflect = m.kr.multiply(traceRay(scene, newray, thresh.multiply(m.kr), depth-1, stack).clamp());
			color += reflect;
		}

		//calculate the refracted ray
		double ref_ratio;
		double sin_ang = d.cross(i.N).length();
		vec3f N = i.N;
		//Decide going in or out
		const SceneObject *mi = NULL, *mt = NULL;
		int stack_idx = -1;
		vector<const SceneObject*>::reverse_iterator itr;
		//1 use the normal to decide whether to go in or out
		//0: travel through, 1: in, 2: out
		char travel = 0;
		if(i.N.dot(d) <= -RAY_EPSILON) {
			//from outer surface in
			//test whether the object has two face
			ray test_ray(r.at(i.t) + d * 2 * RAY_EPSILON, -d);
			isect test_i;
			if(i.obj->intersect(r, test_i) && test_i.N.dot(N) > -RAY_EPSILON) {
				//has interior
				travel = 1;
			}
		}
		else {
			travel = 2;
		}

		if(travel == 1) {
			if(!stack.empty()) {
				mi = stack.back();
			}
			mt = i.obj;
			stack.push_back(mt);
		}
		else if(travel == 2) {
			//if it is in our stack, then we must pop it
			for(itr = stack.rbegin(); itr != stack.rend(); ++itr) {
				if(*itr == i.obj) {
					mi = *itr;
					vector<const SceneObject*>::iterator ii = itr.base() - 1;
					stack_idx = ii - stack.begin();
					stack.erase(ii);
					break;
				}
			}
			if(!stack.empty()) {
				mt = stack.back();
			}
		}

		if(N.dot(d) >= RAY_EPSILON) {
			N = -N;
		}
		
		ref_ratio = (mi?(mi->getMaterial().index):1.0) / (mt?(mt->getMaterial().index):1.0);

		if(!m.kt.iszero() && (ref_ratio < 1.0 + RAY_EPSILON || sin_ang < 1.0 / ref_ratio + RAY_EPSILON)) {
			//No total internal reflection
			//We do refraction now
			double c = N.dot(-d);
			direction = (ref_ratio * c - sqrt(1 - ref_ratio * ref_ratio * (1 - c * c))) * N + ref_ratio * d;
			newray = ray(position, direction);
			vec3f refraction = m.kt.multiply(traceRay(scene, newray, thresh.multiply(m.kt), depth-1, stack).clamp());
			color += refraction;
		}

		if(travel == 1) {
			stack.pop_back();
		}
		else if(travel == 2) {
			if(mi) {
				stack.insert(stack.begin() + stack_idx, mi);
			}
//.........这里部分代码省略.........

bool UniformGrid::intersectsCell(const Model& model, const CellCoord& coord) {
  // Left side
  // Bottom left point
  Point3D p0 = pointAt(coord);
  Point3D p1(p0[0], p0[1] + cellSize, p0[2]);
  Point3D p2(p0[0], p0[1] + cellSize, p0[2] + cellSize);
  Point3D p3(p0[0], p0[1], p0[2] + cellSize);

  // Right side
  Point3D p4(p0[0] + cellSize, p0[1], p0[2]);
  Point3D p5(p0[0] + cellSize, p0[1] + cellSize, p0[2]);
  Point3D p6(p0[0] + cellSize, p0[1] + cellSize, p0[2] + cellSize);
  Point3D p7(p0[0] + cellSize, p0[1], p0[2] + cellSize);

  const std::vector<Point3D> pts = {p0, p1, p2, p3, p4, p5, p6, p7};

  auto cellMat = translationMatrix(p0[0], p0[1], p0[2]) * cellSizeScaleMatrix;
  // But we need the inverse of course
  cellMat = cellMat.invert();

  // Check if a pt is in the cell
  auto inCell = [&] (const Point3D& pt) -> bool {
    return p0[0] <= pt[0] && pt[0] <= (p0[0] + cellSize) &&
           p0[1] <= pt[1] && pt[1] <= (p0[1] + cellSize) &&
           p0[2] <= pt[2] && pt[2] <= (p0[2] + cellSize);
  };

  // First, we need to get the 8 points of the bounding box
  auto bbox = model.getBoundingBox();

  auto inBoundingBox = [&bbox] (const Point3D& pt) -> bool {
    // We are in the box if we are in between all the opposite parallel planes
    const auto c1 = bbox[0]; // Bottom back left corner
    const auto v1a = bbox[1] - c1; // Bottom back left to bottom back right
    const auto v1b = bbox[3] - c1; // Bottom back left to top back left
    const auto v1c = bbox[4] - c1; // Bottom back left to bottom front left
    const auto n1 = v1a.cross(v1b); // Back face
    const auto n2 = v1b.cross(v1c); // Left face
    const auto n3 = v1c.cross(v1a); // Bottom face

    const auto c2 = bbox[6]; // Top front right corner
    const auto v2a = bbox[5] - c2; // Top front right to bottom front right
    const auto v2b = bbox[7] - c2; // Top front right to top front left
    const auto v2c = bbox[2] - c2; // Top front right to top back right
    // We want this to be opposite sign (i.e. not pointing inwards)
    // so we do the opposite cross as above
    const auto n4 = v2b.cross(v2a); // Front face
    const auto n5 = v2c.cross(v2b); // Top face
    const auto n6 = v2a.cross(v2c); // Right face

    return betweenPlanes(n1, c1, n4, c2, pt) &&
           betweenPlanes(n2, c1, n6, c2, pt) &&
           betweenPlanes(n3, c1, n5, c2, pt);
  };

  // A corner of the bbox being inside the cell implies an intersection
  // between the bbox and the cell.
  for (const auto& pt : bbox) {
    if (inCell(pt)) {
      return true;
    }
  }

  // Similarly, a corner of cell inside bbox implies intersection
  for (const auto& pt : pts) {
    if (inBoundingBox(pt)) {
      return true;
    }
  }

  // Check if any of the 12 lines from bbox intersect this cell
  HitRecord hr;
  for (size_t i = 0; i < 8; ++i) {
    // This is the vector of one edge
    Vector3D v = bbox[(i % 4 == 0) ? i + 3 : i - 1] - bbox[i];
    Ray ray(bbox[i] - v, bbox[i]);
    if (utilityCube.intersects(ray, &hr, cellMat) && 1 <= hr.t && hr.t <= 2) {
      // This edge of the bounding box intersects our cell cube.
      return true;
    }
  }
  for (size_t i = 0; i < 4; ++i) {
    Vector3D v = bbox[i + 4] - bbox[i];
    Ray ray(bbox[i] - v, bbox[i]);
    if (utilityCube.intersects(ray, &hr, cellMat) && 1 <= hr.t && hr.t <= 2) {
      // This edge of the bounding box intersects our cell cube.
      return true;
    }
  }

  // Now check if any of the 12 lines from this cell intersect the model
  for (size_t i = 0; i < pts.size(); ++i) {
    Vector3D v = pts[(i % 4 == 0) ? i + 3 : i - 1] - pts[i];
    // Note: We are doing pts[i] - v and checking for t between 1 and 2.
    // This is equivalent to checking between 0 and 1 without doing the
    // subtraction, *but* we have an epsilon check in the intersects code.
    // For this case, we do *not* want to bother with epsilon check, so we
    // will check from 1 to 2 to avoid it.
    if (model.intersects(Ray(pts[i] - v, pts[i]), &hr)) {
      if (1 <= hr.t && hr.t <= 2) {
//.........这里部分代码省略.........

/**
 *  Causes the object to be positioned in front of the tank every
 *  frame.
 *
 *  Adds all non-sensor geoms from object to the tank's
 *  body. Collision callbacks for tank are not installed on the picked
 *  up object!
 */
bool Tank::pickupObject(RigidBody * object)
{
    assert(!carried_object_);
    
    carried_object_ = object;
    Vector docking_offset = params_.get<Vector>("tank.docking_pos");

    if (getLocation() == CL_SERVER_SIDE)
    {
        // First we have to check whether the LOS to the object is given.

        // Use turret pos because tank position will likely be below terrain...
        Vector tank_pos    = target_object_->getPosition() + target_object_->vecToWorld(turret_pos_);
        Vector docking_pos = target_object_->getPosition() + target_object_->vecToWorld(docking_offset);
        Vector ab = docking_pos - tank_pos;

        pickup_los_given_ = true;
    
        physics::OdeRayGeom ray(ab.length());
        ray.set(tank_pos, ab);
        target_object_->getSimulator()->getStaticSpace()->collide(
            &ray, physics::CollisionCallback(this, &Tank::pickupRayCollisionCallback));
        if (pickup_los_given_)
        {
            target_object_->getSimulator()->getActorSpace()->collide(
                &ray, physics::CollisionCallback(this, &Tank::pickupRayCollisionCallback));
        }

        if (!pickup_los_given_)
        {
            carried_object_ = NULL;
            return false;
        }
    }
    
    s_log << Log::debug('l')
          << *this
          << " now carries "
          << *object
          << "\n";

    physics::OdeRigidBody * obj_body  = object->getProxy() ? object->getProxy() : object->getTarget();
    physics::OdeRigidBody * this_body = getProxy()         ? getProxy()         : getTarget();

    Matrix offset(true);
    offset.getTranslation() = docking_offset;
    
    for (unsigned g=0; g<obj_body->getGeoms().size(); ++g)
    {
        if (obj_body->getGeoms()[g]->isSensor()) continue;
        
        physics::OdeGeom * clone = obj_body->getGeoms()[g]->instantiate();

        clone->setName(clone->getName() + "-clone");
        clone->setMass(0.0f);
        clone->setOffset(offset);
        clone->setSpace(this_body->getSimulator()->getActorSpace());
        
        this_body->addGeom(clone);

        clone->setCategory(obj_body->getGeoms()[g]->getCategory());

        ++num_carried_object_geoms_;
    }

    return true;
}

// VolPathIntegrator Method Definitions
Spectrum VolPathIntegrator::Li(const RayDifferential &r, const Scene &scene,
                               Sampler &sampler, MemoryArena &arena,
                               int depth) const {
    ProfilePhase p(Prof::SamplerIntegratorLi);
    Spectrum L(0.f), alpha(1.f);
    RayDifferential ray(r);
    bool specularBounce = false;
    for (int bounces = 0;; ++bounces) {
        // Store intersection into _isect_
        SurfaceInteraction isect;
        bool foundIntersection = scene.Intersect(ray, &isect);

        // Sample the participating medium, if present
        MediumInteraction mi;
        if (ray.medium) alpha *= ray.medium->Sample(ray, sampler, arena, &mi);
        if (alpha.IsBlack()) break;

        // Handle an interaction with a medium or a surface
        if (mi.IsValid()) {
            // Handle medium scattering case
            Vector3f wo = -ray.d, wi;
            L += alpha * UniformSampleOneLight(mi, scene, sampler, arena, true);
            Point2f phaseSample = sampler.Get2D();
            mi.phase->Sample_p(wo, &wi, phaseSample);
            ray = mi.SpawnRay(wi);
        } else {
            // Handle surface scattering case

            // Possibly add emitted light and terminate
            if (bounces == 0 || specularBounce) {
                // Add emitted light at path vertex or from the environment
                if (foundIntersection)
                    L += alpha * isect.Le(-ray.d);
                else
                    for (const auto &light : scene.lights)
                        L += alpha * light->Le(ray);
            }
            if (!foundIntersection || bounces >= maxDepth) break;

            // Compute scattering functions and skip over medium boundaries
            isect.ComputeScatteringFunctions(ray, arena, true);
            if (!isect.bsdf) {
                ray = isect.SpawnRay(ray.d);
                bounces--;
                continue;
            }

            // Sample illumination from lights to find attenuated path
            // contribution
            L += alpha *
                 UniformSampleOneLight(isect, scene, sampler, arena, true);

            // Sample BSDF to get new path direction
            Vector3f wo = -ray.d, wi;
            Float pdf;
            BxDFType flags;
            Spectrum f = isect.bsdf->Sample_f(wo, &wi, sampler.Get2D(), &pdf,
                                              BSDF_ALL, &flags);
            if (f.IsBlack() || pdf == 0.f) break;
            alpha *= f * AbsDot(wi, isect.shading.n) / pdf;
            Assert(std::isinf(alpha.y()) == false);
            specularBounce = (flags & BSDF_SPECULAR) != 0;
            ray = isect.SpawnRay(wi);

            // Account for attenuated subsurface scattering, if applicable
            if (isect.bssrdf && (flags & BSDF_TRANSMISSION)) {
                // Importance sample the BSSRDF
                SurfaceInteraction pi;
                Spectrum S = isect.bssrdf->Sample_S(
                    scene, sampler.Get1D(), sampler.Get2D(), arena, &pi, &pdf);
#ifndef NDEBUG
                Assert(std::isinf(alpha.y()) == false);
#endif
                if (S.IsBlack() || pdf == 0) break;
                alpha *= S / pdf;

                // Account for the attenuated direct subsurface scattering
                // component
                L += alpha *
                     UniformSampleOneLight(pi, scene, sampler, arena, true);

                // Account for the indirect subsurface scattering component
                Spectrum f = pi.bsdf->Sample_f(pi.wo, &wi, sampler.Get2D(),
                                               &pdf, BSDF_ALL, &flags);
                if (f.IsBlack() || pdf == 0.f) break;
                alpha *= f * AbsDot(wi, pi.shading.n) / pdf;
#ifndef NDEBUG
                Assert(std::isinf(alpha.y()) == false);
#endif
                specularBounce = (flags & BSDF_SPECULAR) != 0;
                ray = pi.SpawnRay(wi);
            }
        }

        // Possibly terminate the path
        if (bounces > 3) {
            Float continueProbability = std::min((Float).5, alpha.y());
            if (sampler.Get1D() > continueProbability) break;
            alpha /= continueProbability;
//.........这里部分代码省略.........