C++ AngleMatrix函數代碼示例

//-----------------------------------------------------------------------------
// Purpose: 
//-----------------------------------------------------------------------------
void CPropServerVehicleManhack::GetVehicleViewPosition( int nRole, Vector *pAbsOrigin, QAngle *pAbsAngles, float *pFOV )
{
	Assert( nRole == VEHICLE_ROLE_DRIVER );
	CBasePlayer *pPlayer = ToBasePlayer( GetDrivableVehicle()->GetDriver() );
	Assert( pPlayer );//*/

	//commented out because this really should be setting the manhack angle, not the vehicle angle
	*pAbsAngles = pPlayer->EyeAngles(); // yuck. this is an in/out parameter.
	//*pAbsOrigin = pPlayer->EyePosition();

	CNPC_Manhack *pManhack=NULL;

	if (GetManhack())
		pManhack=GetManhack()->GetManhack();
	
	if (pManhack != NULL) 
	{
		Vector  m_vecManhackEye = GetManhack()->GetManhackEyePosition(); //pManhack->GetManhackView();
		QAngle m_angManhackEye = pManhack->GetAbsAngles();

		matrix3x4_t vehicleEyePosToWorld;

		AngleMatrix( m_angManhackEye, vehicleEyePosToWorld );

		// Dampen the eye positional change as we drive around.
		//*pAbsAngles = pPlayer->EyeAngles();

		CPropVehicleManhack *pDriveable = assert_cast<CPropVehicleManhack*>(GetDrivableVehicle());
		if (pDriveable) pDriveable->DampenEyePosition( m_vecManhackEye, m_angManhackEye );
	
		// Compute the relative rotation between the unperturbed eye attachment + the eye angles
		matrix3x4_t cameraToWorld;
		AngleMatrix( *pAbsAngles, cameraToWorld );

		matrix3x4_t worldToEyePos;
		MatrixInvert( vehicleEyePosToWorld, worldToEyePos );

		matrix3x4_t vehicleCameraToEyePos;
		ConcatTransforms( worldToEyePos, cameraToWorld, vehicleCameraToEyePos );

		AngleMatrix( m_angManhackEye, m_vecManhackEye, vehicleEyePosToWorld );

		// Now treat the relative eye angles as being relative to this new, perturbed view position...
		matrix3x4_t newCameraToWorld;
		ConcatTransforms( vehicleEyePosToWorld, vehicleCameraToEyePos, newCameraToWorld );

		// output new view abs angles
		MatrixAngles( newCameraToWorld, *pAbsAngles );

		// UNDONE: *pOrigin would already be correct in single player if the HandleView() on the server ran after vphysics
		MatrixGetColumn( newCameraToWorld, 3, *pAbsOrigin );

	} else DevMsg("fail\n");
}

