/* -*- Objc -*- */ /* * $Id: Quaternion.m,v 1.1 2003/06/30 15:55:03 c-leo Exp $ * * Copyright (C) 2003 Free Software Foundation, Inc. * * This file is part of GNU Hégémonie. * * This program is free software; you can redistribute it and/or * modify it under the terms of the GNU General Public License * as published by the Free Software Foundation; either version 2 * of the License, or (at your option) any later version. * * This program 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 General Public License for more details. * * You should have received a copy of the GNU General Public License * along with this program; if not, write to the Free Software * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. */ #include #include #include #include "Quaternion.h" /* *value min of the cos of the angle between the quaternions directions * while interpolation */ #define MIN_VALUE_SPHERICAL_INTERPOLATION 0.1f /* numerical uncertainty while calulatation while floats */ #define DELTA 0.000000001 @implementation Quaternion /* * This method builds an instance of Quaternion, * with all the attributes initialised at 0 */ - (id) init { self = [super init]; if (self != nil) { _x = _y = _z = 0.0f; _w = 1.0f; } return self; } /* * This method builds an instance of Quaternion, * with all the attributes initialised with the parameters */ - (id) initWithValuesX: (float) x Y: (float) y Z: (float) z W: (float) w { self = [super init]; if (self != nil) { _x = x; _y = y; _z = z; _w = w; } return self; } /* * Frees memory. */ - (void) dealloc { if (_matrix) free (_matrix); [super dealloc]; } /* This method builds an instance of Quaternion, * by converting the matrix in parameter * This matrix must be a 4x4 matrix, otherwise the function will crash. */ - (id) initWithMatrix: (float *)matrix { NSParameterAssert (matrix); self = [super init]; if (self != nil) { float trace = matrix[0] + matrix[5] + matrix[10] + 1; float scale = 0.0f; if (trace > 0 + DELTA) { scale = (float) (sqrt (trace) * 2); _x = (matrix[9] - matrix[6] ) / scale; _y = (matrix[2] - matrix[8] ) / scale; _z = (matrix[4] - matrix[1] ) / scale; _w = 0.25f * scale; } else { if (matrix[0] > matrix[5] && matrix[0] > matrix[10]) { scale = (float) sqrt (1.0f + matrix[0] - matrix[5] - matrix[10]) *2.0f; _x = 0.25f * scale; _y = (matrix[4] + matrix[1] ) / scale; _z = (matrix[2] + matrix[8] ) / scale; _w = (matrix[9] - matrix[6] ) / scale; } else if (matrix[5] > matrix[10]) { scale = (float) sqrt (1.0f + matrix[5] - matrix[0] - matrix[10]) *2.0f; _x = (matrix[4] + matrix[1]) / scale; _y = 0.25f * scale; _z = (matrix[9] + matrix[6]) / scale; _w = (matrix[2] - matrix[8]) / scale; } else { scale = (float) sqrt (1.0f + matrix[10] - matrix[0] - matrix[5]) *2.0f; _x = (matrix[2] + matrix[8]) / scale; _y = (matrix[9] + matrix[6]) / scale; _z = 0.25f * scale; _w = (matrix[4] - matrix[1]) / scale; } } } return self; } /* * This method returns the matrix 4x4 corresponding to the quaternion. * So it converts a quaternion to a matrix 4x4. */ - (float *) createMatrix { if ((_matrix = (float *) malloc (16 * sizeof (float))) == NULL) { fprintf (stderr, "It ' s impossible to create a 4x4 matrix: " "memory allocation has failed.\n"); exit (-1); } float xx = _x * _x; float xy = _x * _y; float xz = _x * _z; float xw = _x * _w; float yy = _y * _y; float yz = _y * _z; float yw = _y * _w; float zz = _z * _z; float zw = _z * _w; /* first row */ _matrix[0] = 1.0f - 2.0f * ( yy + zz ); _matrix[1] = 2.0f * ( xy - zw ); _matrix[2] = 2.0f * ( xz + yw ); _matrix[3] = 0; /* second row */ _matrix[4] = 2.0f * ( xy + zw ); _matrix[5] = 1.0f - 2.0f * ( xx + zz ); _matrix[6] = 2.0f * ( yz - xw ); _matrix[7] = 0; /* third row */ _matrix[8] = 2.0f * ( xz - yw ); _matrix[9] = 2.0f * ( yz + xw ); _matrix[10] = 1.0f - 2.0f * ( xx + yy ); _matrix[11] = 0; /* fourth row */ _matrix[12] = 0; _matrix[13] = 0; _matrix[14] = 0; _matrix[15] = 1.0f; return _matrix; } /* * Says if the quaternion in parameter is egal to the current quaternionn, i.e. * the values x, y, z and z are egal. */ - (BOOL) isEqual: (Quaternion *) quat { NSParameterAssert (quat); return _x == quat->_x && _y == quat->_y && _z == quat->_z && _w == quat->_w; } - (void) negate { _x = -_x; _y = -_y; _z = -_z; _w = -_w; } /* Performs a spherical linear interpolation of the two quaternions in * parameter, and the current quaternion becomes the quaternion interpolated. * q1 is the starting quaternion, q2 the final quaternion. * t represents the time for the interpolation; it must be valued * between 0 and 1. */ - (void) slerp: (Quaternion *) q1 with: (Quaternion *) q2 accordingTime: (float) t { NSParameterAssert (q1); NSParameterAssert (q2); NSParameterAssert (t >= 0); NSParameterAssert (t <= 1); if ([q1 isEqual: q2]) { [self initWithValuesX: q1->_x Y: q1->_y Z: q1->_z W: q1->_w]; } else { float dotProduct = (q1->_x * q2->_x) + (q1->_y * q2->_y) + (q1->_z * q2->_z) + (q1->_w * q2->_w); float scale0 = 1 - t, scale1 = t; if (dotProduct < 0.0f) /* angle >= 90° */ { [q2 negate]; dotProduct = -dotProduct; } /* Next, we want to actually calculate the spherical interpolation. * Since this calculation is quite computationally expensive, we want to * only perform it if the angle between the 2 quaternions is large * enough to warrant it (otherwise we do a linear interpolation). */ if (1 - dotProduct > MIN_VALUE_SPHERICAL_INTERPOLATION) { float theta = (float) acos (dotProduct); float sinTheta = (float) sin (theta); scale0 = (float) sin ((1-t) * theta) / sinTheta; scale1 = (float) sin (t * theta) / sinTheta; } _x = (scale0 * q1->_x) + (scale1 * q2->_x); _y = (scale0 * q1->_y) + (scale1 * q2->_y); _z = (scale0 * q1->_z) + (scale1 * q2->_z); _w = (scale0 * q1->_w) + (scale1 * q2->_w); } } @end