C++ Fvector::crossproduct方法代码示例

IC static void generate_orthonormal_basis1(const Fvector& dir,Fvector& updir, Fvector& right)
{

	right.crossproduct(dir,updir); //. <->
	right.normalize();
	updir.crossproduct(right,dir);
}

void player_hud::update_inertion(Fmatrix& trans)
{
	if ( inertion_allowed() )
	{
		Fmatrix								xform;
		Fvector& origin						= trans.c; 
		xform								= trans;

		static Fvector						st_last_dir={0,0,0};

		// calc difference
		Fvector								diff_dir;
		diff_dir.sub						(xform.k, st_last_dir);

		// clamp by PI_DIV_2
		Fvector last;						last.normalize_safe(st_last_dir);
		float dot							= last.dotproduct(xform.k);
		if (dot<EPS){
			Fvector v0;
			v0.crossproduct					(st_last_dir,xform.k);
			st_last_dir.crossproduct		(xform.k,v0);
			diff_dir.sub					(xform.k, st_last_dir);
		}

		// tend to forward
		st_last_dir.mad						(diff_dir,TENDTO_SPEED*Device.fTimeDelta);
		origin.mad							(diff_dir,ORIGIN_OFFSET);

		// pitch compensation
		float pitch							= angle_normalize_signed(xform.k.getP());
		origin.mad							(xform.k,	-pitch * PITCH_OFFSET_D);
		origin.mad							(xform.i,	-pitch * PITCH_OFFSET_R);
		origin.mad							(xform.j,	-pitch * PITCH_OFFSET_N);
	}
}

BOOL MeshExpUtility::BuildObject(CEditableObject*& exp_obj, LPCSTR m_ExportName)
{
	bool bResult = true;

	if (m_ExportName[0]==0) return false;

	ELog.Msg(mtInformation,"Building object..." );
	char fname[256]; _splitpath( m_ExportName, 0, 0, fname, 0 );
	exp_obj = xr_new<CEditableObject>(fname);	
	exp_obj->SetVersionToCurrent(TRUE,TRUE);

	ExportItemIt it = m_Items.begin();
	for(;it!=m_Items.end();it++){
		CEditableMesh *submesh = xr_new<CEditableMesh>(exp_obj);
		ELog.Msg(mtInformation,"Converting node '%s'...", it->pNode->GetName());
		if( submesh->Convert(it->pNode) ){
			// transform
			Matrix3 mMatrix;
			mMatrix = it->pNode->GetNodeTM(0)*Inverse(it->pNode->GetParentNode()->GetNodeTM(0));

			Point3	r1	= mMatrix.GetRow(0);
			Point3	r2	= mMatrix.GetRow(1);
			Point3	r3	= mMatrix.GetRow(2);
			Point3	r4	= mMatrix.GetRow(3);
			Fmatrix m;	m.identity();
			m.i.set(r1.x, r1.z, r1.y);
			m.j.set(r2.x, r2.z, r2.y);
			m.k.set(r3.x, r3.z, r3.y);
			m.c.set(r4.x, r4.z, r4.y);
			
			submesh->Transform( m );
			// flip faces
			Fvector v; v.crossproduct(m.j, m.k);
			if(v.dotproduct(m.i)<0.f)	submesh->FlipFaces();
			if(m_ObjectFlipFaces)		submesh->FlipFaces();
			submesh->RecomputeBBox();
			// append mesh
			submesh->SetName			(it->pNode->GetName());
			exp_obj->m_Meshes.push_back	(submesh);
		}else{
			ELog.Msg(mtError,"! can't convert", it->pNode->GetName());
			xr_delete(submesh);
			bResult = false;
			break;
		}
	}

	if (bResult){
		exp_obj->UpdateBox		();
		exp_obj->VerifyMeshNames();
		ELog.Msg				(mtInformation,"Object '%s' contains: %d points, %d faces",
								exp_obj->GetName(), exp_obj->GetVertexCount(), exp_obj->GetFaceCount());
	}else{
		xr_delete(exp_obj);
	}
//-------------------------------------------------------------------
	return bResult;
}

void MK_Frustum(CFrustum& F, float FOV, float _FAR, float A, Fvector &P, Fvector &D, Fvector &U)
{
    float YFov	= deg2rad(FOV);
    float XFov	= deg2rad(FOV/A);

    // 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);

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

    F.CreateFromPoints(_F,4,COP);
}

