C++ Fvector类代码示例

void msimulator_Simulate( Fvector& result, Fvector& start, Fvector& end, float _radius, float _height)
{
    SCollisionData	cl_data;
    float			half_height	= _height/2;

    // Calc BB
    Fbox			b1,b2,bb;
    create_bb		(b1,start,	_radius,_height);
    create_bb		(b2,end,	_radius,_height);
    bb.merge		(b1,b2);
    bb.grow			(0.05f);

    // Collision query
    Fvector			bbC,bbD;
    bb.get_CD		(bbC,bbD);
    XRC.box_options	(0);
    XRC.box_query	(&Level,bbC,bbD);

    // XForm everything to ellipsoid space
    Fvector			xf;
    xf.set			(1/_radius,1/half_height,1/_radius);

    Fvector			Lposition;
    Lposition.set	(start);
    Lposition.y		+= half_height;
    Lposition.mul	(xf);

    Fvector			target;
    target.set		(end);
    target.y		+= half_height;
    target.mul		(xf);

    Fvector			Lvelocity;
    Lvelocity.sub	(end,start);
    Lvelocity.mul	(xf);

    cl_data.vLastSafePosition.set	(Lposition);

    // Get the data for the triangles in question and scale to ellipsoid space
    int tri_count			= XRC.r_count();
    clContactedT.resize		(tri_count);
    if (tri_count) {
        Fvector vel_dir;
        vel_dir.normalize_safe	(Lvelocity);
        for (int i_t=0; i_t<tri_count; i_t++) {
            cl_tri& T			= clContactedT[i_t];
            CDB::RESULT&		rp = XRC.r_begin()[i_t];
//			CDB::TRI&	O		=
            *(Level.get_tris()+rp.id);

            T.p[0].mul			(rp.verts[0],xf);
            T.p[1].mul			(rp.verts[1],xf);
            T.p[2].mul			(rp.verts[2],xf);
            T.N.mknormal		(T.p[0],T.p[1],T.p[2]);

            T.d = -T.N.dotproduct(T.p[0]);
            T.e10.sub(T.p[1],T.p[0]);
            T.e10s = T.e10.magnitude();
            T.e10.div(T.e10s);
            T.e21.sub(T.p[2],T.p[1]);
            T.e21s = T.e21.magnitude();
            T.e21.div(T.e21s);
            T.e02.sub(T.p[0],T.p[2]);
            T.e02s = T.e02.magnitude();
            T.e02.div(T.e02s);
        }
    }

    // call the recursive collision response function
    Fvector POS;

    for (int i=0; i<3; i++) {
        POS.set(msimulator_CollideWithWorld(cl_data, Lposition, Lvelocity, 0));
        if (fsimilar(POS.x,target.x)&&fsimilar(POS.z,target.z)) break;

        Lposition.set	(POS);
        Lvelocity.sub	(target,POS);
        Lvelocity.y		= 0;
    }

    result.div(POS,xf);
    result.y-=half_height;
}

void CLevelGraph::choose_point(const Fvector &start_point, const Fvector &finish_point, const SContour &_contour, int node_id, Fvector &temp_point, int &saved_index) const
{
	SContour					tNextContour;
	SSegment					tNextSegment;
	Fvector						tCheckPoint1 = start_point, tCheckPoint2 = start_point, tIntersectPoint;
	contour						(tNextContour,node_id);
	intersect					(tNextSegment,tNextContour,_contour);
	u32							dwIntersect = intersect(start_point.x,start_point.z,finish_point.x,finish_point.z,tNextSegment.v1.x,tNextSegment.v1.z,tNextSegment.v2.x,tNextSegment.v2.z,&tIntersectPoint.x,&tIntersectPoint.z);
	if (!dwIntersect)
		return;
	for (int i=0; i<4; ++i) {
		switch (i) {
			case 0 : {
				tCheckPoint1	= tNextContour.v1;
				tCheckPoint2	= tNextContour.v2;
				break;
					 }
			case 1 : {
				tCheckPoint1	= tNextContour.v2;
				tCheckPoint2	= tNextContour.v3;
				break;
					 }
			case 2 : {
				tCheckPoint1	= tNextContour.v3;
				tCheckPoint2	= tNextContour.v4;
				break;
					 }
			case 3 : {
				tCheckPoint1	= tNextContour.v4;
				tCheckPoint2	= tNextContour.v1;
				break;
					 }
			default : NODEFAULT;
		}
		dwIntersect				= intersect(start_point.x,start_point.z,finish_point.x,finish_point.z,tCheckPoint1.x,tCheckPoint1.z,tCheckPoint2.x,tCheckPoint2.z,&tIntersectPoint.x,&tIntersectPoint.z);
		if (dwIntersect == LevelGraph::eLineIntersectionIntersect) {
			if (finish_point.distance_to_xz(tIntersectPoint) < finish_point.distance_to_xz(temp_point) + EPS_L) {
				temp_point = tIntersectPoint;
				saved_index = node_id;
			}
		}
		else
			if (dwIntersect == LevelGraph::eLineIntersectionEqual) {
				if (start_point.distance_to_xz(tCheckPoint1) > start_point.distance_to_xz(temp_point))
					if (start_point.distance_to_xz(tCheckPoint1) > start_point.distance_to_xz(tCheckPoint2)) {
						temp_point = tCheckPoint1;
						saved_index = node_id;
					}
					else {
						temp_point = tCheckPoint2;
						saved_index = node_id;
					}
				else
					if (start_point.distance_to_xz(tCheckPoint2) > start_point.distance_to_xz(temp_point)) {
						temp_point = tCheckPoint2;
						saved_index = node_id;
					}

			}
	}
}

