C++ p0函数代码示例

    vecN<vecN<vecN<OutputType, 2>, 4>, 2>
    split_cubicT(c_array<const vecN<InputType, 2> > pts)
    {
      FASTUIDRAWassert(pts.size() == 4);

      vecN<vecN<vecN<OutputType, 2>, 4>, 2> return_value;
      vecN<IntermediateType, 2> p0(pts[0]), p1(pts[1]), p2(pts[2]), p3(pts[3]);
      vecN<IntermediateType, 2> p01, p23, pA, pB, pC;
      const IntermediateType two(2), three(3), four(4), eight(8);

      p01 = (p0 + p1) / two;
      p23 = (p2 + p3) / two;
      pA = (p0 + two * p1 + p2) / four;
      pB = (p1 + two * p2 + p3) / four;
      pC = (p0 + three * p1 + three * p2 + p3) / eight;

      vecN<OutputType, 2> q0(pts[0]), q01(p01), qA(pA), qC(pC);
      vecN<OutputType, 2> qB(pB), q23(p23), q3(pts[3]);

      return_value[0] = vecN<vecN<OutputType, 2>, 4>(q0, q01, qA, qC);
      return_value[1] = vecN<vecN<OutputType, 2>, 4>(qC, qB, q23, q3);

      return return_value;
    }

double C2M::getHit(Vec3 p3, Vec3 n3){

	Vec3 p(p3);
	Vec3 n((n3 - Eye::get()->N*(n3*Eye::get()->N) ).normalize());
	
	double tmin = 999999999999;
	bool hit = false;
	Vec3 hitp;
	for(list<ArrCurve*>::iterator it =_curves.begin(); it!=_curves.end(); it++){

		ArrCurve*  ac = (*it);
		Vec3 p0(ac->getP(0));

		for(int i=1; i<ac->size();i++){
	
			Vec3 p1(ac->getP(i));
			Vec3 n1 = (p1 - p0);

			//double d = (n1%n).normalize()*((p0-p)%n1).normalize();
			//cout<<"dot:="<<d<<"........................"<<endl;

			double len1 = n1.norm();
			double t = getIntersectionDist(p, n, p1, n1.normalize() );

			if (t>0 && t<tmin && ( p + t*n - p1 ).norm()<len1 ){
				tmin = t;
				hit = true;
			}
			p0.set(p1);
		}
	}

	if (hit) 
		return tmin;
	return -1;
}

void MapgenV6::addDirtGravelBlobs() {
	if (getBiome(v2s16(node_min.X, node_min.Z)) != BT_NORMAL)
		return;
	
	PseudoRandom pr(blockseed + 983);
	for (int i = 0; i < volume_nodes/10/10/10; i++) {
		bool only_fill_cave = (myrand_range(0,1) != 0);
		v3s16 size(
			pr.range(1, 8),
			pr.range(1, 8),
			pr.range(1, 8)
		);
		v3s16 p0(
			pr.range(node_min.X, node_max.X) - size.X / 2,
			pr.range(node_min.Y, node_max.Y) - size.Y / 2,
			pr.range(node_min.Z, node_max.Z) - size.Z / 2
		);
		
		MapNode n1((p0.Y > -32 && !pr.range(0, 1)) ? c_dirt : c_gravel);
		for (int z1 = 0; z1 < size.Z; z1++)
		for (int y1 = 0; y1 < size.Y; y1++)
		for (int x1 = 0; x1 < size.X; x1++) {
			v3s16 p = p0 + v3s16(x1, y1, z1);
			u32 i = vm->m_area.index(p);
			if (!vm->m_area.contains(i))
				continue;
			// Cancel if not stone and not cave air
			if (vm->m_data[i].getContent() != c_stone &&
				!(vm->m_flags[i] & VMANIP_FLAG_CAVE))
				continue;
			if (only_fill_cave && !(vm->m_flags[i] & VMANIP_FLAG_CAVE))
				continue;
			vm->m_data[i] = n1;
		}
	}
}

