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

QAngle CGravControllerPoint::TransformAnglesFromPlayerSpace( const QAngle &anglesIn, CBasePlayer *pPlayer )
{
	matrix3x4_t test;
	QAngle angleTest = pPlayer->EyeAngles();
	angleTest.x = 0;
	AngleMatrix( angleTest, test );
	return TransformAnglesToWorldSpace( anglesIn, test );
}

void CRagdollPropAttached::InitRagdollAttached( IPhysicsObject *pAttached, const Vector &forceVector, int forceBone, matrix3x4_t *pPrevBones, matrix3x4_t *pBoneToWorld, float dt, int collisionGroup, CBaseAnimating *pFollow, int boneIndexRoot, const Vector &boneLocalOrigin, int parentBoneAttach, const Vector &worldAttachOrigin )
{
	int ragdollAttachedIndex = 0;
	if ( parentBoneAttach > 0 )
	{
		studiohdr_t *pStudioHdr = GetModelPtr();
		mstudiobone_t *pBone = pStudioHdr->pBone( parentBoneAttach );
		ragdollAttachedIndex = pBone->physicsbone;
	}

	InitRagdoll( forceVector, forceBone, vec3_origin, pPrevBones, pBoneToWorld, dt, collisionGroup, false );
	
	IPhysicsObject *pRefObject = m_ragdoll.list[ragdollAttachedIndex].pObject;

	Vector attachmentPointRagdollSpace;
	pRefObject->WorldToLocal( attachmentPointRagdollSpace, worldAttachOrigin );

	constraint_ragdollparams_t constraint;
	constraint.Defaults();
	matrix3x4_t tmp, worldToAttached, worldToReference, constraintToWorld;

	Vector offsetWS;
	pAttached->LocalToWorld( offsetWS, boneLocalOrigin );

	AngleMatrix( QAngle(0, pFollow->GetAbsAngles().y, 0 ), offsetWS, constraintToWorld );

	constraint.axes[0].SetAxisFriction( -2, 2, 20 );
	constraint.axes[1].SetAxisFriction( 0, 0, 0 );
	constraint.axes[2].SetAxisFriction( -15, 15, 20 );
	
	pAttached->GetPositionMatrix( tmp );
	MatrixInvert( tmp, worldToAttached );

	pRefObject->GetPositionMatrix( tmp );
	MatrixInvert( tmp, worldToReference );

	ConcatTransforms( worldToReference, constraintToWorld, constraint.constraintToReference );
	ConcatTransforms( worldToAttached, constraintToWorld, constraint.constraintToAttached );

	// for now, just slam this to be the passed in value
	MatrixSetColumn( attachmentPointRagdollSpace, 3, constraint.constraintToReference );

	DisableCollisions( pAttached );
	m_pAttachConstraint = physenv->CreateRagdollConstraint( pRefObject, pAttached, m_ragdoll.pGroup, constraint );

	FollowEntity( pFollow );
	SetOwnerEntity( pFollow );
	RagdollActivate( m_ragdoll );

	Relink();
	m_boneIndexAttached = boneIndexRoot;
	m_ragdollAttachedObjectIndex = ragdollAttachedIndex;
	m_attachmentPointBoneSpace = boneLocalOrigin;
	
	Vector vTemp;
	MatrixGetColumn( constraint.constraintToReference, 3, vTemp );
	m_attachmentPointRagdollSpace = vTemp;
}

void NDebugOverlay::BoxAngles(const Vector &origin, const Vector &mins, const Vector &maxs, const QAngle &angles, int r, int g, int b, int a, float duration)
{
	// --------------------------------------------------------------
	// Have to do clip the boxes before sending so we 
	// don't overflow the client message buffer 
	// --------------------------------------------------------------
	CBasePlayer *player = UTIL_PlayerByIndex(1);
	if ( !player )
		return;

	// ------------------------------------
	// Clip boxes that are far away
	// ------------------------------------
	if ((player->GetAbsOrigin() - origin).LengthSqr() > MAX_OVERLAY_DIST_SQR) 
		return;

	// ------------------------------------
	// Clip boxes that are behind the client 
	// ------------------------------------
	Vector clientForward;
	player->EyeVectors( &clientForward );

	// Build a rotation matrix from orientation
	matrix3x4_t fRotateMatrix;
	AngleMatrix(angles, fRotateMatrix);
	
	// Transform the mins and maxs
	Vector tmins, tmaxs;
	VectorRotate( mins, fRotateMatrix, tmins);
	VectorAdd(tmins,origin,tmins);
	VectorRotate( maxs, fRotateMatrix, tmaxs);
	VectorAdd(tmaxs,origin,tmaxs);

	Vector toMins  = tmins - player->GetAbsOrigin();
	Vector toMaxs  = tmaxs - player->GetAbsOrigin();
 	float  dotMins	 = DotProduct(clientForward,toMins);
 	float  dotMaxs	 = DotProduct(clientForward,toMaxs);
	if (dotMins < 0 && dotMaxs < 0)
		return;

	CSingleUserRecipientFilter user( player );

	MessageBegin( user, SVC_DEBUG_BOX_OVERLAY );
		WRITE_VEC3COORD(origin);
		WRITE_VEC3COORD(mins);
		WRITE_VEC3COORD(maxs);
		WRITE_ANGLES(angles); 
		WRITE_SHORT(r);
		WRITE_SHORT(g);
		WRITE_SHORT(b);
		WRITE_SHORT(a);
		WRITE_FLOAT(duration);
	MessageEnd();

}