void CCustomObject::AnimationCreateKey(float t)
{
    Fvector R;		R.set(-PRotation.x,-PRotation.y,-PRotation.z);
    m_Motion->CreateKey(t,PPosition,R);
}

void ComputeCylinder(Fcylinder& C, Fobb& B, FvectorVec& V)
{
    if (V.size()<3) 	{ C.invalidate(); return; }
    // pow(area,(3/2))/volume
    // 2*Pi*R*H+2*Pi*R*R
	
//	Fvector axis;
    float max_hI   	= flt_min;
    float min_hI   	= flt_max;
    float max_rI   	= flt_min;
    float max_hJ   	= flt_min;
    float min_hJ   	= flt_max;
    float max_rJ   	= flt_min;
    float max_hK   	= flt_min;
    float min_hK   	= flt_max;
    float max_rK   	= flt_min;
    Fvector axisJ 		= B.m_rotate.j;
    Fvector axisI 		= B.m_rotate.i;
    Fvector axisK 		= B.m_rotate.k;
    Fvector& c			= B.m_translate;
    for (FvectorIt I=V.begin(); I!=V.end(); I++){
        Fvector tmp;
    	Fvector pt		= *I;
    	Fvector pt_c;	pt_c.sub(pt,c);

    	float pI		= axisI.dotproduct(pt);
        min_hI			= _min(min_hI,pI);
        max_hI			= _max(max_hI,pI);
        tmp.mad			(c,axisI,axisI.dotproduct(pt_c));
        max_rI			= _max(max_rI,tmp.distance_to(pt));
        
    	float pJ		= axisJ.dotproduct(pt);
        min_hJ			= _min(min_hJ,pJ);
        max_hJ			= _max(max_hJ,pJ);
        tmp.mad			(c,axisJ,axisJ.dotproduct(pt_c));
        max_rJ			= _max(max_rJ,tmp.distance_to(pt));

    	float pK		= axisK.dotproduct(pt);
        min_hK			= _min(min_hK,pK);
        max_hK			= _max(max_hK,pK);
        tmp.mad			(c,axisK,axisK.dotproduct(pt_c));
        max_rK			= _max(max_rK,tmp.distance_to(pt));
    }

    float hI			= (max_hI-min_hI);
    float hJ			= (max_hJ-min_hJ);
    float hK			= (max_hK-min_hK);
    float vI			= hI*M_PI*_sqr(max_rI);
    float vJ			= hJ*M_PI*_sqr(max_rJ);
    float vK			= hK*M_PI*_sqr(max_rK);
//    vI					= pow(2*M_PI*max_rI*hI+2*M_PI*_sqr(max_rI),3/2)/vI;
//    vJ					= pow(2*M_PI*max_rJ*hJ+2*M_PI*_sqr(max_rJ),3/2)/vJ;
//    vK					= pow(2*M_PI*max_rK*hK+2*M_PI*_sqr(max_rK),3/2)/vK;
    // pow(area,(3/2))/volume
    // 2*Pi*R*H+2*Pi*R*R
    
    if (vI<vJ){
    	if (vI<vK){
        	//vI;
            C.m_direction.set	(axisI);
            C.m_height			= hI;
            C.m_radius			= max_rI;
        }else{
        	// vK
            C.m_direction.set	(axisK);
            C.m_height			= hK;
            C.m_radius			= max_rK;
        }
    }else {
        //vJ < vI
     	if (vJ<vK){
        	// vJ
            C.m_direction.set	(axisJ);
            C.m_height			= hJ;
            C.m_radius			= max_rJ;
        } else {
            //vK
            C.m_direction.set	(axisK);
            C.m_height			= hK;
            C.m_radius			= max_rK;
        }
    }
    
    C.m_center.set		(B.m_translate);
/*
    if (V.size()<3) { B.invalidate(); return; }
    Mgc::Cylinder CYL	= Mgc::ContCylinder(V.size(), (const Mgc::Vector3*) V.begin());
    B.m_center.set		(CYL.Center());
    B.m_direction.set	(CYL.Direction());
    B.m_height			= CYL.Height();
    B.m_radius			= CYL.Radius();
*/
}