dgFloat32 dgFastRayTest::PolygonIntersect (const dgVector& faceNormal, dgFloat32 maxT, const dgFloat32* const polygon, dgInt32 strideInBytes, const dgInt32* const indexArray, dgInt32 indexCount) const
{
	dgAssert (m_p0.m_w == dgFloat32 (0.0f));
	dgAssert (m_p1.m_w == dgFloat32 (0.0f));

	if (faceNormal.DotProduct(m_unitDir).GetScalar() < dgFloat32 (0.0f)) {
		dgInt32 stride = dgInt32(strideInBytes / sizeof (dgFloat32));
		dgBigVector v0(dgVector(&polygon[indexArray[indexCount - 1] * stride]) & dgVector::m_triplexMask);
		dgBigVector p0(m_p0);
		dgBigVector p0v0(v0 - p0);

		dgBigVector diff(m_diff);
		dgBigVector normal(faceNormal);
		dgFloat64 tOut = normal.DotProduct(p0v0).GetScalar() / normal.DotProduct(diff).GetScalar();
		if ((tOut >= dgFloat64(0.0f)) && (tOut <= maxT)) {
			dgBigVector p (p0 + diff.Scale (tOut));
			dgBigVector unitDir(m_unitDir);
			for (dgInt32 i = 0; i < indexCount; i++) {
				dgInt32 i2 = indexArray[i] * stride;
				dgBigVector v1(dgVector(&polygon[i2]) & dgVector::m_triplexMask);

				dgBigVector edge0(p - v0);
				dgBigVector edge1(v1 - v0);
				dgFloat64 area = unitDir.DotProduct (edge0.CrossProduct(edge1)).GetScalar();
				if (area < dgFloat32 (0.0f)) {
					return 1.2f;
				}
				v0 = v1;
			}

			return dgFloat32(tOut);
		}
	}

	return dgFloat32 (1.2f);
}

void object::test<1>
()
{

    LineIntersector li;
    Coordinate pt(10, 10);
    HotPixel hp(pt, 1, li);

    ensure_equals(hp.getCoordinate(), pt);

    const Envelope& env = hp.getSafeEnvelope();
    ensure_equals(env.toString(), "Env[9.25:10.75,9.25:10.75]");

    Coordinate p0(0, 10);
    Coordinate p1(20, 10);
    ensure("hp.intersects 0 10, 20 10", hp.intersects(p0, p1));

    p1.y = 11; // intersection point within 0.75 distance
    ensure("hp.intersects(0 10, 20 11)", hp.intersects(p0, p1));

    p1.y = 20;
    ensure_not("!hp.intersects(0 10, 20 20)", hp.intersects(p0, p1));

}

static void RenderBodyContactsAndTangentDiretions (NewtonBody* const body, float length)
{
	for (NewtonJoint* joint = NewtonBodyGetFirstContactJoint(body); joint; joint = NewtonBodyGetNextContactJoint(body, joint)) {
		if (NewtonJointIsActive (joint)) {
			for (void* contact = NewtonContactJointGetFirstContact (joint); contact; contact = NewtonContactJointGetNextContact (joint, contact)) {
				dVector point;
				dVector normal;	
				NewtonMaterial* const material = NewtonContactGetMaterial (contact);
				NewtonMaterialGetContactPositionAndNormal (material, body, &point.m_x, &normal.m_x);

				dVector tangnetDir0;
				dVector tangnetDir1;
				NewtonMaterialGetContactTangentDirections(material, body, &tangnetDir0[0], &tangnetDir1[0]);

				// if we are display debug info we need to block other threads from writing the data at the same time
				dVector p1 (point + normal.Scale (length));
				//dVector p0 (point - normal.Scale (length));
				dVector p0 (point);
				glVertex3f (p0.m_x, p0.m_y, p0.m_z);
				glVertex3f (p1.m_x, p1.m_y, p1.m_z);
			}
		}
	}
}

GUIFormSpecMenu::ItemSpec GUIFormSpecMenu::getItemAtPos(v2s32 p) const
{
	core::rect<s32> imgrect(0,0,imgsize.X,imgsize.Y);
	
	for(u32 i=0; i<m_inventorylists.size(); i++)
	{
		const ListDrawSpec &s = m_inventorylists[i];

		for(s32 i=0; i<s.geom.X*s.geom.Y; i++)
		{
			s32 item_i = i + s.start_item_i;
			s32 x = (i%s.geom.X) * spacing.X;
			s32 y = (i/s.geom.X) * spacing.Y;
			v2s32 p0(x,y);
			core::rect<s32> rect = imgrect + s.pos + p0;
			if(rect.isPointInside(p))
			{
				return ItemSpec(s.inventoryloc, s.listname, item_i);
			}
		}
	}

	return ItemSpec(InventoryLocation(), "", -1);
}