void CSpectator::IR_OnKeyboardHold(int cmd)
{
	if (Remote())		return;

	game_cl_mp* pMPGame = smart_cast<game_cl_mp*> (&Game());
	game_PlayerState* PS = Game().local_player;

	if ((cam_active==eacFreeFly)||(cam_active==eacFreeLook)){
		CCameraBase* C	= cameras	[cam_active];
		Fvector vmove={0,0,0};
		switch(cmd){
		case kUP:
		case kDOWN: 
		case kCAM_ZOOM_IN: 
		case kCAM_ZOOM_OUT: 
			cameras[cam_active]->Move(cmd); break;
		case kLEFT:
		case kRIGHT:
			if (eacFreeLook!=cam_active) cameras[cam_active]->Move(cmd); break;
		case kFWD:			
			vmove.mad( C->vDirection, Device.fTimeDelta*Accel_mul );
			break;
		case kBACK:
			vmove.mad( C->vDirection, -Device.fTimeDelta*Accel_mul );
			break;
		case kR_STRAFE:{
			Fvector right;
			right.crossproduct(C->vNormal,C->vDirection);
			vmove.mad( right, Device.fTimeDelta*Accel_mul );
			}break;
		case kL_STRAFE:{
			Fvector right;
			right.crossproduct(C->vNormal,C->vDirection);
			vmove.mad( right, -Device.fTimeDelta*Accel_mul );
			}break;
		}
		if (cam_active != eacFreeFly || (pMPGame->Is_Spectator_Camera_Allowed(eacFreeFly) || (PS && PS->testFlag(GAME_PLAYER_FLAG_SPECTATOR))))
			XFORM().c.add( vmove );
	}
}

void CPHMovementControl::PathDIrPoint(const xr_vector<DetailPathManager::STravelPathPoint> &path,  int index,  float distance,  float precesition, Fvector &dir  )
{
	Fvector to_path_point;
	Fvector corrected_path_dir;CorrectPathDir(GetPathDir(),path,index,corrected_path_dir);
	to_path_point.sub(vPathPoint,vPosition);	//_new position
	float mag=to_path_point.magnitude();

	if(mag<EPS) //near the point
	{  
		if(0==index||m_path_size-1==index) //on path eidge
		{
			dir.set(corrected_path_dir);//??
			return;
		}
		dir.sub(path[index].position,path[index-1].position);
		dir.normalize_safe();
		dir.add(corrected_path_dir);
		dir.normalize_safe();
	}
	to_path_point.mul(1.f/mag);
	if(m_path_size-1==index)//on_path_edge
	{
		dir.set(to_path_point);
		return;
	}


	if(mag<EPS||fis_zero(dXZMag(to_path_point),EPS))
	{
		dir.set(corrected_path_dir);
		return;//mean dir
	}
	
	Fvector tangent;
	tangent.crossproduct(Fvector().set(0,1,0),to_path_point);

	VERIFY(!fis_zero(tangent.magnitude()));
	tangent.normalize();
	if(dir.square_magnitude()>EPS)
	{
		if(tangent.dotproduct(dir)<0.f)tangent.invert();
	}
	else
	{
		if(tangent.dotproduct(corrected_path_dir)<0.f)tangent.invert();
	}

	if(mag>FootRadius())to_path_point.mul(precesition);
	else to_path_point.mul(mag*precesition);
	dir.add(tangent,to_path_point);
	dir.normalize_safe();
}

ik_goal_matrix::e_collide_state CIKFoot::rotate( Fmatrix &xm, const Fplane& p, const Fvector &foot_normal, const Fvector &global_point, bool collide) const
{
		Fvector ax; ax.crossproduct( p.n, foot_normal );
		float s=ax.magnitude( );
		clamp( s, 0.f, 1.f );
		float angle = asinf( -s );
		VERIFY( _valid( angle ) );
		clamp( angle, -M_PI/6, M_PI/6 );
		ik_goal_matrix::e_collide_state cl_state = ik_goal_matrix::cl_undefined;
		if( !fis_zero( s ) )
		{
			cl_state = ik_goal_matrix::cl_aligned;
			ax.mul( 1.f/s );
			ref_bone_to_foot( xm );
			if( collide )
				cl_state = CollideFoot( angle, angle, global_point, foot_normal, xm.c, p, ax);
			//if( cld.m_pick_dir )
			Fvector c = xm.c;
			xm.mulA_43( Fmatrix( ).rotation( ax,  angle ) );
			xm.c = c;
			foot_to_ref_bone( xm );
		}
		return cl_state;
}

