All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends

interval_matrix.cpp

00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2011, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of Willow Garage, Inc. nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  */
00034 
00037 #include "fcl/ccd/interval_matrix.h"
00038 #include <iostream>
00039 
00040 namespace fcl
00041 {
00042 
00043 IMatrix3::IMatrix3() {}
00044 
00045 IMatrix3::IMatrix3(FCL_REAL v)
00046 {
00047   v_[0].setValue(v);
00048   v_[1].setValue(v);
00049   v_[2].setValue(v);
00050 }
00051 
00052 IMatrix3::IMatrix3(const Matrix3f& m)
00053 {
00054   v_[0] = m.getRow(0);
00055   v_[1] = m.getRow(1);
00056   v_[2] = m.getRow(2);
00057 }
00058 
00059 IMatrix3::IMatrix3(FCL_REAL m[3][3][2])
00060 {
00061   v_[0].setValue(m[0]);
00062   v_[1].setValue(m[1]);
00063   v_[2].setValue(m[2]);
00064 }
00065 
00066 IMatrix3::IMatrix3(FCL_REAL m[3][3])
00067 {
00068   v_[0].setValue(m[0]);
00069   v_[1].setValue(m[1]);
00070   v_[2].setValue(m[2]);
00071 }
00072 
00073 IMatrix3::IMatrix3(Interval m[3][3])
00074 {
00075   v_[0].setValue(m[0]);
00076   v_[1].setValue(m[1]);
00077   v_[2].setValue(m[2]);
00078 }
00079 
00080 IMatrix3::IMatrix3(const IVector3& v1, const IVector3& v2, const IVector3& v3)
00081 {
00082   v_[0] = v1;
00083   v_[1] = v2;
00084   v_[2] = v3;
00085 }
00086 
00087 void IMatrix3::setIdentity()
00088 {
00089   v_[0].setValue(1, 0, 0);
00090   v_[1].setValue(0, 1, 0);
00091   v_[2].setValue(0, 0, 1);
00092 }
00093 
00094 IVector3 IMatrix3::getColumn(size_t i) const
00095 {
00096   return IVector3(v_[0][i], v_[1][i], v_[2][i]);
00097 }
00098 
00099 const IVector3& IMatrix3::getRow(size_t i) const
00100 {
00101   return v_[i];
00102 }
00103 
00104 Vec3f IMatrix3::getColumnLow(size_t i) const
00105 {
00106   return Vec3f(v_[0][i][0], v_[1][i][0], v_[2][i][0]);
00107 }
00108 
00109 Vec3f IMatrix3::getRowLow(size_t i) const
00110 {
00111   return Vec3f(v_[i][0][0], v_[i][1][0], v_[i][2][0]);
00112 }
00113 
00114 Vec3f IMatrix3::getColumnHigh(size_t i) const
00115 {
00116   return Vec3f(v_[0][i][1], v_[1][i][1], v_[2][i][1]);
00117 }
00118 
00119 Vec3f IMatrix3::getRowHigh(size_t i) const
00120 {
00121   return Vec3f(v_[i][0][1], v_[i][1][1], v_[i][2][1]);
00122 }
00123 
00124 Matrix3f IMatrix3::getLow() const
00125 {
00126   return Matrix3f(v_[0][0][0], v_[0][1][0], v_[0][2][0],
00127                   v_[1][0][0], v_[1][1][0], v_[1][2][0],
00128                   v_[2][0][0], v_[2][1][0], v_[2][2][0]);
00129 }
00130 
00131 Matrix3f IMatrix3::getHigh() const
00132 {
00133   return Matrix3f(v_[0][0][1], v_[0][1][1], v_[0][2][1],
00134                   v_[1][0][1], v_[1][1][1], v_[1][2][1],
00135                   v_[2][0][1], v_[2][1][1], v_[2][2][1]);
00136 }
00137 
00138 IMatrix3 IMatrix3::operator * (const Matrix3f& m) const
00139 {
00140   const Vec3f& mc0 = m.getColumn(0);
00141   const Vec3f& mc1 = m.getColumn(1);
00142   const Vec3f& mc2 = m.getColumn(2);
00143 
00144   return IMatrix3(IVector3(v_[0].dot(mc0), v_[0].dot(mc1), v_[0].dot(mc2)),
00145                   IVector3(v_[1].dot(mc0), v_[1].dot(mc1), v_[1].dot(mc2)),
00146                   IVector3(v_[2].dot(mc0), v_[2].dot(mc1), v_[2].dot(mc2)));
00147 }
00148 
00149 IVector3 IMatrix3::operator * (const Vec3f& v) const
00150 {
00151   return IVector3(v_[0].dot(v), v_[1].dot(v), v_[2].dot(v));
00152 }
00153 
00154 IVector3 IMatrix3::operator * (const IVector3& v) const
00155 {
00156   return IVector3(v_[0].dot(v), v_[1].dot(v), v_[2].dot(v));
00157 }
00158 
00159 IMatrix3 IMatrix3::operator * (const IMatrix3& m) const
00160 {
00161   const IVector3& mc0 = m.getColumn(0);
00162   const IVector3& mc1 = m.getColumn(1);
00163   const IVector3& mc2 = m.getColumn(2);
00164 
00165   return IMatrix3(IVector3(v_[0].dot(mc0), v_[0].dot(mc1), v_[0].dot(mc2)),
00166                   IVector3(v_[1].dot(mc0), v_[1].dot(mc1), v_[1].dot(mc2)),
00167                   IVector3(v_[2].dot(mc0), v_[2].dot(mc1), v_[2].dot(mc2)));
00168 }
00169 
00170 IMatrix3& IMatrix3::operator *= (const Matrix3f& m)
00171 {
00172   const Vec3f& mc0 = m.getColumn(0);
00173   const Vec3f& mc1 = m.getColumn(1);
00174   const Vec3f& mc2 = m.getColumn(2);
00175 
00176   v_[0].setValue(v_[0].dot(mc0), v_[0].dot(mc1), v_[0].dot(mc2));
00177   v_[1].setValue(v_[1].dot(mc0), v_[1].dot(mc1), v_[1].dot(mc2));
00178   v_[2].setValue(v_[2].dot(mc0), v_[2].dot(mc1), v_[2].dot(mc2));
00179   return *this;
00180 }
00181 
00182 
00183 IMatrix3& IMatrix3::operator *= (const IMatrix3& m)
00184 {
00185   const IVector3& mc0 = m.getColumn(0);
00186   const IVector3& mc1 = m.getColumn(1);
00187   const IVector3& mc2 = m.getColumn(2);
00188 
00189   v_[0].setValue(v_[0].dot(mc0), v_[0].dot(mc1), v_[0].dot(mc2));
00190   v_[1].setValue(v_[1].dot(mc0), v_[1].dot(mc1), v_[1].dot(mc2));
00191   v_[2].setValue(v_[2].dot(mc0), v_[2].dot(mc1), v_[2].dot(mc2));
00192   return *this;
00193 }
00194 
00195 IMatrix3 IMatrix3::operator + (const IMatrix3& m) const
00196 {
00197   return IMatrix3(v_[0] + m.v_[0], v_[1] + m.v_[1], v_[2] + m.v_[2]);
00198 }
00199 
00200 IMatrix3& IMatrix3::operator += (const IMatrix3& m)
00201 {
00202   v_[0] += m.v_[0];
00203   v_[1] += m.v_[1];
00204   v_[2] += m.v_[2];
00205   return *this;
00206 }
00207 
00208 IMatrix3 IMatrix3::operator - (const IMatrix3& m) const
00209 {
00210   return IMatrix3(v_[0] - m.v_[0], v_[1] - m.v_[1], v_[2] - m.v_[2]);
00211 }
00212 
00213 IMatrix3& IMatrix3::operator -= (const IMatrix3& m)
00214 {
00215   v_[0] -= m.v_[0];
00216   v_[1] -= m.v_[1];
00217   v_[2] -= m.v_[2];
00218   return *this;
00219 }
00220 
00221 void IMatrix3::print() const
00222 {
00223   std::cout << "[" << v_[0][0][0] << "," << v_[0][0][1] << "]" << " [" << v_[0][1][0] << "," << v_[0][1][1] << "]" << " [" << v_[0][2][0] << "," << v_[0][2][1] << "]" << std::endl;
00224   std::cout << "[" << v_[1][0][0] << "," << v_[1][0][1] << "]" << " [" << v_[1][1][0] << "," << v_[1][1][1] << "]" << " [" << v_[1][2][0] << "," << v_[1][2][1] << "]" << std::endl;
00225   std::cout << "[" << v_[2][0][0] << "," << v_[2][0][1] << "]" << " [" << v_[2][1][0] << "," << v_[2][1][1] << "]" << " [" << v_[2][2][0] << "," << v_[2][2][1] << "]" << std::endl;
00226 }
00227 
00228 }