void setup(MeshType & mesh, viennagrid::quadrilateral_tag)
{
  typedef typename viennagrid::result_of::point<MeshType>::type               PointType;
  typedef typename viennagrid::result_of::vertex_handle<MeshType>::type       VertexHandleType;

  PointType p0(0.0, 0.0);
  PointType p1(1.0, 0.0);
  PointType p2(2.0, 0.0);
  PointType p3(2.0, 1.0);
  PointType p4(1.0, 1.0);
  PointType p5(0.0, 1.0);

  std::cout << "Adding vertices to segment:" << std::endl;
  VertexHandleType vh0 = viennagrid::make_vertex( mesh, p0 );
  VertexHandleType vh1 = viennagrid::make_vertex( mesh, p1 );
  VertexHandleType vh2 = viennagrid::make_vertex( mesh, p2 );
  VertexHandleType vh3 = viennagrid::make_vertex( mesh, p3 );
  VertexHandleType vh4 = viennagrid::make_vertex( mesh, p4 );
  VertexHandleType vh5 = viennagrid::make_vertex( mesh, p5 );

  viennagrid::make_quadrilateral( mesh, vh0, vh1, vh5, vh4 );
  viennagrid::make_quadrilateral( mesh, vh1, vh2, vh4, vh3 );

}

void RemoveMultiple ( const std::list<ID>& listToTreat, std::list< std::pair<ID, ID> >& finalList )
{
    ID counter = 0;
    std::list<ID> temporaryList ( listToTreat );

    //! Sort the list
    temporaryList.sort();

    //! initialize the new list
    std::pair <ID, ID> p0 ( temporaryList.front() , counter );
    finalList.push_back ( p0 );

    //! We remove the multiple occurences :
    for ( std::list<ID>::iterator it = temporaryList.begin() ; it != temporaryList.end() ; ++ it )
    {
        if ( ( *it ) != finalList.back().first )
        {
            counter ++ ;
            //! Add to the list the new value
            std::pair <ID, ID> p ( ( *it ) , counter );
            finalList.push_back ( p );
        }
    }
}

int main(int argc,char **argv)
{
    if(argc!=2){
	fprintf(stderr,"usage: %s scanner.so\n",argv[0]);
        fprintf(stderr,"type 'make plugins' to make available plugins\n");
	exit(1);
    }

    /* Strip extension and path */
    std::string fname = argv[1];
    scanner_t *fn=0;
    std::string name = fname;
    size_t dot = name.rfind('.');
    if(dot==std::string::npos){
	fprintf(stderr,"%s: cannot strip extension\n",name.c_str());
	exit(1);
    }
    name = name.substr(0,dot);         

    /* Strip dir */
    size_t slash = name.rfind('.');
    if(slash!=std::string::npos){
        name = name.substr(slash+1);
    }

#ifdef HAVE_DLOPEN
    if(fname.find('.')==std::string::npos){
        fname = "./" + fname;               // fedora requires a complete path name
    }

#ifdef HAVE_DLOPEN_PREFLIGHT
    if(!dlopen_preflight(fname.c_str())){
	fprintf(stderr,"dlopen_preflight - cannot open %s: %s",fname.c_str(),dlerror());
        exit(1);
    }
#endif

    void *lib=dlopen(fname.c_str(), RTLD_LAZY);
    if(lib==0){
        fprintf(stderr,"fname=%s\n",fname.c_str());
        fprintf(stderr,"dlopen: %s\n",dlerror());
        exit(1);
    }

    fn=(scanner_t *)dlsym(lib, name.c_str());
    if(fn==0){
        fprintf(stderr,"dlsym: %s\n",dlerror());
        exit(1);
    }

#endif
#ifdef HAVE_LOADLIBRARY
    /* Use Win32 LoadLibrary function */
    /* See http://msdn.microsoft.com/en-us/library/ms686944(v=vs.85).aspx */
    HINSTANCE hinstLib = LoadLibrary(TEXT(fname.c_str()));
    if(hinstLib==0){
        fprintf(stderr,"LoadLibrary(%s) failed",fname.c_str());
        exit(1);
    }
    MYPROC fn = (MYPROC)GetProcAddress(hinstLib,name.c_str());
    if(fn==0){
        fprintf(stderr,"GetProcAddress(%s) failed",name.c_str());
        exit(1);
    }
#endif

    feature_recorder_set fs(0,my_hasher,feature_recorder_set::NO_INPUT,feature_recorder_set::NO_OUTDIR);
    uint8_t buf[100];
    pos0_t p0("");
    sbuf_t sbuf(p0,buf,sizeof(buf),sizeof(buf),false);
    scanner_params sp(scanner_params::PHASE_STARTUP,sbuf,fs);
    recursion_control_block rcb(0,"STAND");
    scanner_info si;
    sp.info = &si;
    (*fn)(sp,rcb);
    std::cout << "Loaded scanner '" << si.name << "' by " << si.author << "\n";
#ifdef HAVE_DLOPEN
    dlclose(lib);
#endif
    return 0;
}