void CAPC2FourWheelServerVehicle::GetVehicleViewPosition( int nRole, Vector *pAbsOrigin, QAngle *pAbsAngles )
{
	//FixMe, wtf?
	#ifndef DEBUG
	Assert( nRole == VEHICLE_DRIVER );
	#endif
	CBaseCombatCharacter *pPlayer = GetPassenger( VEHICLE_ROLE_DRIVER );
	Assert( pPlayer );

	float flPitchFactor=1.0;
	*pAbsAngles = pPlayer->EyeAngles();
	matrix3x4_t vehicleEyePosToWorld;
	Vector vehicleEyeOrigin;
	QAngle vehicleEyeAngles;
	GetAPC()->GetAttachment( "cannon_muzzle", vehicleEyeOrigin, vehicleEyeAngles );
	Vector up,forward;
	GetAPC()->GetVectors(NULL,&forward,&up);
	vehicleEyeOrigin+=(forward*37)+(up*35);
	AngleMatrix( vehicleEyeAngles, vehicleEyePosToWorld );

//#ifdef HL2_DLL
//	// View dampening.
//	if ( r_VehicleViewDampen.GetInt() )
//	{
//		GetAPC()->DampenEyePosition( vehicleEyeOrigin, vehicleEyeAngles );
//	}
//#endif

	// Compute the relative rotation between the unperterbed eye attachment + the eye angles
	matrix3x4_t cameraToWorld;
	AngleMatrix( *pAbsAngles, cameraToWorld );

	matrix3x4_t worldToEyePos;
	MatrixInvert( vehicleEyePosToWorld, worldToEyePos );

	matrix3x4_t vehicleCameraToEyePos;
	ConcatTransforms( worldToEyePos, cameraToWorld, vehicleCameraToEyePos );

	// Now perterb the attachment point
	vehicleEyeAngles.x = RemapAngleRange( PITCH_CURVE_ZERO * flPitchFactor, PITCH_CURVE_LINEAR, vehicleEyeAngles.x );
	vehicleEyeAngles.z = RemapAngleRange( ROLL_CURVE_ZERO * flPitchFactor, ROLL_CURVE_LINEAR, vehicleEyeAngles.z );
	AngleMatrix( vehicleEyeAngles, vehicleEyeOrigin, vehicleEyePosToWorld );

	// Now treat the relative eye angles as being relative to this new, perterbed view position...
	matrix3x4_t newCameraToWorld;
	ConcatTransforms( vehicleEyePosToWorld, vehicleCameraToEyePos, newCameraToWorld );

	// output new view abs angles
	MatrixAngles( newCameraToWorld, *pAbsAngles );

	// UNDONE: *pOrigin would already be correct in single player if the HandleView() on the server ran after vphysics
	MatrixGetColumn( newCameraToWorld, 3, *pAbsOrigin );
}

//-----------------------------------------------------------------------------
// Purpose: 
//-----------------------------------------------------------------------------
void CChoreoGenericServerVehicle::GetVehicleViewPosition( int nRole, Vector *pAbsOrigin, QAngle *pAbsAngles, float *pFOV /*= NULL*/ )
{
	// FIXME: This needs to be reconciled with the other versions of this function!
	Assert( nRole == VEHICLE_ROLE_DRIVER );
	CBasePlayer *pPlayer = ToBasePlayer( GetDrivableVehicle()->GetDriver() );
	Assert( pPlayer );

	// Use the player's eyes instead of the attachment point
	if ( GetVehicle()->m_bForcePlayerEyePoint )
	{
		// Call to BaseClass because CBasePlayer::EyePosition calls this function.
	  *pAbsOrigin = pPlayer->CBaseCombatCharacter::EyePosition();
	  *pAbsAngles = pPlayer->CBaseCombatCharacter::EyeAngles();
		return;
	}

	*pAbsAngles = pPlayer->EyeAngles(); // yuck. this is an in/out parameter.

	float flPitchFactor = 1.0;
	matrix3x4_t vehicleEyePosToWorld;
	Vector vehicleEyeOrigin;
	QAngle vehicleEyeAngles;
	GetVehicle()->GetAttachment( "vehicle_driver_eyes", vehicleEyeOrigin, vehicleEyeAngles );
	AngleMatrix( vehicleEyeAngles, vehicleEyePosToWorld );

	// Compute the relative rotation between the unperterbed eye attachment + the eye angles
	matrix3x4_t cameraToWorld;
	AngleMatrix( *pAbsAngles, cameraToWorld );

	matrix3x4_t worldToEyePos;
	MatrixInvert( vehicleEyePosToWorld, worldToEyePos );

	matrix3x4_t vehicleCameraToEyePos;
	ConcatTransforms( worldToEyePos, cameraToWorld, vehicleCameraToEyePos );

	// Now perterb the attachment point
	vehicleEyeAngles.x = RemapAngleRange( PITCH_CURVE_ZERO * flPitchFactor, PITCH_CURVE_LINEAR, vehicleEyeAngles.x );
	vehicleEyeAngles.z = RemapAngleRange( ROLL_CURVE_ZERO * flPitchFactor, ROLL_CURVE_LINEAR, vehicleEyeAngles.z );

	AngleMatrix( vehicleEyeAngles, vehicleEyeOrigin, vehicleEyePosToWorld );

	// Now treat the relative eye angles as being relative to this new, perterbed view position...
	matrix3x4_t newCameraToWorld;
	ConcatTransforms( vehicleEyePosToWorld, vehicleCameraToEyePos, newCameraToWorld );

	// output new view abs angles
	MatrixAngles( newCameraToWorld, *pAbsAngles );

	// UNDONE: *pOrigin would already be correct in single player if the HandleView() on the server ran after vphysics
	MatrixGetColumn( newCameraToWorld, 3, *pAbsOrigin );
}

