← Back to branch summary

~ubuntu-branches/ubuntu/wily/opencollada/wily-proposed

« back to all changes in this revision

Viewing changes to COLLADABaseUtils/src/Math/COLLADABUMathMatrix4.cpp

  • Committer: Package Import Robot
  • Author(s): Matteo F. Vescovi
  • Date: 2015-05-14 17:23:27 UTC
  • Revision ID: package-import@ubuntu.com-20150514172327-f862u8envms01fra
Tags: upstream-0.1.0~20140703.ddf8f47+dfsg1
Import upstream version 0.1.0~20140703.ddf8f47+dfsg1

Show diffs side-by-side

added added

removed removed

Lines of Context:
COLLADABaseUtils/src/Math/COLLADABUMathMatrix4.cpp
 
1
/*
 
2
    Copyright (c) 2008-2009 NetAllied Systems GmbH
 
3
 
 
4
    This file is part of COLLADABaseUtils.
 
5
 
 
6
    Licensed under the MIT Open Source License, 
 
7
    for details please see LICENSE file or the website
 
8
    http://www.opensource.org/licenses/mit-license.php
 
9
*/
 
10
 
 
11
#include "COLLADABUStableHeaders.h"
 
12
#include "Math/COLLADABUMathMatrix4.h"
 
13
 
 
14
namespace COLLADABU
 
