C++ Matrix4f::Inverted方法代码示例

Matrix4f CalculateCameraTimeWarpMatrix( const Quatf &from, const Quatf &to )
{
    Matrix4f		lastSensorMatrix = Matrix4f( to ).Transposed();
    Matrix4f		lastViewMatrix = Matrix4f( from ).Transposed();
	Matrix4f		timeWarp = ( lastSensorMatrix * lastViewMatrix.Inverted() );

	return timeWarp;
}

// -1 to 1 range on panelMatrix, returns -2,-2 if looking away from the panel
Vector2f	GazeCoordinatesOnPanel( const Matrix4f & viewMatrix, const Matrix4f panelMatrix, const bool floatingPanel )
{
	// project along -Z in the viewMatrix onto the Z = 0 plane of activityMatrix

	const Vector3f viewForward = MatrixForward( viewMatrix ).Normalized();

	Vector3f panelForward;
	float approach;
	if ( floatingPanel )
	{
		Matrix4f mat = panelMatrix;
		mat.SetTranslation( Vector3f( 0.0f ) );
		panelForward = mat.Transform( Vector3f( 0.0f, 0.0f, 1.0f ) ).Normalized();
		approach = viewForward.Dot( panelForward );
		if ( approach >= -0.1 )
		{	// looking away
			return Vector2f( -2.0f, -2.0f );
		}
	}
	else
	{
		panelForward = -MatrixForward( panelMatrix ).Normalized();
		approach = viewForward.Dot( panelForward );
		if ( approach <= 0.1 )
		{	// looking away
			return Vector2f( -2.0f, -2.0f );
		}
	}

	const Matrix4f panelInvert = panelMatrix.Inverted();
	const Matrix4f viewInvert = viewMatrix.Inverted();

	const Vector3f viewOrigin = viewInvert.Transform( Vector3f( 0.0f ) );
	const Vector3f panelOrigin = MatrixOrigin( panelMatrix );

	// Should we disallow using panels from behind?
	const float d = panelOrigin.Dot( panelForward );
	const float t = -( viewOrigin.Dot( panelForward ) + d ) / approach;

	const Vector3f impact = viewOrigin + viewForward * t;
	const Vector3f localCoordinate = panelInvert.Transform( impact );

	return Vector2f( localCoordinate.x, localCoordinate.y );
}

// -1 to 1 range on screenMatrix, returns -2,-2 if looking away from the screen
Vector2f MoviePlayerView::GazeCoordinatesOnScreen( const Matrix4f & viewMatrix, const Matrix4f screenMatrix ) const
{
	// project along -Z in the viewMatrix onto the Z = 0 plane of screenMatrix
	const Vector3f viewForward = MatrixForward( viewMatrix ).Normalized();

	Vector3f screenForward;
	if ( Cinema.SceneMgr.FreeScreenActive )
	{
		// FIXME: free screen matrix is inverted compared to bounds screen matrix.
		screenForward = -Vector3f( screenMatrix.M[0][2], screenMatrix.M[1][2], screenMatrix.M[2][2] ).Normalized();
	}
	else
	{
		screenForward = -MatrixForward( screenMatrix ).Normalized();
	}

	const float approach = viewForward.Dot( screenForward );
	if ( approach <= 0.1f )
	{
		// looking away
		return Vector2f( -2.0f, -2.0f );
	}

	const Matrix4f panelInvert = screenMatrix.Inverted();
	const Matrix4f viewInvert = viewMatrix.Inverted();

	const Vector3f viewOrigin = viewInvert.Transform( Vector3f( 0.0f ) );
	const Vector3f panelOrigin = MatrixOrigin( screenMatrix );

	// Should we disallow using panels from behind?
	const float d = panelOrigin.Dot( screenForward );
	const float t = -( viewOrigin.Dot( screenForward ) + d ) / approach;

	const Vector3f impact = viewOrigin + viewForward * t;
	const Vector3f localCoordinate = panelInvert.Transform( impact );

	return Vector2f( localCoordinate.x, localCoordinate.y );
}

