All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends

taylor_vector.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/taylor_vector.h"
00038 
00039 namespace fcl
00040 {
00041 
00042 TVector3::TVector3()
00043 {
00044 }
00045 
00046 TVector3::TVector3(const boost::shared_ptr<TimeInterval>& time_interval)
00047 {
00048   setTimeInterval(time_interval);
00049 }
00050 
00051 TVector3::TVector3(TaylorModel v[3])
00052 {
00053   i_[0] = v[0];
00054   i_[1] = v[1];
00055   i_[2] = v[2];
00056 }
00057 
00058 TVector3::TVector3(const TaylorModel& v1, const TaylorModel& v2, const TaylorModel& v3)
00059 {
00060   i_[0] = v1;
00061   i_[1] = v2;
00062   i_[2] = v3;
00063 }
00064 
00065 TVector3::TVector3(const Vec3f& v, const boost::shared_ptr<TimeInterval>& time_interval)
00066 {
00067   i_[0] = TaylorModel(v[0], time_interval);
00068   i_[1] = TaylorModel(v[1], time_interval);
00069   i_[2] = TaylorModel(v[2], time_interval);
00070 }
00071 
00072 void TVector3::setZero()
00073 {
00074   i_[0].setZero();
00075   i_[1].setZero();
00076   i_[2].setZero();
00077 }
00078 
00079 TVector3 TVector3::operator + (const TVector3& other) const
00080 {
00081   return TVector3(i_[0] + other.i_[0], i_[1] + other.i_[1], i_[2] + other.i_[2]);
00082 }
00083 
00084 TVector3 TVector3::operator + (FCL_REAL d) const
00085 {
00086   return TVector3(i_[0], i_[1], i_[2] + d);
00087 }
00088 
00089 TVector3& TVector3::operator += (FCL_REAL d)
00090 {
00091   i_[2] += d;
00092   return *this;
00093 }
00094 
00095 TVector3 TVector3::operator - (const TVector3& other) const
00096 {
00097   return TVector3(i_[0] - other.i_[0], i_[1] - other.i_[1], i_[2] - other.i_[2]);
00098 }
00099 
00100 TVector3& TVector3::operator += (const TVector3& other)
00101 {
00102   i_[0] += other.i_[0];
00103   i_[1] += other.i_[1];
00104   i_[2] += other.i_[2];
00105   return *this;
00106 }
00107 
00108 TVector3& TVector3::operator -= (const TVector3& other)
00109 {
00110   i_[0] -= other.i_[0];
00111   i_[1] -= other.i_[1];
00112   i_[2] -= other.i_[2];
00113   return *this;
00114 }
00115 
00116 TVector3 TVector3::operator * (const TaylorModel& d) const
00117 {
00118   return TVector3(i_[0] * d, i_[1] * d, i_[2] * d);
00119 }
00120 
00121 TVector3& TVector3::operator *= (const TaylorModel& d)
00122 {
00123   i_[0] *= d;
00124   i_[1] *= d;
00125   i_[2] *= d;
00126   return *this;
00127 }
00128 
00129 TVector3 TVector3::operator * (FCL_REAL d) const
00130 {
00131   return TVector3(i_[0] * d, i_[1] * d, i_[2] * d);
00132 }
00133 
00134 TVector3& TVector3::operator *= (FCL_REAL d)
00135 {
00136   i_[0] *= d;
00137   i_[1] *= d;
00138   i_[2] *= d;
00139   return *this;
00140 }
00141 
00142 const TaylorModel& TVector3::operator [] (size_t i) const
00143 {
00144   return i_[i];
00145 }
00146 
00147 TaylorModel& TVector3::operator [] (size_t i)
00148 {
00149   return i_[i];
00150 }
00151 
00152 TaylorModel TVector3::dot(const TVector3& other) const
00153 {
00154   return i_[0] * other.i_[0] + i_[1] * other.i_[1] + i_[2] * other.i_[2];
00155 }
00156 
00157 TVector3 TVector3::cross(const TVector3& other) const
00158 {
00159   return TVector3(i_[1] * other.i_[2] - i_[2] * other.i_[1], 
00160                   i_[2] * other.i_[0] - i_[0] * other.i_[2],
00161                   i_[0] * other.i_[1] - i_[1] * other.i_[0]);
00162 }
00163 
00164 TaylorModel TVector3::dot(const Vec3f& other) const
00165 {
00166   return i_[0] * other[0] + i_[1] * other[1] + i_[2] * other[2];
00167 }
00168 
00169 TVector3 TVector3::cross(const Vec3f& other) const
00170 {
00171   return TVector3(i_[1] * other[2] - i_[2] * other[1], 
00172                   i_[2] * other[0] - i_[0] * other[2],
00173                   i_[0] * other[1] - i_[1] * other[0]);
00174 }
00175 
00176 FCL_REAL TVector3::volumn() const
00177 {
00178   return i_[0].getBound().diameter() * i_[1].getBound().diameter() * i_[2].getBound().diameter();
00179 }
00180 
00181 IVector3 TVector3::getBound() const
00182 {
00183   return IVector3(i_[0].getBound(), i_[1].getBound(), i_[2].getBound());
00184 }
00185 
00186 void TVector3::print() const
00187 {
00188   i_[0].print();
00189   i_[1].print();
00190   i_[2].print();
00191 }
00192 
00193 IVector3 TVector3::getBound(FCL_REAL t) const
00194 {
00195   return IVector3(i_[0].getBound(t), i_[1].getBound(t), i_[2].getBound(t));
00196 }
00197 
00198 TaylorModel TVector3::squareLength() const
00199 {
00200   return i_[0] * i_[0] + i_[1] * i_[1] + i_[2] * i_[2];
00201 }
00202 
00203 void TVector3::setTimeInterval(const boost::shared_ptr<TimeInterval>& time_interval)
00204 {
00205   i_[0].setTimeInterval(time_interval);
00206   i_[1].setTimeInterval(time_interval);
00207   i_[2].setTimeInterval(time_interval);
00208 }
00209 
00210 void generateTVector3ForLinearFunc(TVector3& v, const Vec3f& position, const Vec3f& velocity)
00211 {
00212   generateTaylorModelForLinearFunc(v.i_[0], position[0], velocity[0]);
00213   generateTaylorModelForLinearFunc(v.i_[1], position[1], velocity[1]);
00214   generateTaylorModelForLinearFunc(v.i_[2], position[2], velocity[2]);
00215 }
00216 
00217 
00218 
00219 
00220 
00221 }