svn merge ^/trunk/blender -r47023:HEAD
[blender-staging.git] / intern / cycles / util / util_transform.cpp
1 /*
2  * Copyright 2011, Blender Foundation.
3  *
4  * This program is free software; you can redistribute it and/or
5  * modify it under the terms of the GNU General Public License
6  * as published by the Free Software Foundation; either version 2
7  * of the License, or (at your option) any later version.
8  *
9  * This program is distributed in the hope that it will be useful,
10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
12  * GNU General Public License for more details.
13  *
14  * You should have received a copy of the GNU General Public License
15  * along with this program; if not, write to the Free Software Foundation,
16  * Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
17  */
18
19 /*
20  * Adapted from code with license:
21  * 
22  * Copyright (c) 2002, Industrial Light & Magic, a division of Lucas
23  * Digital Ltd. LLC. All rights reserved.
24  * 
25  * Redistribution and use in source and binary forms, with or without
26  * modification, are permitted provided that the following conditions are
27  * met:
28  * * Redistributions of source code must retain the above copyright
29  *   notice, this list of conditions and the following disclaimer.
30  * * Redistributions in binary form must reproduce the above copyright
31  *   notice, this list of conditions and the following disclaimer in
32  *   the documentation and/or other materials provided with the
33  *   distribution.
34  * * Neither the name of Industrial Light & Magic nor the names of its
35  *   contributors may be used to endorse or promote products derived
36  *   from this software without specific prior written permission. 
37  * 
38  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
39  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
40  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
41  * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
42  * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
43  * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
44  * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
45  * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
46  * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
47  * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
48  * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
49  */
50
51 #include "util_math.h"
52 #include "util_transform.h"
53
54 CCL_NAMESPACE_BEGIN
55
56 /* Transform Inverse */
57
58 static bool transform_matrix4_gj_inverse(float R[][4], float M[][4])
59 {
60         /* forward elimination */
61         for(int i = 0; i < 4; i++) {
62                 int pivot = i;
63                 float pivotsize = M[i][i];
64
65                 if(pivotsize < 0)
66                         pivotsize = -pivotsize;
67
68                 for(int j = i + 1; j < 4; j++) {
69                         float tmp = M[j][i];
70
71                         if(tmp < 0)
72                                 tmp = -tmp;
73
74                         if(tmp > pivotsize) {
75                                 pivot = j;
76                                 pivotsize = tmp;
77                         }
78                 }
79
80                 if(pivotsize == 0)
81                         return false;
82
83                 if(pivot != i) {
84                         for(int j = 0; j < 4; j++) {
85                                 float tmp;
86
87                                 tmp = M[i][j];
88                                 M[i][j] = M[pivot][j];
89                                 M[pivot][j] = tmp;
90
91                                 tmp = R[i][j];
92                                 R[i][j] = R[pivot][j];
93                                 R[pivot][j] = tmp;
94                         }
95                 }
96
97                 for(int j = i + 1; j < 4; j++) {
98                         float f = M[j][i] / M[i][i];
99
100                         for(int k = 0; k < 4; k++) {
101                                 M[j][k] -= f*M[i][k];
102                                 R[j][k] -= f*R[i][k];
103                         }
104                 }
105         }
106
107         /* backward substitution */
108         for(int i = 3; i >= 0; --i) {
109                 float f;
110
111                 if((f = M[i][i]) == 0)
112                         return false;
113
114                 for(int j = 0; j < 4; j++) {
115                         M[i][j] /= f;
116                         R[i][j] /= f;
117                 }
118
119                 for(int j = 0; j < i; j++) {
120                         f = M[j][i];
121
122                         for(int k = 0; k < 4; k++) {
123                                 M[j][k] -= f*M[i][k];
124                                 R[j][k] -= f*R[i][k];
125                         }
126                 }
127         }
128
129         return true;
130 }
131
132 Transform transform_inverse(const Transform& tfm)
133 {
134         Transform tfmR = transform_identity();
135         float M[4][4], R[4][4];
136
137         memcpy(R, &tfmR, sizeof(R));
138         memcpy(M, &tfm, sizeof(M));
139
140         if(!transform_matrix4_gj_inverse(R, M)) {
141                 /* matrix is degenerate (e.g. 0 scale on some axis), ideally we should
142                  * never be in this situation, but try to invert it anyway with tweak */
143                 M[0][0] += 1e-8f;
144                 M[1][1] += 1e-8f;
145                 M[2][2] += 1e-8f;
146
147                 if(!transform_matrix4_gj_inverse(R, M))
148                         return transform_identity();
149         }
150
151         memcpy(&tfmR, R, sizeof(R));
152
153         return tfmR;
154 }
155
156 /* Motion Transform */
157
158 static float4 transform_to_quat(const Transform& tfm)
159 {
160         double trace = tfm[0][0] + tfm[1][1] + tfm[2][2];
161         float4 qt;
162
163         if(trace > 0.0) {
164                 double s = sqrt(trace + 1.0);
165
166                 qt.w = (float)(s/2.0);
167                 s = 0.5/s;
168
169                 qt.x = (float)((double)(tfm[2][1] - tfm[1][2]) * s);
170                 qt.y = (float)((double)(tfm[0][2] - tfm[2][0]) * s);
171                 qt.z = (float)((double)(tfm[1][0] - tfm[0][1]) * s);
172         }
173         else {
174                 int i = 0;
175
176                 if(tfm[1][1] > tfm[i][i])
177                         i = 1;
178                 if(tfm[2][2] > tfm[i][i])
179                         i = 2;
180
181                 int j = (i + 1)%3;
182                 int k = (j + 1)%3;
183
184                 double s = sqrt((double)(tfm[i][i] - (tfm[j][j] + tfm[k][k])) + 1.0);
185
186                 double q[3];
187                 q[i] = s * 0.5;
188                 if(s != 0.0)
189                         s = 0.5/s;
190
191                 double w = (double)(tfm[k][j] - tfm[j][k]) * s;
192                 q[j] = (double)(tfm[j][i] + tfm[i][j]) * s;
193                 q[k] = (double)(tfm[k][i] + tfm[i][k]) * s;
194
195                 qt.x = (float)q[0];
196                 qt.y = (float)q[1];
197                 qt.z = (float)q[2];
198                 qt.w = (float)w;
199         }
200
201         return qt;
202 }
203
204 static void transform_decompose(Transform *decomp, const Transform *tfm)
205 {
206         /* extract translation */
207         decomp->y = make_float4(tfm->x.w, tfm->y.w, tfm->z.w, 0.0f);
208
209         /* extract rotation */
210         Transform M = *tfm;
211         M.x.w = 0.0f; M.y.w = 0.0f; M.z.w = 0.0f; M.w.w = 1.0f;
212
213         Transform R = M;
214         float norm;
215         int iteration = 0;
216
217         do {
218                 Transform Rnext;
219                 Transform Rit = transform_inverse(transform_transpose(R));
220
221                 for(int i = 0; i < 4; i++)
222                         for(int j = 0; j < 4; j++)
223                                 Rnext[i][j] = 0.5f * (R[i][j] + Rit[i][j]);
224                 
225                 norm = 0.0f;
226                 for(int i = 0; i < 3; i++) {
227                         norm = max(norm,
228                                 fabsf(R[i][0] - Rnext[i][0]) +
229                                 fabsf(R[i][1] - Rnext[i][1]) +
230                                 fabsf(R[i][2] - Rnext[i][2]));
231                 }
232
233                 R = Rnext;
234                 iteration++;
235         } while(iteration < 100 && norm > 1e-4f);
236
237         if(transform_negative_scale(R))
238                 R = R * transform_scale(-1.0f, -1.0f, -1.0f); /* todo: test scale */
239
240         decomp->x = transform_to_quat(R);
241
242         /* extract scale and pack it */
243         Transform scale = transform_inverse(R) * M;
244         decomp->y.w = scale.x.x;
245         decomp->z = make_float4(scale.x.y, scale.x.z, scale.y.x, scale.y.y);
246         decomp->w = make_float4(scale.y.z, scale.z.x, scale.z.y, scale.z.z);
247 }
248
249 void transform_motion_decompose(MotionTransform *decomp, const MotionTransform *motion)
250 {
251         transform_decompose(&decomp->pre, &motion->pre);
252         transform_decompose(&decomp->post, &motion->post);
253 }
254
255 CCL_NAMESPACE_END
256