IntersectRayBoundsResult IntersectRayBounds(const Matrix4f &centerViewMatrix,
                                            const Matrix4f &targetWorldMatrix,
                                            const GlGeometry &targetGeometry,
                                            bool axisInWorld) {

  Matrix4f worldToModelM = targetWorldMatrix.Inverted();
  Matrix4f invertedCenterViewM = centerViewMatrix.Inverted();
  Vector3f inWorldCenterViewPos = invertedCenterViewM.GetTranslation();
  Quatf centerViewRot = Quatf(invertedCenterViewM);

  const Vector3f rayStart = worldToModelM.Transform(inWorldCenterViewPos);
  const Vector3f rayDir = worldToModelM.Transform(centerViewRot.Rotate(
                              Vector3f(0.0f, 0.0f, -1.0f))) -
                          rayStart;
  const Vector3f boundingBoxMins = targetGeometry.localBounds.GetMins();
  const Vector3f boundingBoxMaxs = targetGeometry.localBounds.GetMaxs();
  float t0 = 0.0f;
  float t1 = 0.0f;

  bool intersected = Intersect_RayBounds(rayStart, rayDir, boundingBoxMins,
                                         boundingBoxMaxs, t0, t1);

  IntersectRayBoundsResult result;
  result.intersected = intersected && t0 > 0;

  if (intersected) {
    result.first = rayStart + t0 * rayDir;
    result.second = rayStart + t1 * rayDir;

    if (axisInWorld) {
      result.first = targetWorldMatrix.Transform(result.first);
      result.second = targetWorldMatrix.Transform(result.second);
    }
  }

  return result;
}

//==============================
// VRMenuMgrLocal::Finish
void VRMenuMgrLocal::Finish( Matrix4f const & viewMatrix )
{
	if ( NumSubmitted == 0 )
	{
		return;
	}

	Matrix4f invViewMatrix = viewMatrix.Inverted(); // if the view is never scaled or sheared we could use Transposed() here instead
	Vector3f viewPos = invViewMatrix.GetTranslation();

	// sort surfaces
	SortKeys.Resize( NumSubmitted );
	for ( int i = 0; i < NumSubmitted; ++i )
	{
		// the sort key is a combination of the distance squared, reinterpreted as an integer, and the submission index
		// this sorts on distance while still allowing submission order to contribute in the equal case.
		float distSq = ( Submitted[i].Pose.Position - viewPos ).LengthSq();
		int64_t sortKey = *reinterpret_cast< unsigned* >( &distSq );
		SortKeys[i].Key = ( sortKey << 32ULL ) | i;
	}
	
	Alg::QuickSort( SortKeys );
}