15
{
 
16
 
 
17
    namespace Math
 
18
    {
 
19
 
 
20
        const Matrix4 Matrix4::ZERO(
 
21
            ISNOTIDENTITY,
 
22
            0, 0, 0, 0,
 
23
            0, 0, 0, 0,
 
24
            0, 0, 0, 0,
 
25
            0, 0, 0, 0 );
 
26
 
 
27
        const Matrix4 Matrix4::IDENTITY(
 
28
            ISIDENTITY,
 
29
            1, 0, 0, 0,
 
30
            0, 1, 0, 0,
 
31
            0, 0, 1, 0,
 
32
            0, 0, 0, 1 );
 
33
 
 
34
        const Matrix4 Matrix4::CLIPSPACE2DTOIMAGESPACE(
 
35
            ISNOTIDENTITY,
 
36
            0.5, 0, 0, 0.5,
 
37
            0, -0.5, 0, 0.5,
 
38
            0, 0, 1, 0,
 
39
            0, 0, 0, 1 );
 
40
 
 
41
        inline Real
 
42
        MINOR( const Matrix4& m, const size_t r0, const size_t r1, const size_t r2,
 
43
               const size_t c0, const size_t c1, const size_t c2 )
 
44
        {
 
45
            return m[ r0 ][ c0 ] * ( m[ r1 ][ c1 ] * m[ r2 ][ c2 ] - m[ r2 ][ c1 ] * m[ r1 ][ c2 ] ) -
 
46
                   m[ r0 ][ c1 ] * ( m[ r1 ][ c0 ] * m[ r2 ][ c2 ] - m[ r2 ][ c0 ] * m[ r1 ][ c2 ] ) +
 
47
                   m[ r0 ][ c2 ] * ( m[ r1 ][ c0 ] * m[ r2 ][ c1 ] - m[ r2 ][ c0 ] * m[ r1 ][ c1 ] );
 
48
        }
 
49
 
 
50
 
 
51
        Matrix4 Matrix4::adjoint() const
 
52
        {
 
53
            return Matrix4( MINOR( *this, 1, 2, 3, 1, 2, 3 ),
 
54
                            -MINOR( *this, 0, 2, 3, 1, 2, 3 ),
 
55
                            MINOR( *this, 0, 1, 3, 1, 2, 3 ),
 
56
                            -MINOR( *this, 0, 1, 2, 1, 2, 3 ),
 
57
 
 
58
                            -MINOR( *this, 1, 2, 3, 0, 2, 3 ),
 
59
                            MINOR( *this, 0, 2, 3, 0, 2, 3 ),
 
60
                            -MINOR( *this, 0, 1, 3, 0, 2, 3 ),
 
61
                            MINOR( *this, 0, 1, 2, 0, 2, 3 ),
 
62
 
 
63
                            MINOR( *this, 1, 2, 3, 0, 1, 3 ),
 
64
                            -MINOR( *this, 0, 2, 3, 0, 1, 3 ),
 
65
                            MINOR( *this, 0, 1, 3, 0, 1, 3 ),
 
66
                            -MINOR( *this, 0, 1, 2, 0, 1, 3 ),
 
67
 
 
68
                            -MINOR( *this, 1, 2, 3, 0, 1, 2 ),
 
69
                            MINOR( *this, 0, 2, 3, 0, 1, 2 ),
 
70
                            -MINOR( *this, 0, 1, 3, 0, 1, 2 ),
 
71
                            MINOR( *this, 0, 1, 2, 0, 1, 2 ) );
 
72
        }
 
73
 
 
74
 
 
75
        Real Matrix4::determinant() const
 
76
        {
 
77
            if ( mState == ISIDENTITY )
 
78
                return 1;
 
79
 
 
80
            return m[ 0 ][ 0 ] * MINOR( *this, 1, 2, 3, 1, 2, 3 ) -
 
81
                   m[ 0 ][ 1 ] * MINOR( *this, 1, 2, 3, 0, 2, 3 ) +
 
82
                   m[ 0 ][ 2 ] * MINOR( *this, 1, 2, 3, 0, 1, 3 ) -
 
83
                   m[ 0 ][ 3 ] * MINOR( *this, 1, 2, 3, 0, 1, 2 );
 
84
        }
 
85
 
 
86
        Matrix4 Matrix4::inverse() const
 
87
        {
 
88
            if ( mState == ISIDENTITY )
 
89
                return * this;
 
90
 
 
91
            return adjoint() * ( 1.0f / determinant() );
 
92
        }
 
93
 
 
94
        void Matrix4::setRotate( Vector3 point, Real theta, Real phi, Real alpha )
 
95
        //void setRotate(Point3d A, // rotate about a line centred on A
 
96
        {
 
97
            mState = UNKNOWN;
 
98
            Real cosAlpha, sinAlpha, cosPhi, sinPhi,
 
99
            cosTheta, sinTheta, cosPhi2, sinPhi2,
 
100
            cosTheta2, sinTheta2, c, a1, a2, a3;
 
101
            a1 = point.x;
 
102
            a2 = point.y;
 
103
            a3 = point.z;
 
104
 
 
105
            cosPhi = cos( phi );
 
106
            sinPhi = sin( phi );
 
107
            cosPhi2 = cosPhi * cosPhi;
 
108
            sinPhi2 = sinPhi * sinPhi;
 
109
            cosTheta = cos( theta );
 
110
            sinTheta = sin( theta );
 
111
            cosTheta2 = cosTheta * cosTheta;
 
112
            sinTheta2 = sinTheta * sinTheta;
 
113
            cosAlpha = cos( alpha );
 
114
            sinAlpha = sin( alpha );
 
115
            c = 1.0 - cosAlpha;
 
116
            m[ 0 ][ 0 ] = cosTheta2 * ( cosAlpha * cosPhi2 + sinPhi2 )
 
117
                          + cosAlpha * sinTheta2;
 
118
            m[ 1 ][ 0 ] = sinAlpha * cosPhi + c * sinPhi2 * cosTheta * sinTheta;
 
119
            m[ 2 ][ 0 ] = sinPhi * ( cosPhi * cosTheta * c - sinAlpha * sinTheta );
 
120
            m[ 0 ][ 1 ] = sinPhi2 * cosTheta * sinTheta * c - sinAlpha * cosPhi;
 
121
            m[ 1 ][ 1 ] = sinTheta2 * ( cosAlpha * cosPhi2 + sinPhi2 )
 
122
                          + cosAlpha * cosTheta2;
 
123
            m[ 2 ][ 1 ] = sinPhi * ( cosPhi * sinTheta * c + sinAlpha * cosTheta );
 
124
            m[ 0 ][ 2 ] = sinPhi * ( cosPhi * cosTheta * c + sinAlpha * sinTheta );
 
125
            m[ 1 ][ 2 ] = sinPhi * ( cosPhi * sinTheta * c - sinAlpha * cosTheta );
 
126
            m[ 2 ][ 2 ] = cosAlpha * sinPhi2 + cosPhi2;
 
127
            m[ 0 ][ 3 ] = a1 - a1 * m[ 0 ][ 0 ] - a2 * m[ 0 ][ 1 ] - a3 * m[ 0 ][ 2 ];
 
128
            m[ 1 ][ 3 ] = a2 - a1 * m[ 1 ][ 0 ] - a2 * m[ 1 ][ 1 ] - a3 * m[ 1 ][ 2 ];
 
129
            m[ 2 ][ 3 ] = a3 - a1 * m[ 2 ][ 0 ] - a2 * m[ 2 ][ 1 ] - a3 * m[ 2 ][ 2 ];
 
130
        }
 
131
 
 
132
 
 
133
        //-----------------------------------------------------------------------
 
134
        void Matrix4::makeTransform( const Vector3& position, const Vector3& scale, const Quaternion& orientation )
 
135
        {
 
136
            // Ordering:
 
137
            //    1. Scale
 
138
            //    2. Rotate
 
139
            //    3. Translate
 
140
            mState = UNKNOWN;
 
141
            Matrix3 rot3x3, scale3x3;
 
142
            orientation.toRotationMatrix( rot3x3 );
 
143
            scale3x3 = Matrix3::ZERO;
 
144
            scale3x3[ 0 ][ 0 ] = scale.x;
 
145
            scale3x3[ 1 ][ 1 ] = scale.y;
 
146
            scale3x3[ 2 ][ 2 ] = scale.z;
 
147
 
 
148
            // Set up final matrix with scale, rotation and translation
 
149
            *this = rot3x3 * scale3x3;
 
150
            this->setTrans( position );
 
151
 
 
152
            // No projection term
 
153
            m[ 3 ][ 0 ] = 0;
 
154
            m[ 3 ][ 1 ] = 0;
 
155
            m[ 3 ][ 2 ] = 0;
 
156
            m[ 3 ][ 3 ] = 1;
 
157
        }
 
158
 
 
159
    }
 
160
}