void xrBuildNodes()
{
	// begin
	XRC.box_options	(CDB::OPT_FULL_TEST);
	XRC.ray_options	(CDB::OPT_CULL | CDB::OPT_ONLYNEAREST);
	g_nodes.reserve	(1024*1024);

	// Initialize hash
	hash_Initialize ();

	for (u32 em=0; em<Emitters.size(); em++) 
	{
		// Align emitter
		Fvector			Pos = Emitters[em];
		SnapXZ			(Pos);
		Pos.y			+=1;
		Fvector			Dir; Dir.set(0,-1,0);
		
		XRC.ray_query	(&Level,Pos,Dir,3);
		if (XRC.r_count()==0) {
			Msg		("Can't align emitter");
			abort	();
		} else {
			CDB::RESULT& R = *XRC.r_begin();
			Pos.y = Pos.y - R.range;
		}
		
		// Build first node
		int oldcount = g_nodes.size();
		int start = BuildNode		(Pos,Pos);
		if (start==InvalidNode)		continue;
		if (start<oldcount)			continue;

		// Estimate nodes
		Fvector	LevelSize;
		LevelBB.getsize				(LevelSize);
		u32	estimated_nodes		= iFloor(LevelSize.x/g_params.fPatchSize)*iFloor(LevelSize.z/g_params.fPatchSize);
		
		// General cycle
		for (u32 i=0; i<g_nodes.size(); i++)
		{
			// left 
			if (g_nodes[i].n1==UnkonnectedNode)
			{
				Pos.set			(g_nodes[i].Pos);
				Pos.x			-=	g_params.fPatchSize;
				int	id			=	BuildNode(g_nodes[i].Pos,Pos);
				g_nodes[i].n1	=	id;
			}
			// fwd
			if (g_nodes[i].n2==UnkonnectedNode)
			{
				Pos.set			(g_nodes[i].Pos);
				Pos.z			+=	g_params.fPatchSize;
				int id			=	BuildNode(g_nodes[i].Pos,Pos);
				g_nodes[i].n2	=	id;
			}
			// right
			if (g_nodes[i].n3==UnkonnectedNode)
			{
				Pos.set			(g_nodes[i].Pos);
				Pos.x			+=	g_params.fPatchSize;
				int id			=	BuildNode(g_nodes[i].Pos,Pos);
				g_nodes[i].n3	=	id;
			}
			// back
			if (g_nodes[i].n4==UnkonnectedNode)
			{
				Pos.set			(g_nodes[i].Pos);
				Pos.z			-=	g_params.fPatchSize;
				int	id			=	BuildNode(g_nodes[i].Pos,Pos);
				g_nodes[i].n4	=	id;
			}
			if (i%512==0) {
				Status("%d / %d nodes created.",g_nodes.size()-i,g_nodes.size());

				float	p1	= float(i)/float(g_nodes.size());
				float	p2	= float(g_nodes.size())/estimated_nodes;
				float	p	= 0.1f*p1+0.9f*p2;

				clamp	(p,0.f,1.f);
				Progress(p);
			}
		}
	}
	Msg("Freeing memory...");
	hash_Destroy	();
}

