Adding minimum distance (head position to all LS) compensation

include/ITANCTC.h

@@ -50,7 +50,7 @@ class ITADatasource;
   * Crosstalk-Cancellation-Engine for arbitrary loudspeaker
   *					configurations using pseudo-inverse matrix solution with
   *					all channels active
-  * 
+  *
   */
 class ITA_CTC_API ITANCTC
 {
@@ -118,7 +118,7 @@ public:
 		int N;					//!< Number of loudspeakers
 		int iCTCFilterLength;	//!< CTC filter taps, i.e. 4096
 		double dSampleRate;		//!< Sampling rate
-		float fSpeedOfSound;	//!< Speed of sound, m/s
+		float dSpeedOfSound;	//!< Speed of sound, m/s
 		int iOptimization;		//!< Optimization algorithm (future work, see features/room_compensation)
 		float fCrossTalkCancellationFactor; //!< Factor for cross-talk cancellation (none = 0.0, full = 1.0)
 		float fWaveIncidenceAngleCompensationFactor; //!< Factor for cross-talk cancellation (none = 0.0, full = 1.0)
@@ -144,8 +144,8 @@ public:
 
 	//! Constructor
 	/**
-	  * Creates the NCTC module for N selected loudspeakers using the poses (position & orientation) 
-	  * given by the specified configuration file. The number of loudspeakers is defined 
+	  * Creates the NCTC module for N selected loudspeakers using the poses (position & orientation)
+	  * given by the specified configuration file. The number of loudspeakers is defined
 	  * through the configuration's SelectedSpeakers list. The configuration may have more
 	  * loudspeakers defined than are selected in the end.
 	  *
@@ -217,9 +217,9 @@ public:
 
 	//! Set additional delay parameter in seconds (affects all channels)
 	/**
-      * The default value is half of the filter length.
-      *
-      * @note Will overwrite existing individual values.
+	  * The default value is half of the filter length.
+	  *
+	  * @note Will overwrite existing individual values.
 	  *
 	  * @param fDelayTime Set the time delay (all channels)
 	  */
@@ -230,12 +230,12 @@ public:
 	  * The CTC filter set for each channel (loudspeaker) will be delayd by given time. This is
 	  * helpful to overcome latency issues with different types of loudspeakers.
 	  *
-      * The default value is half of the filter length.
-      *
+	  * The default value is half of the filter length.
+	  *
 	  * \param vfDelayTime Set the time delay (each channel/loudspeaker individually)
 	  */
 	void SetDelayTime( std::vector< float > vfDelayTime );
-	
+
 	//! Sets the optimization
 	/**
 	  * Optimization setter, i.e. early reflection compensation for aixCAVE (see enum of Config)
@@ -262,7 +262,7 @@ public:
 	  *
 	  */
 	void SetHRIR( const DAFFContentIR* pHRIR );
-	
+
 	//! Update the head position and orientation (combination = pose)
 	/**
 	  * Use this method to update the head pose (position and/or orientation). This is
@@ -284,7 +284,13 @@ public:
 	  * \important	Not thread safe
 	  */
 	Pose GetHeadPose() const;
-	
+
+	//! Returns the minimum distance of head to all loudspeakers
+	/**
+	  *
+	  */
+	float GetMinimumDistanceHead2LS() const;
+
 	//! Calculate the CTC filters
 	/**
 	  * Starts calculation
@@ -307,7 +313,7 @@ private:
 	std::atomic< float > m_fCrossTalkCancellationFactor;		//!< Factor for cross-talk cancellation (none = 0.0, full = 1.0)
 	std::atomic< float > m_fWaveIncidenceAngleCompensationFactor;		//!< Factor for cross-talk cancellation (none = 0.0, full = 1.0)
 	const DAFFContentIR* m_pHRIR;	//!< HRIR dataset pointer
-	
+
 	Pose m_oHeadPose;				//!< Current head Pose data
 
 	ITAFFT m_fft, m_ifft;			//!< Internal FFT and IFFT transformations
@@ -318,7 +324,7 @@ private:
 	std::vector< ITAHDFTSpectra* > m_vpHelper2x2;	//!< Two-by-two helper matrix
 	ITAHDFTSpectrum* t;		//!< Helper
 	ITAHDFTSpectrum* det;	//!< Helper
-	int GetLoudspeakerSide(int);
+	int GetLoudspeakerSide( int );
 	//! Adds a HRIR into the target filter
 	/**
 	* @param oLoudspeakerPose	Pose of loudspeaker (or virtual source)
@@ -327,7 +333,7 @@ private:
 	*
 	* @return True, if samples from HRIR could be copied into target filter, false, if target filter is too short
 	*/
-	bool AddHRIR( const Pose& oLoudspeakerPose, ITASampleFrame& sfTargetIR, bool& bOutOfRange, double dGain = 1.0f ) const;
+	void AddHRIR( const Pose& oLoudspeakerPose, ITASampleFrame& sfTargetIR, bool& bOutOfRange, const double dGain = 1.0f, const int iDistanceCompensationSamples = 0 ) const;
 
 	friend class ITACTCStreamFilter;
 };