//-----------------------------------------------------------------------------
// Purpose: 
// Input  : flConvergencePerc - 
//			vecBasis - 
//-----------------------------------------------------------------------------
void CNPC_Combine_Cannon::UpdateAncillaryBeams( float flConvergencePerc, const Vector &vecOrigin, const Vector &vecBasis )
{
	// Multiple beams deviate from the basis direction by a certain number of degrees and "converge" 
	// at the basis vector over a duration of time, the position in that duration expressed by 
	// flConvergencePerc.  The beams are most deviated at 0 and fully converged at 1.

	float flRotationOffset = (2*M_PI)/(float)NUM_ANCILLARY_BEAMS; // Degrees separating each beam, in radians
	float flDeviation = DEG2RAD(90) * ( 1.0f - flConvergencePerc );
	float flOffset;
	Vector vecFinal;
	Vector vecOffset;
	
	matrix3x4_t matRotate;
	QAngle vecAngles;
	VectorAngles( vecBasis, vecAngles );
	vecAngles[PITCH] += 90.0f;
	AngleMatrix( vecAngles, vecOrigin, matRotate );

	trace_t	tr;

	float flScale = LINE_LENGTH * flDeviation;

	// For each beam, find its offset and trace outwards to place its endpoint
	for ( int i = 0; i < NUM_ANCILLARY_BEAMS; i++ )
	{
		if ( flConvergencePerc >= 0.99f )
		{
			m_pAncillaryBeams[i]->TurnOn();
			continue;
		}

		m_pAncillaryBeams[i]->TurnOff();

		// Find the number of radians offset we are
		flOffset = (float) i * flRotationOffset + DEG2RAD( 30.0f );
		flOffset += (M_PI/8.0f) * sin( gpGlobals->curtime * 3.0f );
		
		// Construct a circle that's also offset by the line's length
		vecOffset.x = cos( flOffset ) * flScale;
		vecOffset.y = sin( flOffset ) * flScale;
		vecOffset.z = LINE_LENGTH;

		// Rotate this whole thing into the space of the basis vector
		VectorRotate( vecOffset, matRotate, vecFinal );
		VectorNormalize( vecFinal );

		// Trace a line down that vector to find where we'll eventually stop our line
		UTIL_TraceLine( vecOrigin, vecOrigin + ( vecFinal * LINE_LENGTH ), MASK_SHOT, this, COLLISION_GROUP_NONE, &tr );

		// Move the beam to that position
		m_pAncillaryBeams[i]->SetBrightness( static_cast<int>(255.0f * flConvergencePerc) );
		m_pAncillaryBeams[i]->SetEndPos( tr.startpos );
		m_pAncillaryBeams[i]->SetStartPos( tr.endpos );
	}
}

//-----------------------------------------------------------------------------
// Purpose: Draw a circle whose center is at a position and is facing a specified direction
//-----------------------------------------------------------------------------
void NDebugOverlay::Circle( const Vector &position, const QAngle &angles, float radius, int r, int g, int b, int a, bool bNoDepthTest, float flDuration )
{
	// Setup our transform matrix
	matrix3x4_t xform;
	AngleMatrix( angles, position, xform );
	Vector xAxis, yAxis;
	// default draws circle in the y/z plane
	MatrixGetColumn( xform, 2, xAxis );
	MatrixGetColumn( xform, 1, yAxis );
	Circle( position, xAxis, yAxis, radius, r, g, b, a, bNoDepthTest, flDuration );
}