//-----------------------------------------------------------------------------
// Purpose: 
//-----------------------------------------------------------------------------
void CASW_Prediction::SetupMove( C_BasePlayer *player, CUserCmd *ucmd, IMoveHelper *pHelper, 
	CMoveData *move )
{
	// Call the default SetupMove code.
	BaseClass::SetupMove( player, ucmd, pHelper, move );
	
	CASW_Player *pASWPlayer = static_cast<CASW_Player*>( player );
	if ( !asw_allow_detach.GetBool() )
	{		
		if ( pASWPlayer && pASWPlayer->GetMarine() )
		{
			// this forces horizontal movement
			move->m_vecAngles.x = 0;
			move->m_vecViewAngles.x = 0;
		}
	}

	CBaseEntity *pMoveParent = player->GetMoveParent();
	if (!pMoveParent)
	{
		move->m_vecAbsViewAngles = move->m_vecViewAngles;
	}
	else
	{
		matrix3x4_t viewToParent, viewToWorld;
		AngleMatrix( move->m_vecViewAngles, viewToParent );
		ConcatTransforms( pMoveParent->EntityToWorldTransform(), viewToParent, viewToWorld );
		MatrixAngles( viewToWorld, move->m_vecAbsViewAngles );
	}
	CASW_MoveData *pASWMove = static_cast<CASW_MoveData*>( move );
	pASWMove->m_iForcedAction = ucmd->forced_action;
	// setup trace optimization
	g_pGameMovement->SetupMovementBounds( move );
}

void CPlayerPickupController::ComputePlayerMatrix( matrix3x4_t &out )
{
	QAngle angles = m_pPlayer->EyeAngles();
	Vector origin = m_pPlayer->EyePosition();
	
	// 0-360 / -180-180
	angles.x = AngleDistance( angles.x, 0 );
	angles.x = clamp( angles.x, -PLAYER_LOOK_PITCH_RANGE, PLAYER_LOOK_PITCH_RANGE );

	float feet = m_pPlayer->GetAbsMins().z;
	float eyes = origin.z;
	float zoffset = 0;
	// moving up (negative pitch is up)
	if ( angles.x < 0 )
	{
		zoffset = RemapVal( angles.x, 0, -PLAYER_LOOK_PITCH_RANGE, PLAYER_HOLD_LEVEL_EYES, PLAYER_HOLD_UP_EYES );
	}
	else
	{
		zoffset = RemapVal( angles.x, 0, PLAYER_LOOK_PITCH_RANGE, PLAYER_HOLD_LEVEL_EYES, PLAYER_HOLD_DOWN_FEET + (feet - eyes) );
	}
//	origin.z += zoffset;
	angles.x = 0;
	AngleMatrix( angles, origin, out );
}