src/ITANCTC.cpp

@@ -87,28 +87,6 @@ void ITANCTC::UpdateHeadPose( const Pose& oHead )
 int ITANCTC::GetLoudspeakerSide( int iNumLoudspeaker )
 {
 	Pose oDest = GetLoudspeakerPose( iNumLoudspeaker );
-	VistaVector3D vConn = oDest.vPos - m_oHeadPose.vPos;
-	double dDistanceMeters = double( vConn.GetLength() );
-	float fLS2HeadDelaySamples = ( float ) dDistanceMeters / m_oConfig.fSpeedOfSound * ( float ) m_oConfig.dSampleRate;
-
-	// 1/r attenuation due to distance law
-	// Note: Gain of HRIR dataset is normalized to 1 m according to convention
-	float fDistanceGain = 1 / ( float ) dDistanceMeters;
-
-	// @todo jst: mit VistaQuaternion lsen
-	/*
-	VistaVector3D oFrom = vConn;
-	oFrom.Normalize();
-	VistaQuaternion qOri( Vista::ViewVector, oFrom );
-	VistaQuaternion qHead2LS = qOri * m_oHeadPose.qOrient;
-	float fX, fY, fZ;
-	qHead2LS.GetAngles( fX, fY, fZ );
-
-	VistaVector3D v3View = qHead2LS.GetViewDir();
-
-	float fPhi = 180.0f / PI_F * fY;
-	float fTheta = 180.0f / PI_F * fX;
-	*/
 
 	double dPhiDeg, dThetaDeg;
 
@@ -130,15 +108,14 @@ int ITANCTC::GetLoudspeakerSide( int iNumLoudspeaker )
 
 }
 
