Esempio n. 1
0
        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;
        }
Esempio n. 2
0
        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;
        }
Esempio n. 3
0
        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;
        }
Esempio n. 4
0
        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;
        }
Esempio n. 5
0
        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;
        }
Esempio n. 6
0
        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;
            }
        }
Esempio n. 7
0
        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;
        }
Esempio n. 8
0
        // 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;
        }