← Back to branch summary

~ubuntu-branches/ubuntu/vivid/emscripten/vivid

« back to all changes in this revision

Viewing changes to tests/box2d/glui/quaternion.cpp

  • Committer: Package Import Robot
  • Author(s): Sylvestre Ledru
  • Date: 2013-05-02 13:11:51 UTC
  • Revision ID: package-import@ubuntu.com-20130502131151-q8dvteqr1ef2x7xz
Tags: upstream-1.4.1~20130504~adb56cb
Import upstream version 1.4.1~20130504~adb56cb

Show diffs side-by-side

added added

removed removed

Lines of Context:
tests/box2d/glui/quaternion.cpp
 
1
/***********************************************************************
 
2
 
 
3
  quaternion.cpp - A quaternion class
 
4
 
 
5
  -------------------------------------------------------------------
 
6
 
 
7
  GLUI User Interface Toolkit (LGPL)
 
8
  Copyright (c) 1998 Paul Rademacher
 
9
 
 
10
  WWW:    http://sourceforge.net/projects/glui/
 
11
  Forums: http://sourceforge.net/forum/?group_id=92496
 
12
 
 
13
  This library is free software; you can redistribute it and/or
 
14
  modify it under the terms of the GNU Lesser General Public
 
15
  License as published by the Free Software Foundation; either
 
16
  version 2.1 of the License, or (at your option) any later version.
 
17
 
 
18
  This library is distributed in the hope that it will be useful,
 
19
  but WITHOUT ANY WARRANTY; without even the implied warranty of
 
20
  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 
21
  Lesser General Public License for more details.
 
22
 
 
23
  You should have received a copy of the GNU Lesser General Public
 
24
  License along with this library; if not, write to the Free Software
 
25
  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 
26
 
 
27
************************************************************************
 
28
 
 
29
  Feb 1998, Paul Rademacher (rademach@cs.unc.edu)
 
30
  Oct 2003, Nigel Stewart - GLUI Code Cleaning
 
31
  
 
32
************************************************************************/
 
33
 
 
34
#include "quaternion.h"
 
35
#include <cmath>
 
36
#include "glui_internal.h"
 
37
 
 
38
/******************************************* constructors **************/
 
39
 
 
40
quat::quat()
 
41
{
 
42
    *this = quat_identity();
 
43
}
 
44
 
 
45
quat::quat(const float x, const float y, const float z, const float w)
 
46
{
 
47
    v.set( x, y, z );
 
48
    s = w;
 
49
}
 
50
 
 
51
quat::quat(const vec3 &_v, const float _s)
 
52
{
 
53
    set( _v, _s );
 
54
}
 
55
 
 
56
quat::quat(const float _s, const vec3 &_v)
 
57
{
 
58
    set( _v, _s );
 
59
}
 
60
 
 
61
quat::quat(const float *d)
 
62
{
 
63
    v[0] = d[0];
 
64
    v[1] = d[1];
 
65
    v[2] = d[2];
 
66
    s    = d[3];
 
67
}
 
68
 
 
69
quat::quat(const double *d)
 
70
{
 
71
    v[0] = (float) d[0];
 
72
    v[1] = (float) d[1];
 
73
    v[2] = (float) d[2];
 
74
    s    = (float) d[3];
 
75
}
 
76
 
 
77
quat::quat(const quat &q)
 
78
{
 
79
    v = q.v;
 
80
    s = q.s;
 
81
}
 
82
 
 
83
void quat::set(const vec3 &_v, const float _s)
 
84
{
 
85
    v = _v;
 
86
    s = _s;
 
87
}
 
88
 
 
89
quat &quat::operator=(const quat &q)
 
90
{ 
 
91
    v = q.v;  
 
92
    s = q.s; 
 
93
    return *this; 
 
94
}
 
95
 
 
96
/******** quat friends ************/
 
97
 
 
98
quat operator + (const quat &a, const quat &b)
 
99
{
 
100
    return quat( a.s+b.s, a.v+b.v );
 
101
}
 
102
 
 
103
quat operator - (const quat &a, const quat &b)
 
104
{
 
105
    return quat( a.s-b.s, a.v-b.v );
 
106
}
 
107
 
 
108
quat operator - (const quat &a )
 
109
{
 
110
    return quat( -a.s, -a.v );
 
111
}
 
112
 
 
113
quat operator * ( const quat &a, const quat &b)
 
114
{
 
115
    return quat( a.s*b.s - a.v*b.v, a.s*b.v + b.s*a.v + a.v^b.v );
 
116
}
 
117
 
 
118
quat operator * ( const quat &a, const float t)
 
119
{
 
120
    return quat( a.v * t, a.s * t );
 
121
}
 
122
 
 
123
quat operator * ( const float t, const quat &a )
 