//-----------------------------------------------------------------------------
// Purpose: Prepares for running movement
// Input  : *player - 
//			*ucmd - 
//			*pHelper - 
//			*move - 
//			time - 
//-----------------------------------------------------------------------------
void CPlayerMove::SetupMove( CBasePlayer *player, CUserCmd *ucmd, IMoveHelper *pHelper, CMoveData *move )
{
	VPROF( "CPlayerMove::SetupMove" );

	// Allow sound, etc. to be created by movement code
	move->m_bFirstRunOfFunctions = true;

	// Prepare the usercmd fields
	move->m_nImpulseCommand		= ucmd->impulse;	
	move->m_vecViewAngles		= ucmd->viewangles;

	CBaseEntity *pMoveParent = player->GetMoveParent();
	if (!pMoveParent)
	{
		move->m_vecAbsViewAngles = move->m_vecViewAngles;
	}
	else
	{
		matrix3x4_t viewToParent, viewToWorld;
		AngleMatrix( move->m_vecViewAngles, viewToParent );
		ConcatTransforms( pMoveParent->EntityToWorldTransform(), viewToParent, viewToWorld );
		MatrixAngles( viewToWorld, move->m_vecAbsViewAngles );
	}

	move->m_nButtons			= ucmd->buttons;

	// Ingore buttons for movement if at controls
	if ( player->GetFlags() & FL_ATCONTROLS )
	{
		move->m_flForwardMove		= 0;
		move->m_flSideMove			= 0;
		move->m_flUpMove				= 0;
	}
	else
	{
		move->m_flForwardMove		= ucmd->forwardmove;
		move->m_flSideMove			= ucmd->sidemove;
		move->m_flUpMove				= ucmd->upmove;
	}

	// Prepare remaining fields
	move->m_flClientMaxSpeed		= player->m_flMaxspeed;
	move->m_nOldButtons			= player->m_Local.m_nOldButtons;
	move->m_vecAngles			= player->pl.v_angle;

	move->m_vecVelocity			= player->GetAbsVelocity();

	move->m_nPlayerHandle		= player;

	move->m_vecAbsOrigin		= player->GetAbsOrigin();

	// Copy constraint information
	if ( player->m_hConstraintEntity.Get() )
		move->m_vecConstraintCenter = player->m_hConstraintEntity.Get()->GetAbsOrigin();
	else
		move->m_vecConstraintCenter = player->m_vecConstraintCenter;
	move->m_flConstraintRadius = player->m_flConstraintRadius;
	move->m_flConstraintWidth = player->m_flConstraintWidth;
	move->m_flConstraintSpeedFactor = player->m_flConstraintSpeedFactor;
}

void msModel::SetupJoints()
{
	for (size_t i = 0; i < m_joints.size(); i++)
	{
		ms3d_joint_t *joint = &m_joints[i];
		joint->parentIndex = FindJointByName(joint->parentName);
	}

	for (size_t i = 0; i < m_joints.size(); i++)
	{
		ms3d_joint_t *joint = &m_joints[i];
		AngleMatrix(joint->rot, joint->matLocalSkeleton);
		joint->matLocalSkeleton[0][3]= joint->pos[0];
		joint->matLocalSkeleton[1][3]= joint->pos[1];
		joint->matLocalSkeleton[2][3]= joint->pos[2];

		if (joint->parentIndex == -1)
		{
			memcpy(joint->matGlobalSkeleton, joint->matLocalSkeleton, sizeof(joint->matGlobalSkeleton));
		}
		else
		{
			ms3d_joint_t *parentJoint = &m_joints[joint->parentIndex];
			R_ConcatTransforms(parentJoint->matGlobalSkeleton, joint->matLocalSkeleton, joint->matGlobalSkeleton);
		}

		SetupTangents();
	}
}

static QAngle AlignAngles( const QAngle &angles, float cosineAlignAngle )
{
	matrix3x4_t alignMatrix;
	AngleMatrix( angles, alignMatrix );

	// NOTE: Must align z first
	for ( int j = 3; --j >= 0; )
	{
		Vector vec;
		MatrixGetColumn( alignMatrix, j, vec );
		for ( int i = 0; i < 3; i++ )
		{
			if ( fabs(vec[i]) > cosineAlignAngle )
			{
				vec[i] = SIGN(vec[i]);
				vec[(i+1)%3] = 0;
				vec[(i+2)%3] = 0;
				MatrixSetColumn( vec, j, alignMatrix );
				MatrixOrthogonalize( alignMatrix, j );
				break;
			}
		}
	}

	QAngle out;
	MatrixAngles( alignMatrix, out );
	return out;
}

