kimcinoo
/
efl
forked from enlightenment/efl
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
efl/src/lib/eina/eina_quaternion.c

610 lines
17 KiB
C
Raw Blame History

/* EINA - Drawing Library
* Copyright (C) 2007-2014 Jorge Luis Zapata
*
* This library is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 2.1 of the License, or (at your option) any later version.
*
* This library is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License along with this library.
* If not, see <http://www.gnu.org/licenses/>.
*/
#ifdef HAVE_CONFIG_H
# include "config.h"
#endif
#include "eina_private.h"
#include <math.h>
#include "eina_fp.h"
#include "eina_matrix.h"
#include "eina_quaternion.h"
EAPI Eina_F16p16
eina_quaternion_f16p16_norm(const Eina_Quaternion_F16p16 *q)
{
Eina_F16p16 s;
s = eina_f16p16_add(eina_f16p16_add(eina_f16p16_mul(q->w, q->w),
eina_f16p16_mul(q->x, q->x)),
eina_f16p16_add(eina_f16p16_mul(q->y, q->y),
eina_f16p16_mul(q->z, q->z)));
return eina_f16p16_sqrt(s);
}
EAPI void
eina_quaternion_f16p16_negative(Eina_Quaternion_F16p16 *out,
const Eina_Quaternion_F16p16 *in)
{
out->w = eina_f16p16_sub(0, in->w);
out->x = eina_f16p16_sub(0, in->x);
out->y = eina_f16p16_sub(0, in->y);
out->z = eina_f16p16_sub(0, in->z);
}
EAPI void
eina_quaternion_f16p16_add(Eina_Quaternion_F16p16 *out,
const Eina_Quaternion_F16p16 *a,
const Eina_Quaternion_F16p16 *b)
{
out->w = eina_f16p16_add(a->w, b->w);
out->x = eina_f16p16_add(a->x, b->x);
out->y = eina_f16p16_add(a->y, b->y);
out->z = eina_f16p16_add(a->z, b->z);
}
EAPI void
eina_quaternion_f16p16_mul(Eina_Quaternion_F16p16 *out,
const Eina_Quaternion_F16p16 *a,
const Eina_Quaternion_F16p16 *b)
{
out->w = eina_f16p16_sub(eina_f16p16_sub(eina_f16p16_mul(a->w, b->w),
eina_f16p16_mul(a->x, b->x)),
eina_f16p16_sub(eina_f16p16_mul(a->y, b->y),
eina_f16p16_mul(a->z, b->z)));
out->x = eina_f16p16_add(eina_f16p16_add(eina_f16p16_mul(a->w, b->x),
eina_f16p16_mul(a->x, b->w)),
eina_f16p16_sub(eina_f16p16_mul(a->y, b->z),
eina_f16p16_mul(a->z, b->y)));
out->y = eina_f16p16_add(eina_f16p16_sub(eina_f16p16_mul(a->w, b->y),
eina_f16p16_mul(a->x, b->z)),
eina_f16p16_add(eina_f16p16_mul(a->y, b->w),
eina_f16p16_mul(a->z, b->x)));
out->z = eina_f16p16_add(eina_f16p16_add(eina_f16p16_mul(a->w, b->z),
eina_f16p16_mul(a->x, b->y)),
eina_f16p16_sub(eina_f16p16_mul(a->z, b->w),
eina_f16p16_mul(a->y, b->y)));
}
EAPI void
eina_quaternion_f16p16_scale(Eina_Quaternion_F16p16 *out,
const Eina_Quaternion_F16p16 *a,
Eina_F16p16 b)
{
out->w = eina_f16p16_scale(a->w, b);
out->x = eina_f16p16_scale(a->x, b);
out->y = eina_f16p16_scale(a->y, b);
out->z = eina_f16p16_scale(a->z, b);
}
EAPI void
eina_quaternion_f16p16_conjugate(Eina_Quaternion_F16p16 *out,
const Eina_Quaternion_F16p16 *in)
{
out->w = in->w;
out->x = eina_f16p16_sub(0, in->x);
out->y = eina_f16p16_sub(0, in->y);
out->z = eina_f16p16_sub(0, in->z);
}
EAPI Eina_F16p16
eina_quaternion_f16p16_dot(const Eina_Quaternion_F16p16 *a,
const Eina_Quaternion_F16p16 *b)
{
return eina_f16p16_add(eina_f16p16_add(eina_f16p16_mul(a->w, b->w),
eina_f16p16_mul(a->x, b->x)),
eina_f16p16_add(eina_f16p16_mul(a->y, b->y),
eina_f16p16_mul(a->z, b->z)));
}
EAPI void
eina_quaternion_f16p16_normalized(Eina_Quaternion_F16p16 *out,
const Eina_Quaternion_F16p16 *in)
{
Eina_F16p16 norm;
norm = eina_quaternion_f16p16_norm(in);
eina_quaternion_f16p16_scale(out, in,
eina_f16p16_div(eina_f16p16_int_from(1),
norm));
}
EAPI void
eina_quaternion_f16p16_lerp(Eina_Quaternion_F16p16 *out,
const Eina_Quaternion_F16p16 *a,
const Eina_Quaternion_F16p16 *b,
Eina_F16p16 pos)
{
if (pos == 0)
{
*out = *a;
return ;
}
else if (pos == eina_f16p16_int_from(1))
{
*out = *b;
return ;
}
out->w = eina_f16p16_add(a->w,
eina_f16p16_mul(eina_f16p16_sub(b->w, a->w),
pos));
out->x = eina_f16p16_add(a->x,
eina_f16p16_mul(eina_f16p16_sub(b->x, a->x),
pos));
out->y = eina_f16p16_add(a->y,
eina_f16p16_mul(eina_f16p16_sub(b->y, a->y),
pos));
out->z = eina_f16p16_add(a->z,
eina_f16p16_mul(eina_f16p16_sub(b->z, a->z),
pos));
}
EAPI void
eina_quaternion_f16p16_slerp(Eina_Quaternion_F16p16 *out,
const Eina_Quaternion_F16p16 *a,
const Eina_Quaternion_F16p16 *b,
Eina_F16p16 pos)
{
Eina_Quaternion_F16p16 bp;
Eina_Quaternion_F16p16 left, right;
Eina_F16p16 dot;
Eina_F16p16 pos1, pos2;
if (pos == 0)
{
*out = *a;
return ;
}
else if (pos == eina_f16p16_int_from(1))
{
*out = *b;
return ;
}
dot = eina_quaternion_f16p16_dot(a, b);
if (dot >= eina_f16p16_int_from(0))
{
bp = *b;
}
else
{
eina_quaternion_f16p16_negative(&bp, b);
dot = eina_f16p16_sub(0, dot);
}
pos1 = eina_f16p16_sub(eina_f16p16_int_from(1), pos);
pos2 = pos;
if (eina_f16p16_sub(eina_f16p16_int_from(1), dot) > eina_f16p16_float_from(0.0000001))
{
Eina_F32p32 angle;
Eina_F16p16 sangle;
angle = eina_f32p32_double_from(acos(eina_f16p16_double_to(dot)));
sangle = eina_f32p32_to_f16p16(eina_f32p32_sin(angle));
if (sangle > eina_f16p16_float_from(0.0000001))
{
pos1 = eina_f32p32_to_f16p16(eina_f32p32_div(eina_f32p32_sin(eina_f32p32_mul(eina_f16p16_to_f32p32(pos1), angle)),
eina_f16p16_to_f32p32(sangle)));
pos2 = eina_f32p32_to_f16p16(eina_f32p32_div(eina_f32p32_sin(eina_f32p32_mul(eina_f16p16_to_f32p32(pos2), angle)),
eina_f16p16_to_f32p32(sangle)));
}
}
eina_quaternion_f16p16_scale(&left, a, pos1);
eina_quaternion_f16p16_scale(&right, b, pos2);
eina_quaternion_f16p16_add(out, &left, &right);
}
EAPI void
eina_quaternion_f16p16_nlerp(Eina_Quaternion_F16p16 *out,
const Eina_Quaternion_F16p16 *a,
const Eina_Quaternion_F16p16 *b,
Eina_F16p16 pos)
{
Eina_Quaternion_F16p16 bp, left, right;
Eina_Quaternion_F16p16 not_normalize;
double dot;
if (pos == 0)
{
*out = *a;
return ;
}
else if (pos == eina_f16p16_int_from(1))
{
*out = *b;
return ;
}
dot = eina_quaternion_f16p16_dot(a, b);
if (dot >= eina_f16p16_int_from(0))
{
bp = *b;
}
else
{
eina_quaternion_f16p16_negative(&bp, b);
dot = -dot;
}
eina_quaternion_f16p16_scale(&left, a,
eina_f16p16_sub(eina_f16p16_int_from(1), pos));
eina_quaternion_f16p16_scale(&right, b, pos);
eina_quaternion_f16p16_add(&not_normalize, &left, &right);
eina_quaternion_f16p16_normalized(out, &not_normalize);
}
EAPI void
eina_quaternion_f16p16_rotate(Eina_Point_3D_F16p16 *p,
const Eina_Point_3D_F16p16 *center,
const Eina_Quaternion_F16p16 *q)
{
const Eina_Point_3D_F16p16 zero = { 0, 0, 0 };
Eina_F16p16 x, y, z;
Eina_F16p16 uvx, uvy, uvz;
Eina_F16p16 uuvx, uuvy, uuvz;
if (!center) center = &zero;
x = eina_f16p16_sub(p->x, center->x);
y = eina_f16p16_sub(p->y, center->y);
z = eina_f16p16_sub(p->z, center->z);
uvx = eina_f16p16_sub(eina_f16p16_mul(q->y, z),
eina_f16p16_mul(q->z, y));
uvy = eina_f16p16_sub(eina_f16p16_mul(q->z, x),
eina_f16p16_mul(q->x, z));
uvz = eina_f16p16_sub(eina_f16p16_mul(q->x, y),
eina_f16p16_mul(q->y, x));
uuvx = eina_f16p16_sub(eina_f16p16_mul(q->y, uvz),
eina_f16p16_mul(q->z, uvy));
uuvy = eina_f16p16_sub(eina_f16p16_mul(q->z, uvx),
eina_f16p16_mul(q->x, uvz));
uuvz = eina_f16p16_sub(eina_f16p16_mul(q->x, uvy),
eina_f16p16_mul(q->y, uvx));
uvx = eina_f16p16_mul(eina_f16p16_scale(q->w, 2), uvx);
uvy = eina_f16p16_mul(eina_f16p16_scale(q->w, 2), uvy);
uvz = eina_f16p16_mul(eina_f16p16_scale(q->w, 2), uvz);
uuvx = eina_f16p16_scale(uuvx, 2);
uuvy = eina_f16p16_scale(uuvy, 2);
uuvz = eina_f16p16_scale(uuvz, 2);
p->x = eina_f16p16_add(eina_f16p16_add(center->x, x),
eina_f16p16_add(uvx, uuvx));
p->y = eina_f16p16_add(eina_f16p16_add(center->y, y),
eina_f16p16_add(uvy, uuvy));
p->z = eina_f16p16_add(eina_f16p16_add(center->z, z),
eina_f16p16_add(uvz, uuvz));
}
EAPI void
eina_quaternion_f16p16_rotation_matrix3_get(Eina_Matrix3_F16p16 *m,
const Eina_Quaternion_F16p16 *q)
{
Eina_F16p16 x, y, z;
Eina_F16p16 xx, xy, xz;
Eina_F16p16 yy, yz;
Eina_F16p16 zz;
Eina_F16p16 wx, wy, wz;
x = eina_f16p16_scale(q->x, 2);
y = eina_f16p16_scale(q->y, 2);
z = eina_f16p16_scale(q->z, 2);
xx = eina_f16p16_mul(q->x, x);
xy = eina_f16p16_mul(q->x, y);
xz = eina_f16p16_mul(q->x, z);
yy = eina_f16p16_mul(q->y, y);
yz = eina_f16p16_mul(q->y, z);
zz = eina_f16p16_mul(q->z, z);
wx = eina_f16p16_mul(q->w, x);
wy = eina_f16p16_mul(q->w, y);
wz = eina_f16p16_mul(q->w, z);
m->xx = eina_f16p16_sub(eina_f16p16_int_to(1),
eina_f16p16_add(yy, zz));
m->xy = eina_f16p16_add(xy, wz);
m->xz = eina_f16p16_sub(xz, wy);
m->yx = eina_f16p16_sub(xy, wz);
m->yy = eina_f16p16_sub(eina_f16p16_int_to(1),
eina_f16p16_add(xx, zz));
m->yz = eina_f16p16_add(yz, wx);
m->zx = eina_f16p16_add(xz, wy);
m->zy = eina_f16p16_add(yz, wx);
m->zz = eina_f16p16_sub(eina_f16p16_int_to(1),
eina_f16p16_add(xx, yy));
}
EAPI double
eina_quaternion_norm(const Eina_Quaternion *q)
{
double s;
s = q->w * q->w + q->x * q->x + q->y * q->y + q->z * q->z;
return sqrt(s);
}
EAPI void
eina_quaternion_negative(Eina_Quaternion *out,
const Eina_Quaternion *in)
{
out->w = - in->w;
out->x = - in->x;
out->y = - in->y;
out->z = - in->z;
}
EAPI void
eina_quaternion_add(Eina_Quaternion *out,
const Eina_Quaternion *a,
const Eina_Quaternion *b)
{
out->w = a->w + b->w;
out->x = a->x + b->x;
out->y = a->y + b->y;
out->z = a->z + b->z;
}
EAPI void
eina_quaternion_mul(Eina_Quaternion *out,
const Eina_Quaternion *a,
const Eina_Quaternion *b)
{
out->w = a->w * b->w - a->x * b->x - a->y * b->y - a->z * b->z;
out->x = a->w * b->x + a->x * b->w + a->y * b->z - a->z * b->y;
out->y = a->w * b->y - a->x * b->z + a->y * b->w + a->z * b->x;
out->z = a->w * b->z + a->x * b->y - a->y * b->y + a->z * b->w;
}
EAPI void
eina_quaternion_scale(Eina_Quaternion *out,
const Eina_Quaternion *a,
double b)
{
out->w = a->w * b;
out->x = a->x * b;
out->y = a->y * b;
out->z = a->z * b;
}
EAPI void
eina_quaternion_conjugate(Eina_Quaternion *out,
const Eina_Quaternion *in)
{
out->w = in->w;
out->x = - in->x;
out->y = - in->y;
out->z = - in->z;
}
EAPI double
eina_quaternion_dot(const Eina_Quaternion *a,
const Eina_Quaternion *b)
{
return a->w * b->w + a->x * b->x + a->y * b->y + a->z * b->z;
}
EAPI void
eina_quaternion_normalized(Eina_Quaternion *out,
const Eina_Quaternion *in)
{
double norm;
norm = eina_quaternion_norm(in);
eina_quaternion_scale(out, in, 1.0 / norm);
}
EAPI void
eina_quaternion_lerp(Eina_Quaternion *out,
const Eina_Quaternion *a,
const Eina_Quaternion *b,
double pos)
{
if (pos == 0)
{
*out = *a;
return ;
}
else if (pos == 1.0)
{
*out = *b;
return ;
}
out->w = a->w + pos * (b->w - a->w);
out->x = a->x + pos * (b->x - a->x);
out->y = a->y + pos * (b->y - a->y);
out->z = a->z + pos * (b->z - a->z);
}
EAPI void
eina_quaternion_slerp(Eina_Quaternion *out,
const Eina_Quaternion *a,
const Eina_Quaternion *b,
double pos)
{
Eina_Quaternion bp;
Eina_Quaternion left, right;
double dot;
double pos1, pos2;
if (pos == 0)
{
*out = *a;
return ;
}
else if (pos == 1.0)
{
*out = *b;
return ;
}
dot = eina_quaternion_dot(a, b);
if (dot >= 0)
{
bp = *b;
}
else
{
eina_quaternion_negative(&bp, b);
dot = -dot;
}
pos1 = 1.0 - pos;
pos2 = pos;
if ((1.0 - dot) > 0.0000001)
{
double angle;
double sangle;
angle = acos(dot);
sangle = sin(angle);
if (sangle > 0.0000001)
{
pos1 = sin(pos1 * angle) / sangle;
pos2 = sin(pos2 * angle) / sangle;
}
}
eina_quaternion_scale(&left, a, pos1);
eina_quaternion_scale(&right, b, pos2);
eina_quaternion_add(out, &left, &right);
}
EAPI void
eina_quaternion_nlerp(Eina_Quaternion *out,
const Eina_Quaternion *a,
const Eina_Quaternion *b,
double pos)
{
Eina_Quaternion bp, left, right;
Eina_Quaternion not_normalize;
double dot;
if (pos == 0)
{
*out = *a;
return ;
}
else if (pos == 1.0)
{
*out = *b;
return ;
}
dot = eina_quaternion_dot(a, b);
if (dot >= 0)
{
bp = *b;
}
else
{
eina_quaternion_negative(&bp, b);
dot = -dot;
}
eina_quaternion_scale(&left, a, 1.0 - pos);
eina_quaternion_scale(&right, b, pos);
eina_quaternion_add(&not_normalize, &left, &right);
eina_quaternion_normalized(out, &not_normalize);
}
EAPI void
eina_quaternion_rotate(Eina_Point_3D *p,
const Eina_Point_3D *center,
const Eina_Quaternion *q)
{
double x, y, z;
double uvx, uvy, uvz;
double uuvx, uuvy, uuvz;
x = p->x - center->x;
y = p->y - center->y;
z = p->z - center->z;
uvx = q->y * z - q->z * y;
uvy = q->z * x - q->x * z;
uvz = q->x * y - q->y * x;
uuvx = q->y * uvz - q->z * uvy;
uuvy = q->z * uvx - q->x * uvz;
uuvz = q->x * uvy - q->y * uvx;
uvx *= (2.0f * q->w);
uvy *= (2.0f * q->w);
uvz *= (2.0f * q->w);
uuvx *= 2.0f;
uuvy *= 2.0f;
uuvz *= 2.0f;
p->x = center->x + x + uvx + uuvx;
p->y = center->y + y + uvy + uuvy;
p->z = center->z + z + uvz + uuvz;
}
EAPI void
eina_quaternion_rotation_matrix3_get(Eina_Matrix3 *m,
const Eina_Quaternion *q)
{
double x, y, z;
double xx, xy, xz;
double yy, yz;
double zz;
double wx, wy, wz;
x = 2.0 * q->x;
y = 2.0 * q->y;
z = 2.0 * q->z;
xx = q->x * x;
xy = q->x * y;
xz = q->x * z;
yy = q->y * y;
yz = q->y * z;
zz = q->z * z;
wx = q->w * x;
wy = q->w * y;
wz = q->w * z;
m->xx = 1.0 - yy - zz;
m->xy = xy + wz;
m->xz = xz - wy;
m->yx = xy - wz;
m->yy = 1.0 - xx - zz;
m->yz = yz + wx;
m->zx = xz + wy;
m->zy = yz - wx;
m->zz = 1.0 - xx - yy;
}
EAPI void
eina_matrix3_quaternion_get(Eina_Quaternion *q,
const Eina_Matrix3 *m)
{
}
Reference in New Issue View Git Blame Copy Permalink