// Writes the current calibration for a particular device to a device profile file
// sensor - the sensor that was calibrated
// cal_name - an optional name for the calibration or default if cal_name == NULL
bool SensorFusion::SaveMagCalibration(const char* calibrationName) const
{
    if (CachedSensorInfo.SerialNumber[0] == 0 || !HasMagCalibration())
        return false;
    
    // A named calibration may be specified for calibration in different
    // environments, otherwise the default calibration is used
    if (calibrationName == NULL)
        calibrationName = "default";

    // Generate a mag calibration event
    JSON* calibration = JSON::CreateObject();
    // (hardcoded for now) the measurement and representation method 
    calibration->AddStringItem("Version", "2.0");   
    calibration->AddStringItem("Name", "default");

    // time stamp the calibration
    char time_str[64];
   
#if defined(OVR_OS_WIN32) and !defined(__MINGW32__)
    struct tm caltime;
    localtime_s(&caltime, &MagCalibrationTime);
    strftime(time_str, 64, "%Y-%m-%d %H:%M:%S", &caltime);
#else
    struct tm* caltime;
    caltime = localtime(&MagCalibrationTime);
    strftime(time_str, 64, "%Y-%m-%d %H:%M:%S", caltime);
#endif
   
    calibration->AddStringItem("Time", time_str);

    // write the full calibration matrix
    char matrix[256];
    Matrix4f calmat = GetMagCalibration();
    calmat.ToString(matrix, 256);
    calibration->AddStringItem("CalibrationMatrix", matrix);
    // save just the offset, for backwards compatibility
    // this can be removed when we don't want to support 0.2.4 anymore
    Vector3f center(calmat.M[0][3], calmat.M[1][3], calmat.M[2][3]);
    Matrix4f tmp = calmat; tmp.M[0][3] = tmp.M[1][3] = tmp.M[2][3] = 0; tmp.M[3][3] = 1;
    center = tmp.Inverted().Transform(center);
    Matrix4f oldcalmat; oldcalmat.M[0][3] = center.x; oldcalmat.M[1][3] = center.y; oldcalmat.M[2][3] = center.z; 
    oldcalmat.ToString(matrix, 256);
    calibration->AddStringItem("Calibration", matrix);
    

    String path = GetBaseOVRPath(true);
    path += "/Devices.json";

    // Look for a prexisting device file to edit
    Ptr<JSON> root = *JSON::Load(path);
    if (root)
    {   // Quick sanity check of the file type and format before we parse it
        JSON* version = root->GetFirstItem();
        if (version && version->Name == "Oculus Device Profile Version")
        {   
            int major = atoi(version->Value.ToCStr());
            if (major > MAX_DEVICE_PROFILE_MAJOR_VERSION)
            {
                // don't use the file on unsupported major version number
                root->Release();
                root = NULL;
            }
        }
        else
        {
            root->Release();
            root = NULL;
        }
    }

    JSON* device = NULL;
    if (root)
    {
        device = root->GetFirstItem();   // skip the header
        device = root->GetNextItem(device);
        while (device)
        {   // Search for a previous calibration with the same name for this device
            // and remove it before adding the new one
            if (device->Name == "Device")
            {   
                JSON* item = device->GetItemByName("Serial");
                if (item && item->Value == CachedSensorInfo.SerialNumber)
                {   // found an entry for this device
                    item = device->GetNextItem(item);
                    while (item)
                    {
                        if (item->Name == "MagCalibration")
                        {   
                            JSON* name = item->GetItemByName("Name");
                            if (name && name->Value == calibrationName)
                            {   // found a calibration of the same name
                                item->RemoveNode();
                                item->Release();
                                break;
                            } 
                        }
//.........这里部分代码省略.........

//==============================
// BitmapFontSurfaceLocal::Finish
// transform all vertex blocks into the vertices array so they're ready to be uploaded to the VBO
// We don't have to do this for each eye because the billboarded surfaces are sorted / aligned
// based on their distance from / direction to the camera view position and not the camera direction.
void BitmapFontSurfaceLocal::Finish( Matrix4f const & viewMatrix )
{
    DROID_ASSERT( this != NULL, "BitmapFont" );

	//SPAM( "BitmapFontSurfaceLocal::Finish" );

	Matrix4f invViewMatrix = viewMatrix.Inverted(); // if the view is never scaled or sheared we could use Transposed() here instead
	Vector3f viewPos = invViewMatrix.GetTranslation();

	// sort vertex blocks indices based on distance to pivot
	int const MAX_VERTEX_BLOCKS = 256;
	vbSort_t vbSort[MAX_VERTEX_BLOCKS];
	int const n = VertexBlocks.GetSizeI();
	for ( int i = 0; i < n; ++i )
	{
		vbSort[i].VertexBlockIndex = i;
		VertexBlockType & vb = VertexBlocks[i];
		vbSort[i].DistanceSquared = ( vb.Pivot - viewPos ).LengthSq();
	}

	qsort( vbSort, n, sizeof( vbSort[0] ), VertexBlockSortFn );

	// transform the vertex blocks into the vertices array
	CurIndex = 0;
	CurVertex = 0;

	// TODO:
	// To add multiple-font-per-surface support, we need to add a 3rd component to s and t, 
	// then get the font for each vertex block, and set the texture index on each vertex in 
	// the third texture coordinate.
	for ( int i = 0; i < VertexBlocks.GetSizeI(); ++i )
	{		
		VertexBlockType & vb = VertexBlocks[vbSort[i].VertexBlockIndex];
		Matrix4f transform;
		if ( vb.Billboard )
		{
			if ( vb.TrackRoll )
			{
				transform = invViewMatrix;
			}
			else
			{
                Vector3f textNormal = viewPos - vb.Pivot;
                textNormal.Normalize();
                transform = Matrix4f::CreateFromBasisVectors( textNormal, Vector3f( 0.0f, 1.0f, 0.0f ) );
			}
			transform.SetTranslation( vb.Pivot );
		}
		else
		{
			transform.SetIdentity();
			transform.SetTranslation( vb.Pivot );
		}

		for ( int j = 0; j < vb.NumVerts; j++ )
		{
			fontVertex_t const & v = vb.Verts[j];
			Vertices[CurVertex].xyz = transform.Transform( v.xyz );
			Vertices[CurVertex].s = v.s;
			Vertices[CurVertex].t = v.t;			
			*(UInt32*)(&Vertices[CurVertex].rgba[0]) = *(UInt32*)(&v.rgba[0]);
			*(UInt32*)(&Vertices[CurVertex].fontParms[0]) = *(UInt32*)(&v.fontParms[0]);
			CurVertex++;
		}
		CurIndex += ( vb.NumVerts / 2 ) * 3;
		// free this vertex block
		vb.Free();
	}
	// remove all elements from the vertex block (but don't free the memory since it's likely to be 
	// needed on the next frame.
	VertexBlocks.Clear();

	glBindVertexArrayOES_( Geo.vertexArrayObject );
	glBindBuffer( GL_ARRAY_BUFFER, Geo.vertexBuffer );
	glBufferSubData( GL_ARRAY_BUFFER, 0, CurVertex * sizeof( fontVertex_t ), (void *)Vertices );
	glBindVertexArrayOES_( 0 );
	
    Geo.indexCount = CurIndex;
}

//==============================
// OvrGazeCursorLocal::Frame
void OvrGazeCursorLocal::Frame( Matrix4f const & viewMatrix, float const deltaTime )
{
	//LOG( "OvrGazeCursorLocal::Frame" );
	HiddenFrames -= 1;

	if ( 0 ) //IsActive ) 
	{
		CursorRotation += deltaTime * RotationRateRadians;
		if ( CursorRotation > Mathf::TwoPi )
		{
			CursorRotation -= Mathf::TwoPi;
		}
		else if ( CursorRotation < 0.0f )
		{
			CursorRotation += Mathf::TwoPi;
		}
	}
	else
	{
		CursorRotation = 0.0f;
	}

#if 1
	if ( TimerEndTime > 0.0 )
	{
		double TimeRemaining = TimerEndTime - ovr_GetTimeInSeconds();
		if ( TimeRemaining <= 0.0 )
		{
			TimerEndTime = -1.0;
			TimerShowTime = -1.0;
			ColorTableOffset = Vector2f( 0.0f );
		}
		else
		{
			double duration = TimerEndTime - TimerShowTime;
			double ratio = 1.0f - ( TimeRemaining / duration );
			//SPAM( "TimerEnd = %.2f, TimeRemaining = %.2f, Ratio = %.2f", TimerEndTime, TimeRemaining, ratio );
			ColorTableOffset.x = 0.0f;
			ColorTableOffset.y = float( ratio );
		}
	}
	else
	{
		ColorTableOffset = Vector2f( 0.0f );
	}
#else
	// cycling
	float COLOR_TABLE_CYCLE_RATE = 0.25f;
	ColorTableOffset.x = 0.0f;
	ColorTableOffset.y += COLOR_TABLE_CYCLE_RATE * deltaTime;
	if ( ColorTableOffset.y > 1.0f )
	{
		ColorTableOffset.y -= floorf( ColorTableOffset.y );
	}
	else if ( ColorTableOffset.y < 0.0f )
	{
		ColorTableOffset.y += ceilf( ColorTableOffset.y );
	}
#endif

	const Vector3f viewPos( GetViewMatrixPosition( viewMatrix ) );
	const Vector3f viewFwd( GetViewMatrixForward( viewMatrix ) );

//	Vector3f position = viewPos + viewFwd * ( Info.Distance - DistanceOffset );
	Vector3f position = viewPos + viewFwd * 1.4f; // [email protected]# JDC fixed distance
	CursorScale = 0.0125f;

	Matrix4f viewRot = viewMatrix;
	viewRot.SetTranslation( Vector3f( 0.0f ) );

	// Add one ghost for every four milliseconds.
	// Assume we are going to be at even multiples of vsync, so we don't need to bother
	// keeping an accurate roundoff count.
	const int lerps = deltaTime / 0.004;

	const Matrix4f & prev = CursorTransform[ CurrentTransform % TRAIL_GHOSTS ];
	Matrix4f & now = CursorTransform[ ( CurrentTransform + lerps ) % TRAIL_GHOSTS ];

	now = Matrix4f::Translation( position ) * viewRot.Inverted() * Matrix4f::RotationZ( CursorRotation )
		* Matrix4f::Scaling( CursorScale, CursorScale, 1.0f );

	if ( CurrentTransform > 0 )
	{
		for ( int i = 1 ; i <= lerps ; i++ )
		{
			const float f = (float)i / lerps;
			Matrix4f & tween = CursorTransform[ ( CurrentTransform + i) % TRAIL_GHOSTS ];

			// We only need to build a scatter on the final point that is already set by now
			if ( i != lerps )
			{
				tween = ( ( now * f ) + ( prev * ( 1.0f - f ) ) );
			}

			// When the cursor depth fails, draw a scattered set of ghosts
			Matrix4f & scatter = CursorScatterTransform[ ( CurrentTransform + i) % TRAIL_GHOSTS ];

			// random point in circle
//.........这里部分代码省略.........

//==============================
// OvrGazeCursorLocal::Frame
void OvrGazeCursorLocal::Frame( Matrix4f const & viewMatrix, float const deltaTime )
{
	//LOG( "OvrGazeCursorLocal::Frame" );
	HiddenFrames -= 1;

	if ( RotationRateRadians != 0.0f )	// comparison to exactly 0 is intentional
	{
		CursorRotation += deltaTime * RotationRateRadians;
		if ( CursorRotation > Mathf::TwoPi )
		{
			CursorRotation -= Mathf::TwoPi;
		}
		else if ( CursorRotation < 0.0f )
		{
			CursorRotation += Mathf::TwoPi;
		}
	}
#if 1
	if ( TimerEndTime > 0.0 )
	{
		double TimeRemaining = TimerEndTime - vrapi_GetTimeInSeconds();
		if ( TimeRemaining <= 0.0 )
		{
			TimerEndTime = -1.0;
			TimerShowTime = -1.0;
			ColorTableOffset = Vector2f( 0.0f );
		}
		else
		{
			double duration = TimerEndTime - TimerShowTime;
			double ratio = 1.0f - ( TimeRemaining / duration );
			//SPAM( "TimerEnd = %.2f, TimeRemaining = %.2f, Ratio = %.2f", TimerEndTime, TimeRemaining, ratio );
			ColorTableOffset.x = 0.0f;
			ColorTableOffset.y = float( ratio );
		}
	}
	else
	{
		ColorTableOffset = Vector2f( 0.0f );
	}
#else
	// cycling
	float COLOR_TABLE_CYCLE_RATE = 0.25f;
	ColorTableOffset.x = 0.0f;
	ColorTableOffset.y += COLOR_TABLE_CYCLE_RATE * deltaTime;
	if ( ColorTableOffset.y > 1.0f )
	{
		ColorTableOffset.y -= floorf( ColorTableOffset.y );
	}
	else if ( ColorTableOffset.y < 0.0f )
	{
		ColorTableOffset.y += ceilf( ColorTableOffset.y );
	}
#endif

	const Vector3f viewPos( GetViewMatrixPosition( viewMatrix ) );
	const Vector3f viewFwd( GetViewMatrixForward( viewMatrix ) );

	Vector3f position = viewPos + viewFwd * ( Info.Distance - DistanceOffset );

	Matrix4f viewRot = viewMatrix;
	viewRot.SetTranslation( Vector3f( 0.0f ) );

	// Add one ghost for every four milliseconds.
	// Assume we are going to be at even multiples of vsync, so we don't need to bother
	// keeping an accurate roundoff count.
	const int lerps = static_cast<int>( deltaTime / 0.004f );

	const Matrix4f & prev = CursorTransform[ CurrentTransform % TRAIL_GHOSTS ];
	Matrix4f & now = CursorTransform[ ( CurrentTransform + lerps ) % TRAIL_GHOSTS ];

	now = Matrix4f::Translation( position ) * viewRot.Inverted() * Matrix4f::RotationZ( CursorRotation )
		* Matrix4f::Scaling( CursorScale, CursorScale, 1.0f );

	if ( CurrentTransform > 0 )
	{
		for ( int i = 1 ; i <= lerps ; i++ )
		{
			const float f = (float)i / lerps;
			Matrix4f & tween = CursorTransform[ ( CurrentTransform + i) % TRAIL_GHOSTS ];

			// We only need to build a scatter on the final point that is already set by now
			if ( i != lerps )
			{
				tween = ( ( now * f ) + ( prev * ( 1.0f - f ) ) );
			}

			// When the cursor depth fails, draw a scattered set of ghosts
			Matrix4f & scatter = CursorScatterTransform[ ( CurrentTransform + i) % TRAIL_GHOSTS ];

			// random point in circle
			float	rx, ry;
			while( 1 )
			{
				rx = frand();
				ry = frand();
				if ( (rx*rx + ry*ry < 1.0f ))
				{
//.........这里部分代码省略.........