interval.cpp
Go to the documentation of this file.
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.h"
00038 #include <iostream>
00039 
00040 namespace fcl
00041 {
00042 
00043 Interval bound(const Interval& i, FCL_REAL v)
00044 {
00045   Interval res = i;
00046   if(v < res.i_[0]) res.i_[0] = v;
00047   if(v > res.i_[1]) res.i_[1] = v;
00048   return res;
00049 }
00050 
00051 Interval bound(const Interval& i, const Interval& other)
00052 {
00053   Interval res = i;
00054   if(other.i_[0] < res.i_[0]) res.i_[0] = other.i_[0];
00055   if(other.i_[1] > res.i_[1]) res.i_[1] = other.i_[1];
00056   return res;
00057 }
00058 
00059 Interval Interval::operator * (const Interval& other) const
00060 {
00061   if(other.i_[0] >= 0)
00062   {
00063     if(i_[0] >= 0) return Interval(i_[0] * other.i_[0], i_[1] * other.i_[1]);
00064     if(i_[1] <= 0) return Interval(i_[0] * other.i_[1], i_[1] * other.i_[0]);
00065     return Interval(i_[0] * other.i_[1], i_[1] * other.i_[1]);
00066   }
00067   if(other.i_[1] <= 0)
00068   {
00069     if(i_[0] >= 0) return Interval(i_[1] * other.i_[0], i_[0] * other.i_[1]);
00070     if(i_[1] <= 0) return Interval(i_[1] * other.i_[1], i_[0] * other.i_[0]);
00071     return Interval(i_[1] * other.i_[0], i_[0] * other.i_[0]);
00072   }
00073 
00074   if(i_[0] >= 0) return Interval(i_[1] * other.i_[0], i_[1] * other.i_[1]);
00075   if(i_[1] <= 0) return Interval(i_[0] * other.i_[1], i_[0] * other.i_[0]);
00076 
00077   FCL_REAL v00 = i_[0] * other.i_[0];
00078   FCL_REAL v11 = i_[1] * other.i_[1];
00079   if(v00 <= v11)
00080   {
00081     FCL_REAL v01 = i_[0] * other.i_[1];
00082     FCL_REAL v10 = i_[1] * other.i_[0];
00083     if(v01 < v10) return Interval(v01, v11);
00084     return Interval(v10, v11);
00085   }
00086 
00087   FCL_REAL v01 = i_[0] * other.i_[1];
00088   FCL_REAL v10 = i_[1] * other.i_[0];
00089   if(v01 < v10) return Interval(v01, v00);
00090   return Interval(v10, v00);
00091 }
00092 
00093 Interval& Interval::operator *= (const Interval& other)
00094 {
00095   if(other.i_[0] >= 0)
00096   {
00097     if(i_[0] >= 0) 
00098     {
00099       i_[0] *= other.i_[0];
00100       i_[1] *= other.i_[1];
00101     }
00102     else if(i_[1] <= 0)
00103     {
00104       i_[0] *= other.i_[1];
00105       i_[1] *= other.i_[0];
00106     }
00107     else
00108     {
00109       i_[0] *= other.i_[1];
00110       i_[1] *= other.i_[1];
00111     }
00112     return *this;
00113   }
00114 
00115   if(other.i_[1] <= 0)
00116   {
00117     if(i_[0] >= 0)
00118     {
00119       FCL_REAL tmp = i_[0];
00120       i_[0] = i_[1] * other.i_[0];
00121       i_[1] = tmp * other.i_[1];
00122     }
00123     else if(i_[1] <= 0)
00124     {
00125       FCL_REAL tmp = i_[0];
00126       i_[0] = i_[1] * other.i_[1];
00127       i_[1] = tmp * other.i_[0];
00128     }
00129     else
00130     {
00131       FCL_REAL tmp = i_[0];
00132       i_[0] = i_[1] * other.i_[0];
00133       i_[1] = tmp * other.i_[0];  
00134     }
00135     return *this;
00136   }
00137 
00138   if(i_[0] >= 0) 
00139   {
00140     i_[0] = i_[1] * other.i_[0];
00141     i_[1] *= other.i_[1];
00142     return *this;
00143   }
00144 
00145   if(i_[1] <= 0) 
00146   {
00147     i_[1] = i_[0] * other.i_[0];
00148     i_[0] *= other.i_[1];
00149     return *this;
00150   }
00151 
00152   FCL_REAL v00 = i_[0] * other.i_[0];
00153   FCL_REAL v11 = i_[1] * other.i_[1];
00154   if(v00 <= v11)
00155   {
00156     FCL_REAL v01 = i_[0] * other.i_[1];
00157     FCL_REAL v10 = i_[1] * other.i_[0];
00158     if(v01 < v10)
00159     {
00160       i_[0] = v01;
00161       i_[1] = v11;
00162     }
00163     else
00164     {
00165       i_[0] = v10;
00166       i_[1] = v11;
00167     }
00168     return *this;
00169   }
00170 
00171   FCL_REAL v01 = i_[0] * other.i_[1];
00172   FCL_REAL v10 = i_[1] * other.i_[0];
00173   if(v01 < v10)
00174   {
00175     i_[0] = v01;
00176     i_[1] = v00;
00177   }
00178   else
00179   {
00180     i_[0] = v10;
00181     i_[1] = v00;
00182   }
00183 
00184   return *this;
00185 }
00186 
00187 Interval Interval::operator / (const Interval& other) const
00188 {
00189   return *this * Interval(1.0 / other.i_[1], 1.0 / other.i_[0]);
00190 }
00191 
00192 Interval& Interval::operator /= (const Interval& other)
00193 {
00194   *this *= Interval(1.0 / other.i_[1], 1.0 / other.i_[0]);
00195   return *this;
00196 }
00197 
00198 void Interval::print() const
00199 {
00200   std::cout << "[" << i_[0] << ", " << i_[1] << "]" << std::endl;
00201 }
00202 
00203 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines


fcl
Author(s): Jia Pan
autogenerated on Tue Jan 15 2013 16:05:30