public static Matrix3f RotateBetween( Vector3f from, Vector3f to ) { from.Normalize(); to.Normalize(); var crossProduct = Vector3f.Cross( from, to ); var normalizedAxis = crossProduct.Normalized(); float sinTheta = crossProduct.Norm(); float cosTheta = Vector3f.Dot( from, to ); float x = normalizedAxis.x; float y = normalizedAxis.y; float z = normalizedAxis.z; var m = new Matrix3f(); m[ 0, 0 ] = x * x * ( 1.0f - cosTheta ) + cosTheta; m[ 0, 1 ] = y * x * ( 1.0f - cosTheta ) - z * sinTheta; m[ 0, 2 ] = z * x * ( 1.0f - cosTheta ) + y * sinTheta; m[ 1, 0 ] = x * y * ( 1.0f - cosTheta ) + z * sinTheta; m[ 1, 1 ] = y * y * ( 1.0f - cosTheta ) + cosTheta; m[ 1, 2 ] = z * y * ( 1.0f - cosTheta ) - x * sinTheta; m[ 2, 0 ] = x * z * ( 1.0f - cosTheta ) - y * sinTheta; m[ 2, 1 ] = y * z * ( 1.0f - cosTheta ) + x * sinTheta; m[ 2, 2 ] = z * z * ( 1.0f - cosTheta ) + cosTheta; return m; }
public static Matrix3f RotateAxis( Vector3f axis, float radians ) { var normalizedAxis = axis.Normalized(); float cosTheta = ( float )( Math.Cos( radians ) ); float sinTheta = ( float )( Math.Sin( radians ) ); float x = normalizedAxis.x; float y = normalizedAxis.y; float z = normalizedAxis.z; var m = new Matrix3f(); m[ 0, 0 ] = x * x * ( 1.0f - cosTheta ) + cosTheta; m[ 0, 1 ] = y * x * ( 1.0f - cosTheta ) - z * sinTheta; m[ 0, 2 ] = z * x * ( 1.0f - cosTheta ) + y * sinTheta; m[ 1, 0 ] = x * y * ( 1.0f - cosTheta ) + z * sinTheta; m[ 1, 1 ] = y * y * ( 1.0f - cosTheta ) + cosTheta; m[ 1, 2 ] = z * y * ( 1.0f - cosTheta ) - x * sinTheta; m[ 2, 0 ] = x * z * ( 1.0f - cosTheta ) - y * sinTheta; m[ 2, 1 ] = y * z * ( 1.0f - cosTheta ) + x * sinTheta; m[ 2, 2 ] = z * z * ( 1.0f - cosTheta ) + cosTheta; return m; }
public static Matrix3f operator * ( Matrix3f lhs, Matrix3f rhs ) { Matrix3f m = new Matrix3f(); for( int i = 0; i < 3; ++i ) { for( int j = 0; j < 3; ++j ) { for( int k = 0; k < 3; ++k ) { m[ i, k ] += lhs[ i, j ] * rhs[ j, k ]; } } } return m; }
public static Matrix3f operator * ( float a, Matrix3f rhs ) { // TODO: make more efficient Matrix3f m = new Matrix3f(); for( int i = 0; i < 3; ++i ) { for( int j = 0; j < 3; ++j ) { m[ i, j ] = a * rhs[ i, j ]; } } return m; }
public static Matrix3f operator - ( Matrix3f lhs, Matrix3f rhs ) { Matrix3f m = new Matrix3f(); for( int i = 0; i < 3; ++i ) { for( int j = 0; j < 3; ++j ) { m[ i, j ] = lhs[ i, j ] - rhs[ i, j ]; } } return m; }
public Matrix3f Inverse() { float cofactor00 = MatrixUtil.Determinant2x2f( m11, m12, m21, m22 ); float cofactor01 = -MatrixUtil.Determinant2x2f( m10, m12, m20, m22 ); float cofactor02 = MatrixUtil.Determinant2x2f( m10, m11, m20, m21 ); float cofactor10 = -MatrixUtil.Determinant2x2f( m01, m02, m21, m22 ); float cofactor11 = MatrixUtil.Determinant2x2f( m00, m02, m20, m22 ); float cofactor12 = -MatrixUtil.Determinant2x2f( m00, m01, m20, m21 ); float cofactor20 = MatrixUtil.Determinant2x2f( m01, m02, m11, m12 ); float cofactor21 = -MatrixUtil.Determinant2x2f( m00, m02, m10, m12 ); float cofactor22 = MatrixUtil.Determinant2x2f( m00, m01, m10, m11 ); float determinant = m00 * cofactor00 + m01 * cofactor01 + m02 * cofactor02; if( determinant == 0.0f ) // exactly { throw new ArgumentException( "Matrix is singular." ); } else { float reciprocalDeterminant = 1.0f / determinant; Matrix3f inverse = new Matrix3f ( cofactor00 * reciprocalDeterminant, cofactor10 * reciprocalDeterminant, cofactor20 * reciprocalDeterminant, cofactor01 * reciprocalDeterminant, cofactor11 * reciprocalDeterminant, cofactor21 * reciprocalDeterminant, cofactor02 * reciprocalDeterminant, cofactor12 * reciprocalDeterminant, cofactor22 * reciprocalDeterminant ); return inverse; } }
public Matrix3f( Matrix3f m ) { this.m00 = m.m00; this.m10 = m.m10; this.m20 = m.m20; this.m01 = m.m01; this.m11 = m.m11; this.m21 = m.m21; this.m02 = m.m02; this.m12 = m.m12; this.m22 = m.m22; }
// TODO: /// <summary> /// Finds the theta and phi to best fit the camera /// </summary> /// <param name="camera"></param> public void FitThetaPhi( OldCamera camera ) { var cameraForward = camera.ViewDirection; var cameraUp = camera.UpVector; // compute theta: // find theta such that the up vector // when rotated about groundPlaneUp lies in the // (ground plane forward)-(ground plane up) plane // which is just the angle theta it makes projected onto the ground plane // TODO: define a ground plane point as well? make it an affine basis... // first: take the incoming up vector in world coordinates // and transform it into the ground plane basis // var gpToWorld = new Matrix3f( GroundPlaneRight, GroundPlaneUp, -GroundPlaneForward ); var gpToWorld = new Matrix3f( -GroundPlaneForward, GroundPlaneRight, GroundPlaneUp ); var worldToGP = gpToWorld.Inverse(); // camera up vector in local coordinates var cameraUpGP = worldToGP * cameraUp; // compute theta var cameraUpGPSpherical = GeometryUtils.RectangularToSpherical( cameraUpGP ); Theta = cameraUpGPSpherical.y; // Phi = MathUtils.PI - cameraUpGPSpherical.z; Phi = 0; }