void Camera::DrawSpike(double rad, const vector3d &viewCoords, const matrix4x4d &viewTransform)
{
	glPushMatrix();

	float znear, zfar;
	Render::GetNearFarClipPlane(znear, zfar);
	double newdist = znear + 0.5f * (zfar - znear);
	double scale = newdist / viewCoords.Length();

	glTranslatef(float(scale*viewCoords.x), float(scale*viewCoords.y), float(scale*viewCoords.z));

	Render::State::UseProgram(0);
	// face the camera dammit
	vector3d zaxis = viewCoords.Normalized();
	vector3d xaxis = vector3d(0,1,0).Cross(zaxis).Normalized();
	vector3d yaxis = zaxis.Cross(xaxis);
	matrix4x4d rot = matrix4x4d::MakeInvRotMatrix(xaxis, yaxis, zaxis);
	glMultMatrixd(&rot[0]);

	glDisable(GL_LIGHTING);
	glDisable(GL_DEPTH_TEST);
	glEnable(GL_BLEND);

	// XXX WRONG. need to pick light from appropriate turd.
	GLfloat col[4];
	glGetLightfv(GL_LIGHT0, GL_DIFFUSE, col);
	glColor4f(col[0], col[1], col[2], 1);
	glBegin(GL_TRIANGLE_FAN);
	glVertex3f(0,0,0);
	glColor4f(col[0], col[1], col[2], 0);

	const float spikerad = float(scale*rad);

	// bezier with (0,0,0) control points
	{
		vector3f p0(0,spikerad,0), p1(spikerad,0,0);
		float t=0.1f; for (int i=1; i<10; i++, t+= 0.1f) {
			vector3f p = (1-t)*(1-t)*p0 + t*t*p1;
			glVertex3fv(&p[0]);
		}
	}
	{
		vector3f p0(spikerad,0,0), p1(0,-spikerad,0);
		float t=0.1f; for (int i=1; i<10; i++, t+= 0.1f) {
			vector3f p = (1-t)*(1-t)*p0 + t*t*p1;
			glVertex3fv(&p[0]);
		}
	}
	{
		vector3f p0(0,-spikerad,0), p1(-spikerad,0,0);
		float t=0.1f; for (int i=1; i<10; i++, t+= 0.1f) {
			vector3f p = (1-t)*(1-t)*p0 + t*t*p1;
			glVertex3fv(&p[0]);
		}
	}
	{
		vector3f p0(-spikerad,0,0), p1(0,spikerad,0);
		float t=0.1f; for (int i=1; i<10; i++, t+= 0.1f) {
			vector3f p = (1-t)*(1-t)*p0 + t*t*p1;
			glVertex3fv(&p[0]);
		}
	}
	glEnd();
	glDisable(GL_BLEND);
	glEnable(GL_LIGHTING);
	glEnable(GL_DEPTH_TEST);
	glPopMatrix();
}