124
{
 
125
    return quat( a.v * t, a.s * t );
 
126
}
 
127
 
 
128
mat4 quat::to_mat4() const
 
129
{
 
130
    float xs, ys, zs, wx, wy, wz, xx, xy, xz, yy, yz, zz;
 
131
 
 
132
    float t  = 2.0f / (v*v + s*s);
 
133
 
 
134
    xs = v[VX]*t;   ys = v[VY]*t;   zs = v[VZ]*t;
 
135
    wx = s*xs;      wy = s*ys;      wz = s*zs;
 
136
    xx = v[VX]*xs;  xy = v[VX]*ys;  xz = v[VX]*zs;
 
137
    yy = v[VY]*ys;  yz = v[VY]*zs;  zz = v[VZ]*zs;
 
138
 
 
139
    mat4 matrix( 
 
140
           1.0f-(yy+zz), xy+wz,        xz-wy,        0.0f,
 
141
           xy-wz,        1.0f-(xx+zz), yz+wx,        0.0f,
 
142
           xz+wy,        yz-wx,        1.0f-(xx+yy), 0.0f,
 
143
           0.0f,         0.0f,         0.0f,         1.0f );
 
144
 
 
145
    return matrix;
 
146
}
 
147
 
 
148
/************************************************* quat_identity() *****/
 
149
/* Returns quaternion identity element                                 */
 
150
 
 
151
quat quat_identity() 
 
152
{
 
153
    return quat( vec3( 0.0, 0.0, 0.0 ), 1.0 );
 
154
}
 
155
 
 
156
/************************************************ quat_slerp() ********/
 
157
/* Quaternion spherical interpolation                                 */
 
158
 
 
159
quat quat_slerp(const quat &from, const quat &to, float t)
 
160
{
 
161
    quat to1;
 
162
    float omega, cosom, sinom, scale0, scale1;
 
163
 
 
164
    /* calculate cosine */
 
165
    cosom = from.v * to.v + from.s + to.s;
 
166
 
 
167
    /* Adjust signs (if necessary) */
 
168
    if ( cosom < 0.0 ) 
 
169
    {
 
170
        cosom = -cosom;
 
171
        to1 = -to;
 
172
    }
 
173
    else
 
174
    {
 
175
        to1 = to;
 
176
    }
 
177
 
 
178
    /* Calculate coefficients */
 
179
    if ((1.0 - cosom) > FUDGE ) 
 
180
    {
 
181
        /* standard case (slerp) */
 
182
        omega =  (float) acos( cosom );
 
183
        sinom =  (float) sin( omega );
 
184
        scale0 = (float) sin((1.0 - t) * omega) / sinom;
 
185
        scale1 = (float) sin(t * omega) / sinom;
 
186
    }
 
187
    else 
 
188
    {
 
189
        /* 'from' and 'to' are very close - just do linear interpolation */
 
190
        scale0 = 1.0f - t;
 
191
        scale1 = t;      
 
192
    }
 
193
 
 
194
    return scale0 * from + scale1 * to1;
 
195
}
 
196
 
 
197
/********************************************** set_angle() ************/
 
198
/* set rot angle (degrees)                                             */
 
199
 
 
200
void quat::set_angle(float f)
 
201
{
 
202
    vec3 axis = get_axis();
 
203
 
 
204
    s = (float) cos( DEG2RAD( f ) / 2.0 );
 
205
 
 
206
    v = axis * (float) sin(DEG2RAD(f) / 2.0);
 
207
}
 
208
 
 
209
/********************************************** scale_angle() ************/
 
210
/* scale rot angle (degrees)                                             */
 
211
 
 
212
void quat::scale_angle(float f)
 
213
{
 
214
    set_angle( f * get_angle() );
 
215
}
 
216
 
 
217
/********************************************** get_angle() ************/
 
218
/* get rot angle (degrees).  Assumes s is between -1 and 1             */
 
219
 
 
220
float quat::get_angle() const
 
221
{
 
222
    return (float) RAD2DEG( 2.0 * acos( s ) );
 
223
}
 
224
 
 
225
/********************************************* get_axis() **************/
 
226
 
 
227
vec3 quat::get_axis() const
 
228
{
 
229
    float scale = (float) sin( acos( s ) );
 
230
 
 
231
    if ( scale < FUDGE AND scale > -FUDGE )
 
232
        return vec3( 0.0, 0.0, 0.0 );
 
233
    else
 
234
        return  v / scale;
 
235
}
 
236
 
 
237
/******************************************* quat::print() ************/
 
238
 
 
239
void quat::print(FILE *dest, const char *name) const
 
240
{
 
241
    fprintf( dest, "%s:   v:<%3.2f %3.2f %3.2f>  s:%3.2f\n", 
 
242
        name, v[0], v[1], v[2], s );
 
243
}