int ESceneAIMapTools::BuildNodes(const Fvector& pos, int sz, bool bIC)
{
    // Align emitter
    Fvector			Pos = pos;
    SnapXZ			(Pos,m_Params.fPatchSize);
    Pos.y			+= 1;
    Fvector			Dir; Dir.set(0,-1,0);

	int cnt			= 0;		
    if (m_CFModel)	cnt=Scene->RayQuery(PQ,Pos,Dir,3,CDB::OPT_ONLYNEAREST|CDB::OPT_CULL,m_CFModel);
    else			cnt=Scene->RayQuery(PQ,Pos,Dir,3,CDB::OPT_ONLYNEAREST|CDB::OPT_CULL,GetSnapList());
    if (0==cnt) {
        ELog.Msg	(mtInformation,"Can't align position.");
        return		0;
    } else {
        Pos.y 		= Pos.y - PQ.r_begin()->range;
    }
		
    // Build first node
    int oldcount 	= m_Nodes.size();
    SAINode* start 	= BuildNode(Pos,Pos,bIC);
    if (!start)		return 0;

    // Estimate nodes
    float estimated_nodes	= (2*sz-1)*(2*sz-1);

	SPBItem* pb 	= 0;
    if (estimated_nodes>1024) pb = UI->ProgressStart(1, "Building nodes...");
    float radius			= sz*m_Params.fPatchSize-EPS_L;
    // General cycle
    for (int k=0; k<(int)m_Nodes.size(); k++){
        SAINode* N 			= m_Nodes[k];
        // left 
        if (0==N->n1){
            Pos.set			(N->Pos);
            Pos.x			-=	m_Params.fPatchSize;
            if (Pos.distance_to(start->Pos)<=radius)
	            N->n1		=	BuildNode(N->Pos,Pos,bIC);
        }
        // fwd
        if (0==N->n2){
            Pos.set			(N->Pos);
            Pos.z			+=	m_Params.fPatchSize;
            if (Pos.distance_to(start->Pos)<=radius)
	            N->n2		=	BuildNode(N->Pos,Pos,bIC);
        }
        // right
        if (0==N->n3){
            Pos.set			(N->Pos);
            Pos.x			+=	m_Params.fPatchSize;
            if (Pos.distance_to(start->Pos)<=radius)
	            N->n3		=	BuildNode(N->Pos,Pos,bIC);
        }
        // back
        if (0==N->n4){
            Pos.set			(N->Pos);
            Pos.z			-=	m_Params.fPatchSize;
            if (Pos.distance_to(start->Pos)<=radius)
	            N->n4		=	BuildNode(N->Pos,Pos,bIC);
        }
        if (estimated_nodes>1024){
            if (k%128==0) {
                float	p1	= float(k)/float(m_Nodes.size());
                float	p2	= float(m_Nodes.size())/estimated_nodes;
                float	p	= 0.1f*p1+0.9f*p2;

                clamp	(p,0.f,1.f);
                pb->Update(p);
                // check need abort && redraw
                if (UI->NeedAbort()) break;
            }
        }
    }
	if (estimated_nodes>1024) UI->ProgressEnd(pb);
    return oldcount-m_Nodes.size();
}

void		CTeleWhirlwindObject::		raise					(float step)
{

		CPhysicsShell*	p					=	get_object()	->PPhysicsShell();
	
		if(!p||!p->isActive())	
			return;
		else
			{
				p->SetAirResistance(0.f,0.f);
				p->set_ApplyByGravity(TRUE);
			}
		u16				element_number		=	p				->get_ElementsNumber();
		Fvector			center				=	m_telekinesis	->Center();
		CPhysicsElement* maxE=p->get_ElementByStoreOrder(0);
		for(u16 element=0;element<element_number;++element)
		{
			float k=strength;//600.f;
			float predict_v_eps=0.1f;
			float mag_eps	   =.01f;

			CPhysicsElement* E=	p->get_ElementByStoreOrder(element);
			if(maxE->getMass()<E->getMass())	maxE=E;
			if (!E->isActive()) continue;
			Fvector pos=E->mass_Center();

			Fvector diff;
			diff.sub(center,pos);
			float mag=_sqrt(diff.x*diff.x+diff.z*diff.z);
			Fvector lc;lc.set(center);
			if(mag>1.f)
			{
				lc.y/=mag;
			}
			diff.sub(lc,pos);
			mag=diff.magnitude();
			float accel=k/mag/mag/mag;//*E->getMass()
			Fvector dir;
			if(mag<mag_eps)
			{
				accel=0.f;
				//Fvector zer;zer.set(0,0,0);
				//E->set_LinearVel(zer);
				dir.random_dir();
			}
			else
			{
				dir.set(diff);dir.mul(1.f/mag);
			}
			Fvector vel;
			E->get_LinearVel(vel);
			float delta_v=accel*fixed_step;
			Fvector delta_vel; delta_vel.set(dir);delta_vel.mul(delta_v);
			Fvector predict_vel;predict_vel.add(vel,delta_vel);
			Fvector delta_pos;delta_pos.set(predict_vel);delta_pos.mul(fixed_step);
			Fvector predict_pos;predict_pos.add(pos,delta_pos);
			
			Fvector predict_diff;predict_diff.sub(lc,predict_pos);
			float predict_mag=predict_diff.magnitude();
			float predict_v=predict_vel.magnitude();

			Fvector force;force.set(dir);
			if(predict_mag>mag && predict_vel.dotproduct(dir)>0.f && predict_v>predict_v_eps)
			{
	
				Fvector motion_dir;motion_dir.set(predict_vel);motion_dir.mul(1.f/predict_v);
				float needed_d=diff.dotproduct(motion_dir);
				Fvector needed_diff;needed_diff.set(motion_dir);needed_diff.mul(needed_d);
				Fvector nearest_p;nearest_p.add(pos,needed_diff);//
				Fvector needed_vel;needed_vel.set(needed_diff);needed_vel.mul(1.f/fixed_step);
				force.sub(needed_vel,vel);
				force.mul(E->getMass()/fixed_step);
			}
			else
			{
				force.mul(accel*E->getMass());
			}
			
			
			E->applyForce(force.x,force.y+get_object()->EffectiveGravity()*E->getMass(),force.z);
		}
		Fvector dist;dist.sub(center,maxE->mass_Center());
		if(dist.magnitude()<m_telekinesis->keep_radius()&&b_destroyable)
		{
			p->setTorque(Fvector().set(0,0,0));
			p->setForce(Fvector().set(0,0,0));
			p->set_LinearVel(Fvector().set(0,0,0));
			p->set_AngularVel(Fvector().set(0,0,0));
			switch_state(TS_Keep);
		}
}