static void ComputePlayerMatrix( CBasePlayer *pPlayer, matrix3x4_t &out )
{
	if ( !pPlayer )
		return;

	QAngle angles = pPlayer->EyeAngles();
	Vector origin = pPlayer->EyePosition();
	
	// 0-360 / -180-180
	//angles.x = init ? 0 : AngleDistance( angles.x, 0 );
	//angles.x = clamp( angles.x, -PLAYER_LOOK_PITCH_RANGE, PLAYER_LOOK_PITCH_RANGE );
	angles.x = 0;

	float feet = pPlayer->GetAbsOrigin().z + pPlayer->WorldAlignMins().z;
	float eyes = origin.z;
	float zoffset = 0;
	// moving up (negative pitch is up)
	if ( angles.x < 0 )
	{
		zoffset = RemapVal( angles.x, 0, -PLAYER_LOOK_PITCH_RANGE, PLAYER_HOLD_LEVEL_EYES, PLAYER_HOLD_UP_EYES );
	}
	else
	{
		zoffset = RemapVal( angles.x, 0, PLAYER_LOOK_PITCH_RANGE, PLAYER_HOLD_LEVEL_EYES, PLAYER_HOLD_DOWN_FEET + (feet - eyes) );
	}
	origin.z += zoffset;
	angles.x = 0;
	AngleMatrix( angles, origin, out );
}

//-----------------------------------------------------------------------------
// Returns the unperterbed view position for a particular role
//-----------------------------------------------------------------------------
bool CBaseTFVehicle::GetRoleViewPosition( int nRole, Vector *pVehicleEyeOrigin, QAngle *pVehicleEyeAngles )
{
	// Generate the view position in world space.
	Vector vAbsOrigin;
	QAngle vAbsAngle;
	bool bUsingThirdPersonCamera = GetRoleAbsViewPosition( nRole, &vAbsOrigin, &vAbsAngle );

	
	// Make a matrix for it.
	matrix3x4_t absMatrix;
	AngleMatrix( vAbsAngle, absMatrix );
	MatrixSetColumn( vAbsOrigin, 3, absMatrix );


	// Transform the matrix into local space.
	matrix3x4_t worldToEntity, local;
	MatrixInvert( EntityToWorldTransform(), worldToEntity );
	ConcatTransforms( worldToEntity, absMatrix, local ); 


	// Suck out the origin and angles.
	pVehicleEyeOrigin->Init( local[0][3], local[1][3], local[2][3] );
	MatrixAngles( local, *pVehicleEyeAngles );

	return bUsingThirdPersonCamera;
}

inline void VectorRotate(const Vector &i, const Angle &angles, Vector &o)
{
	matrix3x4 matrix;

	AngleMatrix(angles, matrix);
	VectorRotate(i, matrix, o);
}

void C_BaseViewModel::ApplyBoneMatrixTransform( matrix3x4_t& transform )
{
	if ( ShouldFlipViewModel() )
	{
		matrix3x4_t viewMatrix, viewMatrixInverse;

		// We could get MATERIAL_VIEW here, but this is called sometimes before the renderer
		// has set that matrix. Luckily, this is called AFTER the CViewSetup has been initialized.
		const CViewSetup *pSetup = view->GetPlayerViewSetup();
		AngleMatrix( pSetup->angles, pSetup->origin, viewMatrixInverse );
		MatrixInvert( viewMatrixInverse, viewMatrix );

		// Transform into view space.
		matrix3x4_t temp, temp2;
		ConcatTransforms( viewMatrix, transform, temp );
		
		// Flip it along X.
		
		// (This is the slower way to do it, and it equates to negating the top row).
		//matrix3x4_t mScale;
		//SetIdentityMatrix( mScale );
		//mScale[0][0] = 1;
		//mScale[1][1] = -1;
		//mScale[2][2] = 1;
		//ConcatTransforms( mScale, temp, temp2 );
		temp[1][0] = -temp[1][0];
		temp[1][1] = -temp[1][1];
		temp[1][2] = -temp[1][2];
		temp[1][3] = -temp[1][3];

		// Transform back out of view space.
		ConcatTransforms( viewMatrixInverse, temp, transform );
	}
}