dgUnsigned32 dgSlidingConstraint::JacobianDerivative (dgContraintDescritor& params)
{
	dgMatrix matrix0;
	dgMatrix matrix1;

	//dgVector angle (CalculateGlobalMatrixAndAngle (matrix0, matrix1));
	CalculateGlobalMatrixAndAngle (matrix0, matrix1);
	m_posit = (matrix0.m_posit - matrix1.m_posit) % matrix0.m_front;
	matrix1.m_posit += matrix1.m_front.Scale3 (m_posit);

	dgAssert (dgAbsf (dgFloat32 (1.0f) - (matrix0.m_front % matrix0.m_front)) < dgFloat32 (1.0e-5f)); 
	dgAssert (dgAbsf (dgFloat32 (1.0f) - (matrix0.m_up % matrix0.m_up)) < dgFloat32 (1.0e-5f)); 
	dgAssert (dgAbsf (dgFloat32 (1.0f) - (matrix0.m_right % matrix0.m_right)) < dgFloat32 (1.0e-5f)); 

	const dgVector& dir1 = matrix0.m_up;
	const dgVector& dir2 = matrix0.m_right;

	dgVector p0 (matrix0.m_posit);
	dgVector p1 (matrix1.m_posit + matrix1.m_front.Scale3 ((p0 - matrix1.m_posit) % matrix1.m_front));

	dgVector q0 (p0 + matrix0.m_front.Scale3(MIN_JOINT_PIN_LENGTH));
	dgVector q1 (p1 + matrix1.m_front.Scale3(MIN_JOINT_PIN_LENGTH));

	dgVector r0 (p0 + matrix0.m_up.Scale3(MIN_JOINT_PIN_LENGTH));
	dgVector r1 (p1 + matrix1.m_up.Scale3(MIN_JOINT_PIN_LENGTH));

	dgPointParam pointDataP;
	dgPointParam pointDataQ;
	dgPointParam pointDataR;
	InitPointParam (pointDataP, m_stiffness, p0, p1);
	InitPointParam (pointDataQ, m_stiffness, q0, q1);
	InitPointParam (pointDataR, m_stiffness, r0, r1);

	CalculatePointDerivative (0, params, dir1, pointDataP, &m_jointForce[0]); 
	CalculatePointDerivative (1, params, dir2, pointDataP, &m_jointForce[1]); 
	CalculatePointDerivative (2, params, dir1, pointDataQ, &m_jointForce[2]); 
	CalculatePointDerivative (3, params, dir2, pointDataQ, &m_jointForce[3]); 
	CalculatePointDerivative (4, params, dir2, pointDataR, &m_jointForce[4]); 

	dgInt32 ret = 5;
	if (m_jointAccelFnt) {
		dgJointCallbackParam axisParam;
		axisParam.m_accel = dgFloat32 (0.0f);
		axisParam.m_timestep = params.m_timestep;
		axisParam.m_minFriction = DG_MIN_BOUND;
		axisParam.m_maxFriction = DG_MAX_BOUND;

		if (m_jointAccelFnt (*this, &axisParam)) {
			if ((axisParam.m_minFriction > DG_MIN_BOUND) || (axisParam.m_maxFriction < DG_MAX_BOUND)) {
				params.m_forceBounds[5].m_low = axisParam.m_minFriction;
				params.m_forceBounds[5].m_upper = axisParam.m_maxFriction;
				params.m_forceBounds[5].m_normalIndex = DG_BILATERAL_FRICTION_CONSTRAINT;
			}

			CalculatePointDerivative (5, params, matrix0.m_front, pointDataP, &m_jointForce[5]); 
			//params.m_jointAccel[5] = axisParam.m_accel;
			SetMotorAcceleration (5, axisParam.m_accel, params);
			ret = 6;
		}
	}

	return dgUnsigned32 (ret);
}