IC void FillSprite	(FVF::LIT*& pv, const Fvector& pos, const Fvector& dir, const Fvector2& lt, const Fvector2& rb, float r1, float r2, u32 clr, float angle)
{
	float sa	= _sin(angle);  
	float ca	= _cos(angle);  
	const Fvector& T 	= dir;
	Fvector R; 	R.crossproduct(T,Device.vCameraDirection).normalize_safe();
	Fvector Vr, Vt;
	Vr.x 		= T.x*r1*sa+R.x*r1*ca;
	Vr.y 		= T.y*r1*sa+R.y*r1*ca;
	Vr.z 		= T.z*r1*sa+R.z*r1*ca;
	Vt.x 		= T.x*r2*ca-R.x*r2*sa;
	Vt.y 		= T.y*r2*ca-R.y*r2*sa;
	Vt.z 		= T.z*r2*ca-R.z*r2*sa;

	Fvector 	a,b,c,d;
	a.sub		(Vt,Vr);
	b.add		(Vt,Vr);
	c.invert	(a);
	d.invert	(b);
	pv->set		(d.x+pos.x,d.y+pos.y,d.z+pos.z, clr, lt.x,rb.y);	pv++;
	pv->set		(a.x+pos.x,a.y+pos.y,a.z+pos.z, clr, lt.x,lt.y);	pv++;
	pv->set		(c.x+pos.x,c.y+pos.y,c.z+pos.z, clr, rb.x,rb.y);	pv++;
	pv->set		(b.x+pos.x,b.y+pos.y,b.z+pos.z,	clr, rb.x,lt.y);	pv++;
}

void CIKLimb::GoalMatrix(Matrix &M,SCalculateData* cd)
{
		VERIFY(cd->m_tri&&cd->m_tri_hight!=-dInfinity);
		const Fmatrix &obj=*cd->m_obj;
		CDB::TRI	*tri=cd->m_tri;
		CKinematics *K=cd->m_K;
		Fvector*	pVerts	= Level().ObjectSpace.GetStaticVerts();
		Fvector normal;
		normal.mknormal	(pVerts[tri->verts[0]],pVerts[tri->verts[1]],pVerts[tri->verts[2]]);
		VERIFY(!fis_zero(normal.magnitude()));

		Fmatrix iobj;iobj.invert(obj);iobj.transform_dir(normal);

		Fmatrix xm;xm.set(K->LL_GetTransform(m_bones[2]));

		//Fvector dbg;
		//dbg.set(Fvector().mul(normal,normal.y*tri_hight
		//	-normal.dotproduct(xm.i)*m_toe_position.x
		//	-normal.dotproduct(xm.j)*m_toe_position.y
		//	-normal.dotproduct(xm.k)*m_toe_position.z-m_toe_position.x
		//	));
		
		normal.invert();
		Fvector ax;ax.crossproduct(normal,xm.i);
		float s=ax.magnitude();
		if(!fis_zero(s))
		{
			ax.mul(1.f/s);

			xm.mulA_43(Fmatrix().rotation(ax,asinf(-s)));
		}

		Fvector otri;iobj.transform_tiny(otri,pVerts[tri->verts[0]]);
		float tp=normal.dotproduct(otri);
		Fvector add;
		add.set(Fvector().mul(normal,-m_toe_position.x+tp-xm.c.dotproduct(normal)));

		xm.c.add(add);
		
	
		Fmatrix H;
		CBoneData& bd=K->LL_GetData(m_bones[0]);
		H.set(bd.bind_transform);
	
		H.mulA_43(K->LL_GetTransform(bd.GetParentID()));
		H.c.set(K->LL_GetTransform(m_bones[0]).c);

	
#ifdef DEBUG
		if(ph_dbg_draw_mask.test(phDbgIKAnimGoalOnly))	xm.set(K->LL_GetTransform(m_bones[2]));
		if(ph_dbg_draw_mask.test(phDbgDrawIKGoal))
		{
			Fmatrix DBGG;
			DBGG.mul_43(obj,xm);
			DBG_DrawMatrix(DBGG,0.2f);

			
			DBGG.mul_43(obj,H);
			DBG_DrawMatrix(DBGG,0.2f);
		}
#endif
		H.invert();
		Fmatrix G; 
		G.mul_43(H,xm);
		XM2IM(G,M);
}