void	CPHCharacter::getForce(Fvector& force)
{
	if(!b_exist)return;
	force.set(*(Fvector*)dBodyGetForce(m_body));
}

void character_hit_animation_controller::PlayHitMotion( const Fvector &dir, const Fvector &bone_pos, u16 bi, CEntityAlive &ea )const
{
	IRenderVisual *pV = ea.Visual( );
	IKinematicsAnimated* CA = smart_cast<IKinematicsAnimated*>( pV );
	IKinematics* K = smart_cast<IKinematics*>( pV );
	
	//play_cycle(CA,all_shift_down,1,block_times[6],1) ;
	if( !( K->LL_BoneCount( ) > bi ) )
		return;

	Fvector dr = dir;
	Fmatrix m;
	GetBaseMatrix( m, ea );

#ifdef DEBUG
	if( ph_dbg_draw_mask1.test( phDbgHitAnims ) )
	{
		DBG_OpenCashedDraw();
		DBG_DrawLine( m.c, Fvector( ).sub( m.c, Fvector( ).mul( dir, 1.5 ) ), D3DCOLOR_XRGB( 255, 0, 255 ) );
		DBG_ClosedCashedDraw( 1000 );
	}
#endif

	m.invert( );
	m.transform_dir( dr );
//
	Fvector hit_point;
	K->LL_GetTransform( bi ).transform_tiny( hit_point, bone_pos );
	ea.XFORM( ).transform_tiny( hit_point );
	m.transform_tiny( hit_point );
	Fvector torqu;		
	torqu.crossproduct( dr, hit_point );
	hit_point.x = 0;


	float rotational_ammount = hit_point.magnitude( ) * g_params.power_factor * g_params.rotational_power_factor;//_abs(torqu.x)
	
	if( torqu.x < 0 )
		play_cycle( CA, hit_downr, 3, block_blends[7], 1 ) ;
	else
		play_cycle( CA, hit_downl, 3, block_blends[6], 1 ) ;

	if( !IsEffected( bi, *K ) )
		return;
	if( torqu.x<0 )
		play_cycle( CA, turn_right, 2, block_blends[4], rotational_ammount ) ;
	else
		play_cycle( CA, turn_left, 2, block_blends[5], rotational_ammount ) ;

	//CA->LL_SetChannelFactor(3,rotational_ammount);

	dr.x = 0;
	dr.normalize_safe();


	dr.mul(g_params.power_factor);
	if( dr.y > g_params.side_sensitivity_threshold )
		play_cycle( CA, rthit_motion, 2, block_blends[0], _abs( dr.y ) ) ;
	else if( dr.y < -g_params.side_sensitivity_threshold )
		play_cycle( CA, lthit_motion, 2, block_blends[1], _abs( dr.y ) ) ;

	if( dr.z<0.f )
		play_cycle( CA, fvhit_motion, 2, block_blends[2], _abs(dr.z) ) ;
	else
		play_cycle( CA, bkhit_motion, 2, block_blends[3], _abs( dr.z ) ) ;

	CA->LL_SetChannelFactor( 2, g_params.anim_channel_factor );


}

void CPHCharacter::GetSavedVelocity(Fvector& vvel)
{
	
	if(IsEnabled())vvel.set(m_safe_velocity);
	else GetVelocity(vvel);
}