dgInt32 dgCollisionConvexPolygon::CalculateContactToConvexHullDescrete(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 polygonInstance = proxy.m_floatingCollision;
    dgAssert(this == polygonInstance->GetChildShape());
    dgAssert(m_count);
    dgAssert(m_count < dgInt32(sizeof (m_localPoly) / sizeof (m_localPoly[0])));

    dgInt32 count = 0;


    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());
    dgVector savedFaceNormal(m_normal);

    dgVector savedPosit (proxy.m_matrix.m_posit);
    proxy.m_matrix.m_posit = dgVector::m_wOne;

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

    dgContact* const contactJoint = proxy.m_contactJoint;
    const dgCollisionInstance* const hull = proxy.m_referenceCollision;

    dgVector normalInHull(proxy.m_matrix.RotateVector(m_normal));
    dgVector pointInHull(hull->SupportVertex(normalInHull.Scale4(dgFloat32(-1.0f)), NULL));
    dgVector p0(proxy.m_matrix.UntransformVector(pointInHull));
    dgVector p1(proxy.m_matrix.UntransformVector(hull->SupportVertex(normalInHull, NULL)));

    dgFloat32 penetration = (m_localPoly[0] - p0) % m_normal + proxy.m_skinThickness;
    if (penetration < dgFloat32(0.0f)) {
        contactJoint->m_closestDistance = -penetration;
        proxy.m_matrix.m_posit = savedPosit;
        return 0;
    }

    contactJoint->m_closestDistance = dgFloat32(0.0f);
    dgFloat32 distance = (m_localPoly[0] - p1) % m_normal;
    if (distance >= dgFloat32(0.0f)) {
        proxy.m_matrix.m_posit = savedPosit;
        return 0;
    }

    dgVector boxSize (hull->GetBoxSize() & dgVector::m_triplexMask);
    dgVector boxOrigin ((hull->GetBoxOrigin() & dgVector::m_triplexMask) + dgVector::m_wOne);

    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));
        dgPlane plane(n, - m_localPoly[i0].DotProduct4 (n).GetScalar());
        plane = proxy.m_matrix.TransformPlane(plane);

        //dgFloat32 supportDist = dgAbsf(plane.m_x) * boxSize.m_x + dgAbsf(plane.m_y) * boxSize.m_y + dgAbsf(plane.m_z) * boxSize.m_z;
        //dgFloat32 centerDist = plane.Evalue(boxOrigin);
        dgFloat32 supportDist = boxSize.DotProduct4 (plane.Abs()).GetScalar();
        dgFloat32 centerDist = plane.DotProduct4 (boxOrigin).GetScalar();

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

        if ((centerDist - supportDist) < dgFloat32(0.0f)) {
            inside = false;
            break;
        }
        i0 = i;
    }

    const dgInt32 hullId = hull->GetUserDataID();
    if (inside & !proxy.m_intersectionTestOnly) {
        dgAssert(penetration >= dgFloat32(0.0f));
        dgVector pointsContacts[64];

        dgAssert(penetration >= 0.0f);
        dgVector point(pointInHull + normalInHull.Scale4(penetration));

        count = hull->CalculatePlaneIntersection(normalInHull.Scale4(dgFloat32(-1.0f)), point, pointsContacts, 1.0f);
        dgVector step(normalInHull.Scale4((proxy.m_skinThickness - penetration) * dgFloat32(0.5f)));

        const dgMatrix& worldMatrix = hull->m_globalMatrix;
        dgContactPoint* const contactsOut = proxy.m_contacts;
        dgAssert(contactsOut);
        dgVector globalNormal(worldMatrix.RotateVector(normalInHull));
        for (dgInt32 i = 0; i < count; i++) {
            contactsOut[i].m_point = worldMatrix.TransformVector(pointsContacts[i] + step);
            contactsOut[i].m_normal = globalNormal;
            contactsOut[i].m_shapeId0 = hullId;
            contactsOut[i].m_shapeId1 = m_faceId;
            contactsOut[i].m_penetration = penetration;
//.........这里部分代码省略.........

dgInt32 dgCollisionConvexPolygon::CalculatePlaneIntersection (const dgVector& normalIn, const dgVector& origin, dgVector* const contactsOut, dgFloat32 normalSign) const
{
    dgVector normal(normalIn);
    dgInt32 count = 0;
    dgFloat32 maxDist = dgFloat32 (1.0f);
    dgFloat32 projectFactor = m_normal % normal;
    if (projectFactor < dgFloat32 (0.0f)) {
        projectFactor *= dgFloat32 (-1.0f);
        normal = normal.Scale3 (dgFloat32 (-1.0f));
    }

    if (projectFactor > dgFloat32 (0.9999f)) {
        for (dgInt32 i = 0; i < m_count; i ++) {
            contactsOut[count] = m_localPoly[i];
            count ++;
        }

#ifdef _DEBUG
        dgInt32 j = count - 1;
        for (dgInt32 i = 0; i < count; i ++) {
            dgVector error (contactsOut[i] - contactsOut[j]);
            dgAssert ((error % error) > dgFloat32 (1.0e-20f));
            j = i;
        }
#endif

    } else if (projectFactor > dgFloat32 (0.1736f)) {
        maxDist = dgFloat32 (0.0f);
        dgPlane plane (normal, - (normal % origin));

        dgVector p0 (m_localPoly[m_count - 1]);
        dgFloat32 side0 = plane.Evalue (p0);
        for (dgInt32 i = 0; i < m_count; i ++) {
            dgVector p1 (m_localPoly[i]);
            dgFloat32 side1 = plane.Evalue (p1);

            if (side0 > dgFloat32 (0.0f)) {
                maxDist = dgMax (maxDist, side0);
                contactsOut[count] = p0 - plane.Scale3 (side0);
                count ++;
                if (count > 1) {
                    dgVector edgeSegment (contactsOut[count - 1] - contactsOut[count - 2]);
                    dgFloat32 error = edgeSegment % edgeSegment;
                    if (error < dgFloat32 (1.0e-8f)) {
                        count --;
                    }
                }

                if (side1 <= dgFloat32 (0.0f)) {
                    dgVector dp (p1 - p0);
                    dgFloat32 t = plane % dp;
                    dgAssert (dgAbsf (t) >= dgFloat32 (0.0f));
                    if (dgAbsf (t) < dgFloat32 (1.0e-8f)) {
                        t = dgSign(t) * dgFloat32 (1.0e-8f);
                    }
                    contactsOut[count] = p0 - dp.Scale3 (side0 / t);
                    count ++;
                    if (count > 1) {
                        dgVector edgeSegment (contactsOut[count - 1] - contactsOut[count - 2]);
                        dgFloat32 error = edgeSegment % edgeSegment;
                        if (error < dgFloat32 (1.0e-8f)) {
                            count --;
                        }
                    }
                }
            } else if (side1 > dgFloat32 (0.0f)) {
                dgVector dp (p1 - p0);
                dgFloat32 t = plane % dp;
                dgAssert (dgAbsf (t) >= dgFloat32 (0.0f));
                if (dgAbsf (t) < dgFloat32 (1.0e-8f)) {
                    t = dgSign(t) * dgFloat32 (1.0e-8f);
                }
                contactsOut[count] = p0 - dp.Scale3 (side0 / t);
                count ++;
                if (count > 1) {
                    dgVector edgeSegment (contactsOut[count - 1] - contactsOut[count - 2]);
                    dgFloat32 error = edgeSegment % edgeSegment;
                    if (error < dgFloat32 (1.0e-8f)) {
                        count --;
                    }
                }
            }

            side0 = side1;
            p0 = p1;
        }
    } else {
        maxDist = dgFloat32 (1.0e10f);
        dgPlane plane (normal, - (normal % origin));

        dgVector p0 (m_localPoly[m_count - 1]);
        dgFloat32 side0 = plane.Evalue (p0);
        for (dgInt32 i = 0; i < m_count; i ++) {
            dgVector p1 (m_localPoly[i]);
            dgFloat32 side1 = plane.Evalue (p1);

            if ((side0 * side1) < dgFloat32 (0.0f)) {
                dgVector dp (p1 - p0);
                dgFloat32 t = plane % dp;
                dgAssert (dgAbsf (t) >= dgFloat32 (0.0f));
//.........这里部分代码省略.........

dgInt32 dgCollisionConvexPolygon::CalculateContactToConvexHullDescrete(const dgWorld* const world, const dgCollisionInstance* const parentMesh, dgCollisionParamProxy& proxy)
{
	dgInt32 count = 0;

	dgAssert(proxy.m_instance0->IsType(dgCollision::dgCollisionConvexShape_RTTI));
	dgAssert(proxy.m_instance1->IsType(dgCollision::dgCollisionConvexPolygon_RTTI));
	dgAssert (proxy.m_instance1->GetGlobalMatrix().TestIdentity());

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

	const dgMatrix& hullMatrix = proxy.m_instance0->m_globalMatrix;
	dgContact* const contactJoint = proxy.m_contactJoint;
	const dgCollisionInstance* const hull = proxy.m_instance0;

	dgVector normalInHull(hullMatrix.UnrotateVector(m_normal));
	dgVector pointInHull(hull->SupportVertex(normalInHull.Scale4(dgFloat32(-1.0f)), NULL));
	dgVector p0(hullMatrix.TransformVector(pointInHull));

	dgFloat32 penetration = (m_localPoly[0] - p0) % m_normal + proxy.m_skinThickness;
	if (penetration < dgFloat32(0.0f)) {
		return 0;
	}

	dgVector p1(hullMatrix.TransformVector(hull->SupportVertex(normalInHull, NULL)));
	contactJoint->m_closestDistance = dgFloat32(0.0f);
	dgFloat32 distance = (m_localPoly[0] - p1) % m_normal;
	if (distance >= dgFloat32(0.0f)) {
		return 0;
	}

	dgVector boxSize (hull->GetBoxSize() & dgVector::m_triplexMask);
	dgVector boxOrigin ((hull->GetBoxOrigin() & dgVector::m_triplexMask) + dgVector::m_wOne);

	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 edgeBoundaryNormal(m_normal * e);
		dgPlane plane(edgeBoundaryNormal, - m_localPoly[i0].DotProduct4 (edgeBoundaryNormal).GetScalar());
		plane = hullMatrix.TransformPlane(plane);

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

		if ((centerDist + supportDist) < dgFloat32(0.0f)) {
			return 0;
		}

		if ((centerDist - supportDist) < dgFloat32(0.0f)) {
			inside = false;
			break;
		}
		i0 = i;
	}

//inside = false;
	dgFloat32 convexSphapeUmbra = hull->GetUmbraClipSize();
	if (m_faceClipSize > convexSphapeUmbra) {
		BeamClipping(dgVector(dgFloat32(0.0f)), convexSphapeUmbra);
		m_faceClipSize = hull->m_childShape->GetBoxMaxRadius();
	}

	const dgInt32 hullId = hull->GetUserDataID();
	if (inside & !proxy.m_intersectionTestOnly) {
		dgAssert(penetration >= dgFloat32(0.0f));
		dgVector contactPoints[64];

		dgAssert(penetration >= 0.0f);
		dgVector point(pointInHull + normalInHull.Scale4(penetration + DG_ROBUST_PLANE_CLIP));

		count = hull->CalculatePlaneIntersection(normalInHull.Scale4(dgFloat32(-1.0f)), point, contactPoints);
		dgVector step(normalInHull.Scale4((proxy.m_skinThickness - penetration) * dgFloat32(0.5f)));

		dgContactPoint* const contactsOut = proxy.m_contacts;
		dgAssert(contactsOut);
		for (dgInt32 i = 0; i < count; i++) {
			contactsOut[i].m_point = hullMatrix.TransformVector(contactPoints[i] + step);
			contactsOut[i].m_normal = m_normal;
			contactsOut[i].m_shapeId0 = hullId;
			contactsOut[i].m_shapeId1 = m_faceId;
			contactsOut[i].m_penetration = penetration;
		}
	} else {
		m_vertexCount = dgUnsigned16 (m_count);
		count = world->CalculateConvexToConvexContacts(proxy);
		dgAssert(proxy.m_intersectionTestOnly || (count >= 0));

		if (count >= 1) {
			dgContactPoint* const contactsOut = proxy.m_contacts;
			if (m_closestFeatureType == 3) {
				for (dgInt32 i = 0; i < count; i++) {
					//contactsOut[i].m_userId = m_faceId;
					contactsOut[i].m_shapeId0 = hullId;
					contactsOut[i].m_shapeId1 = m_faceId;
				}
			} else {
				dgVector normal (contactsOut[0].m_normal);
//.........这里部分代码省略.........