-bool ITANCTC::AddHRIR( const Pose& oDest, ITASampleFrame& sfDestHRIR, bool& bOutOfRange, double dReflectionFactor/* = 1.0f */ ) const
+void ITANCTC::AddHRIR( const Pose& oDest, ITASampleFrame& sfDestHRIR, bool& bOutOfRange, const double dReflectionFactor/* = 1.0f */, const int iDistanceCompensationSamples /* = 0 */ ) const
 {
 	if( sfDestHRIR.channels() != 2 )
 		ITA_EXCEPT1( INVALID_PARAMETER, "Two channel HRIR expected" );
 
 	// Calculate sample delay of HRIR insertion position
-	VistaVector3D vConn = oDest.vPos - m_oHeadPose.vPos;
-	double dDistanceMeters = double( vConn.GetLength() );
-	float fLS2HeadDelaySamples = ( float ) dDistanceMeters / m_oConfig.fSpeedOfSound * ( float ) m_oConfig.dSampleRate;
+	VistaVector3D v3Conn = oDest.vPos - m_oHeadPose.vPos;
+	double dDistanceMeters = double( v3Conn.GetLength() );
 
 	// 1/r attenuation due to distance law
 	// Note: Gain of HRIR dataset is normalized to 1 m according to convention
@@ -161,30 +138,29 @@ bool ITANCTC::AddHRIR( const Pose& oDest, ITASampleFrame& sfDestHRIR, bool& bOut
 
 	double dPhiDeg, dThetaDeg;
 
-	VistaVector3D vDir = oDest.vPos - m_oHeadPose.vPos;
-	vDir.Normalize();
+	VistaVector3D vDir = v3Conn.GetNormalized();
 	VistaVector3D vViewMinusZ = m_oHeadPose.vView * ( -1.0f ); // local z axis
 	const VistaVector3D vRight = vViewMinusZ.Cross( m_oHeadPose.vUp ); // local x axis
 	const double dAzimuthAngleDeg = atan2( vDir.Dot( vRight ), vDir.Dot( m_oHeadPose.vView ) ) * 180.0f / ITAConstants::PI_D;
 	dPhiDeg = ( ( dAzimuthAngleDeg < 0.0f ) ? ( dAzimuthAngleDeg + 360.0f ) : dAzimuthAngleDeg );
 	dThetaDeg = asin( vDir.Dot( m_oHeadPose.vUp ) ) * 180.0f / ITAConstants::PI_D;
 
+	// Determine offset of target IR subtracting also the minimum distance to all LS
 	int iHRIRPreOffset = m_pHRIR->getMinEffectiveFilterOffset();
 	int iHRIRFilerTaps = m_pHRIR->getMaxEffectiveFilterLength();
-	int iOffset = ( std::max )( 0, int( fLS2HeadDelaySamples - iHRIRPreOffset ) ); // remove starting zeros from HRIR
-
-	iOffset = int( fLS2HeadDelaySamples ) + iHRIRPreOffset;
+	int iLS2HeadDelaySamples = int( dDistanceMeters / m_oConfig.dSpeedOfSound * m_oConfig.dSampleRate );
+	int iOffset = ( std::max )( 0, ( int( iLS2HeadDelaySamples ) - iHRIRPreOffset - iDistanceCompensationSamples ) ); // remove starting zeros from HRIR
 
-	// Check against buffer overrun, prevent if necessary (PROBLEM: LS too far away for target filter)
+	// Check against buffer overrun, prevent if necessary and clamp to sweet spot boundary (end of IR)
 	if( iOffset + iHRIRFilerTaps > sfDestHRIR.length() )
-		return false;
+		iOffset = sfDestHRIR.length() - iHRIRFilerTaps;
 
 	int iRecordIndex;
 	m_pHRIR->getNearestNeighbour( DAFF_OBJECT_VIEW, float( dPhiDeg ), float( dThetaDeg ), iRecordIndex, bOutOfRange );
 	m_pHRIR->addFilterCoeffs( iRecordIndex, 0, sfDestHRIR[ 0 ].data() + iOffset, fDistanceGain * float( dReflectionFactor ) );
 	m_pHRIR->addFilterCoeffs( iRecordIndex, 1, sfDestHRIR[ 1 ].data() + iOffset, fDistanceGain * float( dReflectionFactor ) );
 
-	return true;
+	return;
 }
 
 const ITANCTC::Pose& ITANCTC::GetLoudspeakerPose( int iLoudspeakerID ) const
@@ -200,6 +176,9 @@ bool ITANCTC::CalculateFilter( std::vector< ITAHDFTSpectra* >& vpCTCFilter )
 	if( !m_pHRIR )
 		return false;
 
+	float fMinDistance = GetMinimumDistanceHead2LS();
+	int iMinDistanceCompensationSamples = int( fMinDistance / m_oConfig.dSpeedOfSound * m_oConfig.dSampleRate );
+
 	bool bOutOfRange = false;
 	for( int n = 0; n < GetN(); n++ )
 	{
@@ -210,13 +189,12 @@ bool ITANCTC::CalculateFilter( std::vector< ITAHDFTSpectra* >& vpCTCFilter )
 			// Here comes more if ready ... see feature/room_compensation branch
 		case Config::OPTIMIZATION_NONE:
 		default:
-			if( AddHRIR( GetLoudspeakerPose( n + 1 ), m_sfCTC_temp, bOutOfRange ) == false )
-				return false;
+			AddHRIR( GetLoudspeakerPose( n + 1 ), m_sfCTC_temp, bOutOfRange, 1.0f, iMinDistanceCompensationSamples );
 			break;
 		}
 
 		if( bOutOfRange )
-			return false;
+			ITA_EXCEPT_INVALID_PARAMETER( "CTC filters could not be created because HRIR direction is out of bounds and not covered by input HRIR" );
 
 		ITAHDFTSpectra* pHRTF( m_vpHRTFs[ n ] );
 
@@ -244,22 +222,21 @@ bool ITANCTC::CalculateFilter( std::vector< ITAHDFTSpectra* >& vpCTCFilter )
 	/* Sweet spot optimization: modify input HRTFs
 	 *   1. Crosstalk side will be lowered by CTC factor
 	 *   2. HRTF will be flattened by WICK factor
-	 */ 
+	 */
 
 
 	// Prepare HRTF spectra
 
-	int iDFTSize = m_oConfig.iCTCFilterLength + 1;
 	for( int n = 0; n < GetN(); n++ )
 	{
 		ITAHDFTSpectra* pHRTF( m_vpHRTFs[ n ] ); // two-channel
 
 
 		//  --- WICK factor ---
-		
+
 		// First, store original energy of HRTF (left channel)
 		float fEnergy = ( *pHRTF )[ 0 ]->getEnergy();
-		
+
 		// Apply WICK factor only on magnitudes (left channel)
 		for( int i = 0; i < ( *pHRTF )[ 0 ]->getSize(); i++ )
 		{
@@ -295,26 +272,26 @@ bool ITANCTC::CalculateFilter( std::vector< ITAHDFTSpectra* >& vpCTCFilter )
 
 		//  --- CTC compensation factor ---
 		// CTC compensation factors
-		int iLSSide = GetLoudspeakerSide(n + 1);
+		int iLSSide = GetLoudspeakerSide( n + 1 );
 
 		float fRightChannelCTC = 1.0f;
-		if (iLSSide == Config::Loudspeaker::LEFT_SIDE)
+		if( iLSSide == Config::Loudspeaker::LEFT_SIDE )
 			fRightChannelCTC *= m_fCrossTalkCancellationFactor; // only apply to left channel if LS is at left side
 
 		float fLeftChannelCTC = 1.0f;
-		if (iLSSide == Config::Loudspeaker::RIGHT_SIDE)
+		if( iLSSide == Config::Loudspeaker::RIGHT_SIDE )
 			fLeftChannelCTC *= m_fCrossTalkCancellationFactor; // only apply to right channel if LS is at right side
-		
-		(*pHRTF)[0]->mul(fLeftChannelCTC);
-		(*pHRTF)[1]->mul(fRightChannelCTC);
-		
+
+		( *pHRTF )[ 0 ]->mul( fLeftChannelCTC );
+		( *pHRTF )[ 1 ]->mul( fRightChannelCTC );
+
 #ifdef NCTC_EXPORT_FILTER_TO_HARDDRIVE
 		ITAFFTUtils::Export(pHRTF, "HRIR_LS" + IntToString(n + 1) + "_WICKedPlusCTCFactor");
 #endif // NCTC_EXPORT_FILTER_TO_HARDDRIVE
-		
+
 		//  --- Weighting ---
 		float fWeight = float( m_vdWeights[ n ] ); // diag element
-				
+
 		// Element wise (a and d): HWH* -> 2x2
 		t->copy( ( *pHRTF )[ 0 ] );
 		t->mul( fWeight );
@@ -558,6 +535,18 @@ ITANCTC::Pose ITANCTC::GetHeadPose() const
 }
 
 
+float ITANCTC::GetMinimumDistanceHead2LS() const
+{
+	assert( GetN() > 0 );
+	float fMinDistance = ( m_oHeadPose.vPos - GetLoudspeakerPose( 0 ).vPos ).GetLength();
+	for( int n = 1; n < GetN(); n++ )
+	{
+		if( ( m_oHeadPose.vPos - GetLoudspeakerPose( n ).vPos ).GetLength() < fMinDistance )
+			fMinDistance = ( m_oHeadPose.vPos - GetLoudspeakerPose( n ).vPos ).GetLength();
+	}
+	return fMinDistance;
+}
+
 // --- Loudspeaker ---
 
 ITANCTC::Config::Loudspeaker::Loudspeaker()
@@ -607,7 +596,7 @@ ITANCTC::Config::Config()
 	// Set some default values
 	N = 0;
 	iCTCFilterLength = 4096;
-	fSpeedOfSound = 344.0f;
+	dSpeedOfSound = 344.0f;
 	iOptimization = OPTIMIZATION_NONE;
 	fCrossTalkCancellationFactor = 1.0f;
 	fWaveIncidenceAngleCompensationFactor = 1.0f;