void dbg_draw_frustum	(float FOV, float _FAR, float A, Fvector &P, Fvector &D, Fvector &U)
{
    //if (!bDebug)		return;

    float YFov	= deg2rad(FOV*A);
    float XFov	= deg2rad(FOV);

    // calc window extents in camera coords
    float wR=tanf(XFov*0.5f);
    float wL=-wR;
    float wT=tanf(YFov*0.5f);
    float wB=-wT;

    // calc x-axis (viewhoriz) and store cop
    // here we are assuring that vectors are perpendicular & normalized
    Fvector			R,COP;
    D.normalize		();
    R.crossproduct	(D,U);
    R.normalize		();
    U.crossproduct	(R,D);
    U.normalize		();
    COP.set			(P);

    // calculate the corner vertices of the window
    Fvector			sPts[4];  // silhouette points (corners of window)
    Fvector			Offset,T;
    Offset.add		(D,COP);

    sPts[0].mul(R,wR);
    T.mad(Offset,U,wT);
    sPts[0].add(T);
    sPts[1].mul(R,wL);
    T.mad(Offset,U,wT);
    sPts[1].add(T);
    sPts[2].mul(R,wL);
    T.mad(Offset,U,wB);
    sPts[2].add(T);
    sPts[3].mul(R,wR);
    T.mad(Offset,U,wB);
    sPts[3].add(T);

    // find projector direction vectors (from cop through silhouette pts)
    Fvector ProjDirs[4];
    ProjDirs[0].sub(sPts[0],COP);
    ProjDirs[1].sub(sPts[1],COP);
    ProjDirs[2].sub(sPts[2],COP);
    ProjDirs[3].sub(sPts[3],COP);

    //RCache.set_CullMode	(CULL_NONE);
    DRender->CacheSetCullMode(IDebugRender::cmNONE);
    //CHK_DX(HW.pDevice->SetRenderState	(D3DRS_AMBIENT,		0xffffffff			));
    DRender->SetAmbient(0xffffffff);


    Fvector _F[4];
    _F[0].mad(COP, ProjDirs[0], _FAR);
    _F[1].mad(COP, ProjDirs[1], _FAR);
    _F[2].mad(COP, ProjDirs[2], _FAR);
    _F[3].mad(COP, ProjDirs[3], _FAR);

//	u32 CT	= color_rgba(255,255,255,64);
    u32 CL	= color_rgba(0,255,255,255);
    Fmatrix& M	= Fidentity;
    //ref_shader				l_tShaderReference = Level().ObjectSpace.dbgGetShader();
    //RCache.set_Shader		(l_tShaderReference);
    Level().ObjectSpace.m_pRender->SetShader();
//	RCache.dbg_DrawTRI	(M,COP,_F[0],_F[1],CT);
//	RCache.dbg_DrawTRI	(M,COP,_F[1],_F[2],CT);
//	RCache.dbg_DrawTRI	(M,COP,_F[2],_F[3],CT);
//	RCache.dbg_DrawTRI	(M,COP,_F[3],_F[0],CT);
    Level().debug_renderer().draw_line	(M,COP,_F[0],CL);
    Level().debug_renderer().draw_line	(M,COP,_F[1],CL);
    Level().debug_renderer().draw_line	(M,COP,_F[2],CL);
    Level().debug_renderer().draw_line	(M,COP,_F[3],CL);

    Level().debug_renderer().draw_line	(M,_F[0],_F[1],CL);
    Level().debug_renderer().draw_line	(M,_F[1],_F[2],CL);
    Level().debug_renderer().draw_line	(M,_F[2],_F[3],CL);
    Level().debug_renderer().draw_line	(M,_F[3],_F[0],CL);

    //RCache.set_CullMode			(CULL_CCW);
    DRender->CacheSetCullMode(IDebugRender::cmCCW);
    //CHK_DX(HW.pDevice->SetRenderState	(D3DRS_AMBIENT,	0						));
    DRender->SetAmbient(0);
}

// ----------------------------------------------------------------------
// Name  : classifyPoint()
// Input : point - point we wish to classify
//         pO - Origin of plane
//         pN - Normal to plane
// Notes :
// Return: One of 3 classification codes
// -----------------------------------------------------------------------
IC float classifyPoint(const Fvector& point, const Fvector& planeO, const Fvector& planeN)
{
    Fvector dir;
    dir.sub	(point,planeO);
    return dir.dotproduct(planeN);
}

