toxic: build_basis.inl Source File

00001 /*
00002     Sheep - A Rigid Body Dynamics Engine
00003     Copyright (C) 2001-2004 Francois Beaune
00004     Contact: http://toxicengine.sourceforge.net/
00005 
00006     This file is part of Sheep.
00007 
00008     Sheep is free software; you can redistribute it and/or modify
00009     it under the terms of the GNU General Public License as published by
00010     the Free Software Foundation; either version 2 of the License, or
00011     (at your option) any later version.
00012 
00013     Sheep is distributed in the hope that it will be useful,
00014     but WITHOUT ANY WARRANTY; without even the implied warranty of
00015     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00016     GNU General Public License for more details.
00017 
00018     You should have received a copy of the GNU General Public License
00019     along with Sheep; if not, write to the Free Software
00020     Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
00021 */
00022 
00023 inline
00024 void build_basis(const Vector3 &u, Vector3 *v, Vector3 *w) {
00025     assert(u.IsUnitLength());
00026     assert(v && w);
00027 
00028     const Real uz2 = u.m_z * u.m_z;
00029     Real norm = uz2 + u.m_y * u.m_y;
00030 
00031     if(norm > EPS) {
00032         norm = sqrt(norm);
00033 
00034         // v = u ^ (1,0,0).
00035         v->m_x = 0.0;
00036         v->m_y = u.m_z / norm;
00037         v->m_z = -u.m_y / norm;
00038 
00039         // w = u ^ v.
00040         w->m_x = u.m_y * v->m_z - v->m_y * u.m_z;
00041         w->m_y = -v->m_z * u.m_x;
00042         w->m_z = u.m_x * v->m_y;
00043     } else {
00044         norm = uz2 + u.m_x * u.m_x;
00045         assert(norm > EPS);
00046         norm = sqrt(norm);
00047 
00048         // v = u ^ (0,1,0).
00049         v->m_x = -u.m_z / norm;
00050         v->m_y = 0.0;
00051         v->m_z = u.m_x / norm;
00052 
00053         // w = u ^ v.
00054         w->m_x = u.m_y * v->m_z;
00055         w->m_y = u.m_z * v->m_x - v->m_z * u.m_x;
00056         w->m_z = -v->m_x * u.m_y;
00057     }
00058 
00059     // Make sure that v and w vectors are unit-length.
00060     assert(v->IsUnitLength());
00061     assert(w->IsUnitLength());
00062 
00063     // Make sure that (u, v, w) is an orthogonal basis.
00064     assert(fz(u * (*v)));
00065     assert(fz(u * (*w)));
00066     assert(fz((*v) * (*w)));
00067 
00068     // Make sure that (u, v, w) is right handed.
00069     assert(feq(u ^ (*v), *w));
00070     assert(feq((*v) ^ (*w), u));
00071     assert(feq((*w) ^ u, *v));
00072 }