//-----------------------------------------------------------------------------
// Purpose:: Convert data to HL2 measurements, and test direction of raycast.
//-----------------------------------------------------------------------------
void CPhysics_Airboat::pre_raycasts_gameside( int nRaycastCount, IVP_Ray_Solver_Template *pRays,
											  Ray_t *pGameRays, IVP_Raycast_Airboat_Impact *pImpacts )
{
	for ( int iRaycast = 0; iRaycast < nRaycastCount; ++iRaycast )
	{
		// Setup the ray.
		Vector vecStart;
		ConvertPositionToHL( pRays[iRaycast].ray_start_point, vecStart );

		Vector vecEnd;
		Vector vecDirection;
		ConvertDirectionToHL( pRays[iRaycast].ray_normized_direction, vecDirection );
		float flRayLength = IVP2HL( pRays[iRaycast].ray_length );

		// Check to see if that point is in water.
		pImpacts[iRaycast].bInWater = IVP_FALSE;
		if ( m_pGameTrace->VehiclePointInWater( vecStart ) )
		{
			vecDirection.Negate();
			pImpacts[iRaycast].bInWater = IVP_TRUE;
		}

		vecEnd = vecStart + ( vecDirection * flRayLength );

		// Shorten the trace.
		if ( m_pGameTrace->VehiclePointInWater( vecEnd ) )
		{
			pRays[iRaycast].ray_length = AIRBOAT_RAYCAST_DIST_WATER;
			flRayLength = IVP2HL( pRays[iRaycast].ray_length );
			vecEnd = vecStart + ( vecDirection * flRayLength );
		}

		Vector vecZero( 0.0f, 0.0f, 0.0f );
		pGameRays[iRaycast].Init( vecStart, vecEnd, vecZero, vecZero );
	}
}
//-----------------------------------------------------------------------------
// Debugging methods
//-----------------------------------------------------------------------------
void CFourWheelVehiclePhysics::DrawDebugGeometryOverlays()
{
	Vector vecRad(m_debugRadius,m_debugRadius,m_debugRadius);
	for ( int i = 0; i < m_wheelCount; i++ )
	{
		NDebugOverlay::BoxAngles(m_wheelPosition[i], -vecRad, vecRad, m_wheelRotation[i], 0, 255, 45, 0 ,0);
	}

	for ( int iWheel = 0; iWheel < m_wheelCount; iWheel++ )
	{
		IPhysicsObject *pWheel = m_pVehicle->GetWheel( iWheel );
		
		Vector vecPos;
		QAngle vecRot;
		pWheel->GetPosition( &vecPos, &vecRot );

		NDebugOverlay::BoxAngles( vecPos, -vecRad, vecRad, vecRot, 0, 255, 45, 0 ,0 );
	}

#if 1
	// Render vehicle data.
	IPhysicsObject *pBody = m_pOuter->VPhysicsGetObject();
	if ( pBody )
	{
		const vehicleparams_t vehicleParams = m_pVehicle->GetVehicleParams();

		// Draw a red cube as the "center" of the vehicle.
		Vector vecBodyPosition; 
		QAngle angBodyDirection;
		pBody->GetPosition( &vecBodyPosition, &angBodyDirection );
		NDebugOverlay::BoxAngles( vecBodyPosition, Vector( -5, -5, -5 ), Vector( 5, 5, 5 ), angBodyDirection, 255, 0, 0, 0 ,0 );

		matrix3x4_t matrix;
		AngleMatrix( angBodyDirection, vecBodyPosition, matrix );

		// Draw green cubes at axle centers.
		Vector vecAxlePositions[2], vecAxlePositionsHL[2];
		vecAxlePositions[0] = vehicleParams.axles[0].offset;
		vecAxlePositions[1] = vehicleParams.axles[1].offset;

		VectorTransform( vecAxlePositions[0], matrix, vecAxlePositionsHL[0] );		
		VectorTransform( vecAxlePositions[1], matrix, vecAxlePositionsHL[1] );

		NDebugOverlay::BoxAngles( vecAxlePositionsHL[0], Vector( -3, -3, -3 ), Vector( 3, 3, 3 ), angBodyDirection, 0, 255, 0, 0 ,0 );
		NDebugOverlay::BoxAngles( vecAxlePositionsHL[1], Vector( -3, -3, -3 ), Vector( 3, 3, 3 ), angBodyDirection, 0, 255, 0, 0 ,0 );

		// Draw blue cubes at wheel centers.
		Vector vecWheelPositions[4], vecWheelPositionsHL[4];
		vecWheelPositions[0] = vehicleParams.axles[0].offset;
		vecWheelPositions[0] += vehicleParams.axles[0].wheelOffset;
		vecWheelPositions[1] = vehicleParams.axles[0].offset;
		vecWheelPositions[1] -= vehicleParams.axles[0].wheelOffset;
		vecWheelPositions[2] = vehicleParams.axles[1].offset;
		vecWheelPositions[2] += vehicleParams.axles[1].wheelOffset;
		vecWheelPositions[3] = vehicleParams.axles[1].offset;
		vecWheelPositions[3] -= vehicleParams.axles[1].wheelOffset;

		VectorTransform( vecWheelPositions[0], matrix, vecWheelPositionsHL[0] );
		VectorTransform( vecWheelPositions[1], matrix, vecWheelPositionsHL[1] );
		VectorTransform( vecWheelPositions[2], matrix, vecWheelPositionsHL[2] );
		VectorTransform( vecWheelPositions[3], matrix, vecWheelPositionsHL[3] );

		float flWheelRadius = vehicleParams.axles[0].wheels.radius;
		flWheelRadius = IVP2HL( flWheelRadius );
		Vector vecWheelRadius( flWheelRadius, flWheelRadius, flWheelRadius );

		NDebugOverlay::BoxAngles( vecWheelPositionsHL[0], -vecWheelRadius, vecWheelRadius, angBodyDirection, 0, 0, 255, 0 ,0 );
		NDebugOverlay::BoxAngles( vecWheelPositionsHL[1], -vecWheelRadius, vecWheelRadius, angBodyDirection, 0, 0, 255, 0 ,0 );
		NDebugOverlay::BoxAngles( vecWheelPositionsHL[2], -vecWheelRadius, vecWheelRadius, angBodyDirection, 0, 0, 255, 0 ,0 );
		NDebugOverlay::BoxAngles( vecWheelPositionsHL[3], -vecWheelRadius, vecWheelRadius, angBodyDirection, 0, 0, 255, 0 ,0 );

		// Draw wheel raycasts in yellow
		vehicle_debugcarsystem_t debugCarSystem;
		m_pVehicle->GetCarSystemDebugData( debugCarSystem );
		for ( int iWheel = 0; iWheel < 4; ++iWheel )
		{
			Vector vecStart, vecEnd, vecImpact;

			// Hack for now.
			float tmpY = IVP2HL( debugCarSystem.vecWheelRaycasts[iWheel][0].z );
			vecStart.z = -IVP2HL( debugCarSystem.vecWheelRaycasts[iWheel][0].y );
			vecStart.y = tmpY;
			vecStart.x = IVP2HL( debugCarSystem.vecWheelRaycasts[iWheel][0].x );

			tmpY = IVP2HL( debugCarSystem.vecWheelRaycasts[iWheel][1].z );
			vecEnd.z = -IVP2HL( debugCarSystem.vecWheelRaycasts[iWheel][1].y );
			vecEnd.y = tmpY;
			vecEnd.x = IVP2HL( debugCarSystem.vecWheelRaycasts[iWheel][1].x );

			tmpY = IVP2HL( debugCarSystem.vecWheelRaycastImpacts[iWheel].z );
			vecImpact.z = -IVP2HL( debugCarSystem.vecWheelRaycastImpacts[iWheel].y );
			vecImpact.y = tmpY;
			vecImpact.x = IVP2HL( debugCarSystem.vecWheelRaycastImpacts[iWheel].x );

			NDebugOverlay::BoxAngles( vecStart, Vector( -1 , -1, -1 ), Vector( 1, 1, 1 ), angBodyDirection, 0, 255, 0, 0, 0  );
			NDebugOverlay::Line( vecStart, vecEnd, 255, 255, 0, true, 0 );
			NDebugOverlay::BoxAngles( vecEnd, Vector( -1, -1, -1 ), Vector( 1, 1, 1 ), angBodyDirection, 255, 0, 0, 0, 0 );

			NDebugOverlay::BoxAngles( vecImpact, Vector( -0.5f , -0.5f, -0.5f ), Vector( 0.5f, 0.5f, 0.5f ), angBodyDirection, 0, 0, 255, 0, 0  );
		}
	}
#endif
}