//-----------------------------------------------------------------------------
// Initialization
//-----------------------------------------------------------------------------
bool CStaticProp::Init( int index, StaticPropLump_t &lump, model_t *pModel )
{
	m_EntHandle = index | STATICPROP_EHANDLE_MASK; 
	m_Partition = PARTITION_INVALID_HANDLE;

	VectorCopy( lump.m_Origin, m_Origin );
	VectorCopy( lump.m_Angles, m_Angles );
	m_pModel = pModel;
	m_FirstLeaf = lump.m_FirstLeaf;
	m_LeafCount = lump.m_LeafCount;
	m_nSolidType = lump.m_Solid;
	m_Alpha = 255;
	m_Skin = (unsigned char)lump.m_Skin;

	// Cache the collision bounding box since it'll never change.
	modelinfo->GetModelRenderBounds( m_pModel, 0, m_RenderBBoxMin, m_RenderBBoxMax );

	// Cache the model to world matrix since it never changes.
	AngleMatrix( lump.m_Angles, lump.m_Origin, m_ModelToWorld );

	// FIXME: Sucky, but unless we want to re-read the static prop lump when the client is
	// initialized (possible, but also gross), we need to cache off the illum center now
	if (lump.m_Flags & STATIC_PROP_USE_LIGHTING_ORIGIN)
	{
		m_LightingOrigin = lump.m_LightingOrigin;
	}
	else
	{
		modelinfo->GetIlluminationPoint( m_pModel, m_Origin, m_Angles, &m_LightingOrigin );
	}

	return true;
}

CASW_Laser_Mine* CASW_Laser_Mine::ASW_Laser_Mine_Create( const Vector &position, const QAngle &angles, const QAngle &angLaserAim, CBaseEntity *pOwner, CBaseEntity *pMoveParent, bool bFriendly, CBaseEntity *pCreatorWeapon )
{
	CASW_Laser_Mine *pMine = (CASW_Laser_Mine*)CreateEntityByName( "asw_laser_mine" );
	pMine->SetLaserAngle( angLaserAim );
	
	matrix3x4_t wallMatrix;
	AngleMatrix( angles, wallMatrix );

	QAngle angRotateMine( 0, 90, 90 );
	matrix3x4_t fRotateMatrix;
	AngleMatrix( angRotateMine, fRotateMatrix );

	matrix3x4_t finalMatrix;
	QAngle angMine;
	ConcatTransforms( wallMatrix, fRotateMatrix, finalMatrix );
	MatrixAngles( finalMatrix, angMine );

	Vector vecSrc = pOwner->WorldSpaceCenter();
	CASW_Marine *pMarine = dynamic_cast<CASW_Marine*>( pOwner );
	if ( pMarine )
		vecSrc = pMarine->GetOffhandThrowSource();
	pMine->SetAbsAngles( -angMine );
	pMine->Spawn();
	pMine->SetOwnerEntity( pOwner );
	UTIL_SetOrigin( pMine, vecSrc );
	pMine->m_bFriendly = bFriendly;
	pMine->m_hCreatorWeapon.Set( pCreatorWeapon );

	// adjust throw duration based on distance and some randomness
	float flDist = vecSrc.DistTo( position );
	const float flBaseDist = 90.0f;
	float flDistFraction = ( flDist / flBaseDist );
	flDistFraction = clamp<float>( flDistFraction, 0.5f, 2.0f );
	flDistFraction += RandomFloat( 0.0f, 0.2f );
	pMine->StartSpawnFlipping( vecSrc, position, angMine, 0.30f * flDistFraction );

	if( pCreatorWeapon )
		pMine->m_CreatorWeaponClass = pCreatorWeapon->Classify();

	if ( pMoveParent )
	{
		pMine->SetParent( pMoveParent );
		gEntList.AddListenerEntity( pMine );
	}

	return pMine;
}

//-----------------------------------------------------------------------------
// Gets the lighting center
//-----------------------------------------------------------------------------
static void R_StudioGetLightingCenter( studiohdr_t* pStudioHdr, const Vector& origin,
								const QAngle & angles, Vector* pLightingOrigin )
{
	Assert( pLightingOrigin );
	matrix3x4_t matrix;
	AngleMatrix( angles, origin, matrix );
	R_StudioCenter( pStudioHdr, matrix, *pLightingOrigin );
}