BOOL	ValidNode(vertex& N)
{
	// *** Query and cache polygons for ray-casting
	Fvector	PointUp;		PointUp.set(N.Pos);		PointUp.y	+= RCAST_Depth/2;
	Fvector	PointDown;		PointDown.set(N.Pos);	PointDown.y	-= RCAST_Depth/2;

	Fbox	BB;				BB.set	(PointUp,PointUp);		BB.grow(g_params.fPatchSize/2);	// box 1
	Fbox	B2;				B2.set	(PointDown,PointDown);	B2.grow(g_params.fPatchSize/2);	// box 2
	BB.merge(B2			);
	BoxQuery(BB,false	);
	u32	dwCount = XRC.r_count();
	if (dwCount==0)	{
		Log("chasm1");
		return FALSE;			// chasm?
	}

	// *** Transfer triangles and compute sector
	R_ASSERT(dwCount<RCAST_MaxTris);
	static svector<tri,RCAST_MaxTris> tris;		tris.clear();
	for (u32 i=0; i<dwCount; i++)
	{
		tri&		D = tris.last();
		CDB::RESULT&rp = XRC.r_begin()[i];
		*(Level.get_tris()+XRC.r_begin()[i].id);

		D.v[0].set	(rp.verts[0]);
		D.v[1].set	(rp.verts[1]);
		D.v[2].set	(rp.verts[2]);

		Fvector		N;
		N.mknormal	(D.v[0],D.v[1],D.v[2]);
		if (N.y<=0)	continue;

		tris.inc	();
	}
	if (tris.size()==0)	{
		Log("chasm2");
		return FALSE;			// chasm?
	}

	// *** Perform ray-casts and calculate sector
	Fvector P,D,PLP; D.set(0,-1,0);

	float coeff = 0.5f*g_params.fPatchSize/float(RCAST_Count);

	int num_successed_rays = 0;
	for (int x=-RCAST_Count; x<=RCAST_Count; x++) 
	{
		P.x = N.Pos.x + coeff*float(x);
		for (int z=-RCAST_Count; z<=RCAST_Count; z++) {
			P.z = N.Pos.z + coeff*float(z);
			P.y = N.Pos.y;
			N.Plane.intersectRayPoint(P,D,PLP);	// "project" position
			P.y = PLP.y+RCAST_DepthValid/2;

			float	tri_min_range	= flt_max;
			int		tri_selected	= -1;
			float	range = 0.f,u,v;
			for (i=0; i<u32(tris.size()); i++) 
			{
				if (CDB::TestRayTri(P,D,tris[i].v,u,v,range,false)) 
				{
					if (range<tri_min_range) {
						tri_min_range	= range;
						tri_selected	= i;
					}
				}
			}
			if (tri_selected>=0) {
				if (range<RCAST_DepthValid)	num_successed_rays++;

			}
		}
	}
	if (float(num_successed_rays)/float(RCAST_Total) < 0.5f) {
		Msg		("Floating node.");
		return	FALSE;
	}
	return TRUE;
}

void CMonsterSquad::Attack_AssignTargetDir(ENTITY_VEC &members, const CEntity *enemy)
{
	_elem	first;
	_elem	last;

	lines.clear();

	// сортировать по убыванию расстояния от npc до врага 
	std::sort(members.begin(), members.end(), sort_predicate(enemy));
	if (members.empty()) return;

	float delta_yaw = PI_MUL_2 / members.size();

	// обработать ближний элемент
	first.pE		= members.back();
	first.p_from	= first.pE->Position();
	first.yaw		= 0;
	members.pop_back();

	lines.push_back(first);

	// обработать дальний элемент
	if (!members.empty()) {
		last.pE			= members[0];
		last.p_from		= last.pE->Position();
		last.yaw		= PI;
		members.erase	(members.begin());

		lines.push_back(last);
	}

	Fvector target_pos = enemy->Position();
	float	next_right_yaw	= delta_yaw;
	float	next_left_yaw	= delta_yaw;

	// проходим с конца members в начало (начиная с наименьшего расстояния)
	while (!members.empty()) {
		CEntity *pCur;

		pCur = members.back();
		members.pop_back();

		_elem cur_line;
		cur_line.p_from		= pCur->Position();
		cur_line.pE			= pCur;

		// определить cur_line.yaw

		float h1,p1,h2,p2;
		Fvector dir;
		dir.sub(target_pos, first.p_from);
		dir.getHP(h1,p1);	
		dir.sub(target_pos, cur_line.p_from);
		dir.getHP(h2,p2);

		bool b_add_left = false;

		if (angle_normalize_signed(h2 - h1) > 0)  {		// right
			if ((next_right_yaw < PI) && !fsimilar(next_right_yaw, PI, PI/60.f)) b_add_left = false;
			else b_add_left = true;
		} else {										// left
			if ((next_left_yaw < PI) && !fsimilar(next_left_yaw, PI, PI/60.f)) b_add_left = true;
			else b_add_left = false;
		}

		if (b_add_left) {
			cur_line.yaw = -next_left_yaw;
			next_left_yaw += delta_yaw;
		} else {
			cur_line.yaw = next_right_yaw;
			next_right_yaw += delta_yaw;
		}

		lines.push_back(cur_line);
	}

	// Пройти по всем линиям и заполнить таргеты у npc
	float first_h, first_p;
	Fvector d; d.sub(target_pos,first.p_from);
	d.getHP(first_h, first_p);

	for (u32 i = 0; i < lines.size(); i++){
		SSquadCommand command;
		command.type			= SC_ATTACK;
		command.entity			= enemy;
		command.direction.setHP	(first_h + lines[i].yaw, first_p);
		UpdateCommand(lines[i].pE, command);
	}
}