void CBaseMonster::HitEntity(const CEntity *pEntity, float fDamage, float impulse, Fvector &dir, ALife::EHitType hit_type, bool draw_hit_marks)
{
	if (!g_Alive()) return;
	if (!pEntity || pEntity->getDestroy()) return;

	if (!EnemyMan.get_enemy()) return;

	if (EnemyMan.get_enemy() == pEntity) {
		Fvector position_in_bone_space;
		position_in_bone_space.set(0.f,0.f,0.f);

		// перевод из локальных координат в мировые вектора направления импульса
		Fvector hit_dir;
		XFORM().transform_dir	(hit_dir,dir);
		hit_dir.normalize		();

		CEntity		*pEntityNC	= const_cast<CEntity*>(pEntity);
		VERIFY		(pEntityNC);
		
		NET_Packet	l_P;
		SHit		HS;
		HS.GenHeader(GE_HIT, pEntityNC->ID());													//		u_EventGen	(l_P,GE_HIT, pEntityNC->ID());
		HS.whoID			= (ID());															//		l_P.w_u16	(ID());
		HS.weaponID			= (ID());															//		l_P.w_u16	(ID());
		HS.dir				= (hit_dir);														//		l_P.w_dir	(hit_dir);
		HS.power			= (fDamage);														//		l_P.w_float	(fDamage);
		HS.boneID			= (smart_cast<IKinematics*>(pEntityNC->Visual())->LL_GetBoneRoot());//		l_P.w_s16	(smart_cast<IKinematics*>(pEntityNC->Visual())->LL_GetBoneRoot());
		HS.p_in_bone_space	= (position_in_bone_space);											//		l_P.w_vec3	(position_in_bone_space);
		HS.impulse			= (impulse);														//		l_P.w_float	(impulse);
		HS.hit_type			= hit_type;															//		l_P.w_u16	( u16(ALife::eHitTypeWound) );
		HS.Write_Packet(l_P);
		u_EventSend	(l_P);
		
		if (pEntityNC == Actor() && draw_hit_marks) {
			START_PROFILE("BaseMonster/Animation/HitEntity");

			SDrawStaticStruct* s = CurrentGameUI()->AddCustomStatic("monster_claws", false);
			
			float h1,p1;
			Device.vCameraDirection.getHP	(h1,p1);
			Fvector hd				= hit_dir;
			hd.mul					(-1);
			float d = -h1 + hd.getH	();
			s->wnd()->SetHeading	(d);
			Fvector2 wnd_pos = s->wnd()->GetWndPos();
			wnd_pos.y	+= 400.0f*_cos(d);
			wnd_pos.x	+= 500.0f*_sin(d);
			s->wnd()->SetWndPos(wnd_pos);

			STOP_PROFILE;

			//SetAttackEffector			();
			
			float time_to_lock		= fDamage * MAX_LOCK_TIME;
			clamp					(time_to_lock, 0.f, MAX_LOCK_TIME);
			Actor()->lock_accel_for	(int(time_to_lock * 1000));

			//////////////////////////////////////////////////////////////////////////
			//
			//////////////////////////////////////////////////////////////////////////
			
			CEffectorCam* ce = Actor()->Cameras().GetCamEffector((ECamEffectorType)effBigMonsterHit);
			if(!ce)
			{
				const shared_str&	eff_sect = pSettings->r_string(cNameSect(), "actor_hit_effect");	
				if(eff_sect.c_str())
				{
					int id						= -1;
					Fvector						cam_pos,cam_dir,cam_norm;
					Actor()->cam_Active()->Get	(cam_pos,cam_dir,cam_norm);
					cam_dir.normalize_safe		();
					dir.normalize_safe			();

					float ang_diff				= angle_difference	(cam_dir.getH(), dir.getH());
					Fvector						cp;
					cp.crossproduct				(cam_dir,dir);
					bool bUp					=(cp.y>0.0f);

					Fvector cross;
					cross.crossproduct			(cam_dir, dir);
					VERIFY						(ang_diff>=0.0f && ang_diff<=PI);

					float _s1 = PI_DIV_8;
					float _s2 = _s1+PI_DIV_4;
					float _s3 = _s2+PI_DIV_4;
					float _s4 = _s3+PI_DIV_4;

					if(ang_diff<=_s1){
						id = 2;
					}else {
						if(ang_diff>_s1 && ang_diff<=_s2){
							id = (bUp)?5:7;
						}else
							if(ang_diff>_s2 && ang_diff<=_s3){
								id = (bUp)?3:1;
							}else
								if(ang_diff>_s3 && ang_diff<=_s4){
									id = (bUp)?4:6;
								}else
									if(ang_diff>_s4){
//.........这里部分代码省略.........

void CActor::HitMark	(float P, 
						 Fvector dir,			
						 CObject* who, 
						 s16 element, 
						 Fvector position_in_bone_space, 
						 float impulse,  
						 ALife::EHitType hit_type)
{
	// hit marker
	if ( (hit_type==ALife::eHitTypeFireWound||hit_type==ALife::eHitTypeWound_2) && g_Alive() && Local() && /*(this!=who) && */(Level().CurrentEntity()==this) )	
	{
		HUD().Hit(0, P, dir);

	{
		CEffectorCam* ce = Cameras().GetCamEffector((ECamEffectorType)effFireHit);
		if(!ce)
			{
			int id						= -1;
			Fvector						cam_pos,cam_dir,cam_norm;
			cam_Active()->Get			(cam_pos,cam_dir,cam_norm);
			cam_dir.normalize_safe		();
			dir.normalize_safe			();

			float ang_diff				= angle_difference	(cam_dir.getH(), dir.getH());
			Fvector						cp;
			cp.crossproduct				(cam_dir,dir);
			bool bUp					=(cp.y>0.0f);

			Fvector cross;
			cross.crossproduct			(cam_dir, dir);
			VERIFY						(ang_diff>=0.0f && ang_diff<=PI);

			float _s1 = PI_DIV_8;
			float _s2 = _s1+PI_DIV_4;
			float _s3 = _s2+PI_DIV_4;
			float _s4 = _s3+PI_DIV_4;

			if(ang_diff<=_s1){
				id = 2;
			}else
			if(ang_diff>_s1 && ang_diff<=_s2){
				id = (bUp)?5:7;
			}else
			if(ang_diff>_s2 && ang_diff<=_s3){
				id = (bUp)?3:1;
			}else
			if(ang_diff>_s3 && ang_diff<=_s4){
				id = (bUp)?4:6;
			}else
			if(ang_diff>_s4){
				id = 0;
			}else{
				VERIFY(0);
			}

			string64 sect_name;
			sprintf(sect_name,"effector_fire_hit_%d",id);
			AddEffector(this, effFireHit, sect_name, P/100.0f);
			}
		}
	}

}

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

void CHelicopter::MoveStep()
{
    Fvector dir, pathDir;
    float desired_H = m_movement.currPathH;
    float desired_P;
    if(m_movement.type != eMovNone) {

        float dist = m_movement.currP.distance_to(m_movement.desiredPoint);

        dir.sub(m_movement.desiredPoint,m_movement.currP);
        dir.normalize_safe();
        pathDir = dir;
        dir.getHP(desired_H, desired_P);
        float speed_ = _min(m_movement.GetSpeedInDestPoint(), GetMaxVelocity() );

        static float ang = pSettings->r_float	(cNameSect(),"magic_angle");
        if(m_movement.curLinearSpeed>GetMaxVelocity() || angle_difference(m_movement.currPathH,desired_H)>ang)
            m_movement.curLinearAcc = -m_movement.LinearAcc_bk;
        else
            m_movement.curLinearAcc = GetCurrAcc(	m_movement.curLinearSpeed,
                                                    speed_,
                                                    dist*0.95f,
                                                    m_movement.LinearAcc_fw,
                                                    -m_movement.LinearAcc_bk);


        angle_lerp	(m_movement.currPathH, desired_H, m_movement.GetAngSpeedHeading(m_movement.curLinearSpeed), STEP);
        angle_lerp	(m_movement.currPathP, desired_P, m_movement.GetAngSpeedPitch(m_movement.curLinearSpeed), STEP);

        dir.setHP(m_movement.currPathH, m_movement.currPathP);

        float vp = m_movement.curLinearSpeed*STEP+(m_movement.curLinearAcc*STEP*STEP)/2.0f;
        m_movement.currP.mad	(dir, vp);
        m_movement.curLinearSpeed += m_movement.curLinearAcc*STEP;
        static bool aaa = false;
        if(aaa)
            Log("1-m_movement.curLinearSpeed=",m_movement.curLinearSpeed);
        clamp(m_movement.curLinearSpeed,0.0f,1000.0f);
        if(aaa)
            Log("2-m_movement.curLinearSpeed=",m_movement.curLinearSpeed);
    } else { //go stopping
        if( !fis_zero(m_movement.curLinearSpeed) ) {
            m_movement.curLinearAcc = -m_movement.LinearAcc_bk;

            float vp = m_movement.curLinearSpeed*STEP+(m_movement.curLinearAcc*STEP*STEP)/2.0f;
            dir.setHP(m_movement.currPathH, m_movement.currPathP);
            dir.normalize_safe();
            m_movement.currP.mad	(dir, vp);
            m_movement.curLinearSpeed += m_movement.curLinearAcc*STEP;
            clamp(m_movement.curLinearSpeed,0.0f,1000.0f);
//			clamp(m_movement.curLinearSpeed,0.0f,m_movement.maxLinearSpeed);

        } else {
            m_movement.curLinearAcc		= 0.0f;
            m_movement.curLinearSpeed	= 0.0f;
        }

    };

    if(	m_body.b_looking_at_point) {
        Fvector desired_dir;
        desired_dir.sub(m_body.looking_point, m_movement.currP ).normalize_safe();

        float center_desired_H,tmp_P;
        desired_dir.getHP(center_desired_H, tmp_P);
        angle_lerp			(m_body.currBodyHPB.x, center_desired_H, m_movement.GetAngSpeedHeading(m_movement.curLinearSpeed), STEP);
    } else {
        angle_lerp			(m_body.currBodyHPB.x, m_movement.currPathH, m_movement.GetAngSpeedHeading(m_movement.curLinearSpeed), STEP);
    }


    float needBodyP = -m_body.model_pitch_k*m_movement.curLinearSpeed;
    if(m_movement.curLinearAcc < 0) needBodyP*=-1;
    angle_lerp	(m_body.currBodyHPB.y, needBodyP, m_body.model_angSpeedPitch, STEP);


    float sign;
    Fvector cp;
    cp.crossproduct (pathDir,dir);
    (cp.y>0.0)?sign=1.0f:sign=-1.0f;
    float ang_diff = angle_difference (m_movement.currPathH, desired_H);

    float needBodyB = -ang_diff*sign*m_body.model_bank_k*m_movement.curLinearSpeed;
    angle_lerp	(m_body.currBodyHPB.z, needBodyB, m_body.model_angSpeedBank, STEP);


    XFORM().setHPB(m_body.currBodyHPB.x,m_body.currBodyHPB.y,m_body.currBodyHPB.z);

    XFORM().translate_over(m_movement.currP);
}

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

		}
		else
		{
			CPHJoint* J	= (CPHJoint*) dJointGetData(joint);
			if(!J)continue;//hack..
			J->PSecondElement()->InterpolateGlobalPosition(&joint_position);
			CODEGeom* root_geom=J->RootGeom();
			if(root_geom)
			{
				u16 el_position=root_geom->element_position();
				if(element==J->PFirst_element()&&
					el_position<element->numberOfGeoms()&&
					el_position>=m_start_geom_num&&
					el_position<m_end_geom_num
					) applied_to_second=true;
			}
		}
		//accomulate forces applied by joints to first and second parts
		Fvector body_to_joint;
		body_to_joint.sub(joint_position,body_global_pos);
		if(applied_to_second)
		{
			Fvector shoulder;
			shoulder.sub(body_to_joint,body_to_second);
			if(b_body_second)
			{

				Fvector joint_force;
				joint_force.set(*(const Fvector*)feedback->f2);
				second_part_force.add(joint_force);
				Fvector torque;
				torque.crossproduct(shoulder,joint_force);
				second_part_torque.add(torque);

			}
			else
			{

				Fvector joint_force;
				joint_force.set(*(const Fvector*)feedback->f1);
				second_part_force.add(joint_force);

				Fvector torque;
				torque.crossproduct(shoulder,joint_force);
				second_part_torque.add(torque);
			}
		}
		else
		{
			Fvector shoulder;
			shoulder.sub(body_to_joint,body_to_first);
			if(b_body_second)
			{

				Fvector joint_force;
				joint_force.set(*(const Fvector*)feedback->f2);
				first_part_force.add(joint_force);
				Fvector torque;
				torque.crossproduct(shoulder,joint_force);
				first_part_torque.add(torque);
			}
			else
			{
				Fvector joint_force;