BOOL ESceneAIMapTools::CreateNode(Fvector& vAt, SAINode& N, bool bIC)
{
	// *** Query and cache polygons for ray-casting
	Fvector	PointUp;		PointUp.set(vAt);	PointUp.y	+= RCAST_Depth;		SnapXZ	(PointUp,m_Params.fPatchSize);
	Fvector	PointDown;		PointDown.set(vAt);	PointDown.y	-= RCAST_Depth;		SnapXZ	(PointDown,m_Params.fPatchSize);

	Fbox	BB;				BB.set	(PointUp,PointUp);		BB.grow(m_Params.fPatchSize/2);	// box 1
	Fbox	B2;				B2.set	(PointDown,PointDown);	B2.grow(m_Params.fPatchSize/2);	// box 2
	BB.merge				(B2);   
    if (m_CFModel)	Scene->BoxQuery(PQ,BB,CDB::OPT_FULL_TEST,m_CFModel);
    else			Scene->BoxQuery(PQ,BB,CDB::OPT_FULL_TEST,GetSnapList());
	DWORD	dwCount 		= PQ.r_count();
	if (dwCount==0){
//		Log("chasm1");
		return FALSE;			// chasm?
	}

	// *** Transfer triangles and compute sector
//	R_ASSERT(dwCount<RCAST_MaxTris);
	static xr_vector<tri> tris;	tris.reserve(RCAST_MaxTris);	tris.clear();
	for (DWORD i=0; i<dwCount; i++)
	{
    	SPickQuery::SResult* R = PQ.r_begin()+i;
        
        if (R->e_obj&&R->e_mesh){
            CSurface* surf		= R->e_mesh->GetSurfaceByFaceID(R->tag);
//.			SGameMtl* mtl 		= GMLib.GetMaterialByID(surf->_GameMtl());
//.			if (mtl->Flags.is(SGameMtl::flPassable))continue;
            Shader_xrLC* c_sh	= Device.ShaderXRLC.Get(surf->_ShaderXRLCName());
            if (!c_sh->flags.bCollision) 			continue;
        }
    
    	tris.push_back	(tri());
		tri&		D = tris.back();
		Fvector*	V = R->verts;   

		D.v[0]		= &V[0];
		D.v[1]		= &V[1];
		D.v[2]		= &V[2];
		D.N.mknormal(*D.v[0],*D.v[1],*D.v[2]);
		if (D.N.y<=0)	tris.pop_back	();
	}
	if (tris.size()==0){
//		Log("chasm2");
		return FALSE;			// chasm?
	}

	static xr_vector<Fvector>	points;		points.reserve(RCAST_Total); points.clear();
	static xr_vector<Fvector>	normals;	normals.reserve(RCAST_Total);normals.clear();
	Fvector P,D; D.set(0,-1,0);

	float coeff 	= 0.5f*m_Params.fPatchSize/float(RCAST_Count);

	for (int x=-RCAST_Count; x<=RCAST_Count; x++) 
	{
		P.x = vAt.x + coeff*float(x); 
		for (int z=-RCAST_Count; z<=RCAST_Count; z++) {
			P.z = vAt.z + coeff*float(z);
			P.y = vAt.y + 10.f;

			float	tri_min_range	= flt_max;
			int		tri_selected	= -1;
			float	range,u,v;
			for (i=0; i<DWORD(tris.size()); i++){
				if (ETOOLS::TestRayTriA(P,D,tris[i].v,u,v,range,false)){
					if (range<tri_min_range){
						tri_min_range	= range;
						tri_selected	= i;
					}
				}
			}
			if (tri_selected>=0) {
				P.y -= tri_min_range;
				points.push_back(P);
				normals.push_back(tris[tri_selected].N);
			}
		}
	}
	if (points.size()<3) {
//		Msg		("Failed to create node at [%f,%f,%f].",vAt.x,vAt.y,vAt.z);
		return	FALSE;
	}
//.
	float rc_lim = bIC?0.015f:0.7f;
	if (float(points.size())/float(RCAST_Total) < rc_lim) {
//		Msg		("Partial chasm at [%f,%f,%f].",vAt.x,vAt.y,vAt.z);
		return	FALSE;
	}

	// *** Calc normal
	Fvector vNorm;
	vNorm.set(0,0,0);
	for (DWORD n=0; n<normals.size(); n++)
		vNorm.add(normals[n]);
	vNorm.div(float(normals.size()));
	vNorm.normalize();
	/*
	{
		// second algorithm (Magic)
		Fvector N,O;
//.........这里部分代码省略.........