//=========================================================================== // GoTools Core - SINTEF Geometry Tools Core library, version 2.0.1 // // Copyright (C) 2000-2007, 2010 SINTEF ICT, Applied Mathematics, Norway. // // 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 version 2 of the License. // // 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. // // Contact information: E-mail: tor.dokken@sintef.no // SINTEF ICT, Department of Applied Mathematics, // P.O. Box 124 Blindern, // 0314 Oslo, Norway. // // Other licenses are also available for this software, notably licenses // for: // - Building commercial software. // - Building software whose source code you wish to keep private. //=========================================================================== 00021 #ifndef M_PI 00022 #define M_PI 3.14159265358979323846 00023 #endif 00024 00025 using namespace std; 00026 using namespace Go; 00027 00028 00029 //=========================================================================== 00030 void 00031 DirectionCone::setFromArray(const double* start, const double* end, int dim) 00032 //=========================================================================== 00033 { 00034 ASSERT(end-start >= dim); 00035 ASSERT((end-start) % dim == 0); 00036 00037 Point current(start, start + dim); 00038 const double* it = start; 00039 while (current.length() == 0.0) { 00040 it += dim; 00041 if (it == end) { 00042 MESSAGE("Setting a DirectionCone from an array of zero vectors"); 00043 centre_ = current; 00044 angle_ = 0.0; 00045 greater_than_pi_ = 1; 00046 return; 00047 } 00048 current.setValue(it); 00049 } 00050 00051 // if (current.length() == 0.0) { 00052 // THROW("Cannot set cone from array containing zero vectors"); 00053 // return; 00054 // } 00055 00056 centre_ = current; 00057 centre_.normalize(); 00058 angle_ = 0.0; 00059 greater_than_pi_ = 0; 00060 00061 for (const double* it = start + dim; it != end; it += dim) { 00062 current.setValue(it); 00063 if (current.length() > 0.0) { 00064 addUnionWith(current); 00065 if (greater_than_pi_ > 0) 00066 return; 00067 } 00068 } 00069 return; 00070 } 00071 00072 //=========================================================================== 00073 bool DirectionCone::overlaps(const DirectionCone& cone) const 00074 //=========================================================================== 00075 { 00076 if (centre_.length() == 0.0 || cone.centre_.length() == 0.0) { 00077 return false; // Degenerate cone 00078 } 00079 if (greater_than_pi_ || cone.greater_than_pi_) { 00080 return true; // all directions covered 00081 } 00082 00083 double ang = centre_.angle(cone.centre_); 00084 00085 if (ang + angle_ + cone.angle_ < M_PI && angle_ + cone.angle_ < ang) { 00086 return false; 00087 } 00088 return true; 00089 } 00090 00091 00092 //=========================================================================== 00093 bool DirectionCone::perpendicularOverlaps(const DirectionCone& cone) const 00094 //=========================================================================== 00095 { 00096 const double pihalf = 0.5*M_PI; 00097 if (centre_.length() == 0.0 || cone.centre_.length() == 0.0) { 00098 return false; // Degenerate cone 00099 } 00100 00101 if (greater_than_pi_ || cone.greater_than_pi_) { 00102 return true; 00103 } 00104 00105 double ang = centre_.angle(cone.centre_); 00106 if (ang + angle_ < pihalf - cone.angle_ || 00107 ang - pihalf - angle_ > cone.angle_) { 00108 return false; 00109 } 00110 return true; 00111 } 00112 00113 00114 //=========================================================================== 00115 bool DirectionCone::containsDirection(const Point& pt, double tol) const 00116 //=========================================================================== 00117 { 00118 ALWAYS_ERROR_IF(greater_than_pi_ < 0, "Not initialized - cannot call."); 00119 if (greater_than_pi_) { 00120 return true; // surely covered 00121 } 00122 00123 double ang = (centre_ * pt) / pt.length(); 00124 ang = acos(ang); 00125 if (ang > angle_ + tol) 00126 return false; 00127 return true; 00128 } 00129 00130 00131 //=========================================================================== 00132 void DirectionCone::addUnionWith(const Point& pt) 00133 //=========================================================================== 00134 { 00135 ALWAYS_ERROR_IF(greater_than_pi_ < 0, 00136 "Must initialize DirectionCone"); 00137 ALWAYS_ERROR_IF(centre_.dimension() != pt.dimension(), 00138 "Dimension mismatch"); 00139 00140 Point t = pt; 00141 double tlength = t.length(); 00142 if (tlength == 0.0) { 00143 MESSAGE("Ignoring vector of zero length"); 00144 return; 00145 } 00146 t /= tlength; 00147 double theta = centre_ * t; 00148 if (theta < -1.0) // This test and the following is needed for 00149 theta = -1.0; // some reason... @jbt 00150 if (theta > 1.0) 00151 theta = 1.0; 00152 theta = acos(theta); 00153 if (theta <= angle_) 00154 return; 00155 if (theta + angle_ >= M_PI + zero_tol_) { 00156 greater_than_pi_ = 1; 00157 return; 00158 } 00159 00160 // New code 00161 double t1 = sin(0.5 * (theta + angle_)); 00162 double t2 = sin(0.5 * (theta - angle_)); 00163 centre_ = centre_ * t1 + t * t2; 00164 if (centre_.length() == 0.0) { 00165 // The cone spans 180 degrees. 00166 // Happens for instance if first 2 vectors point in opposite directions. 00167 greater_than_pi_ = 1; 00168 return; 00169 // THROW("Can't normalize zero vector!"); 00170 } else { 00171 centre_.normalize(); 00172 } 00173 00174 angle_ = 0.5 * (theta + angle_); 00175 check_angle(); 00176 00177 return; 00178 } 00179 00180 00181 //=========================================================================== 00182 void DirectionCone::addUnionWith(const DirectionCone& cone) 00183 //=========================================================================== 00184 { 00185 ALWAYS_ERROR_IF(greater_than_pi_ < 0 || cone.greater_than_pi_ < 0, 00186 "Must initialize DirectionCones."); 00187 ALWAYS_ERROR_IF(centre_.dimension() != cone.centre_.dimension(), 00188 "Dimension mismatch."); 00189 00190 if (cone.greater_than_pi_) { 00191 greater_than_pi_ = 1; 00192 return; 00193 } 00194 double theta = centre_ * cone.centre_; 00195 theta = acos(theta); 00196 if (theta + angle_ + cone.angle_ >= M_PI + zero_tol_) { 00197 greater_than_pi_ = 1; 00198 return; 00199 } else if (theta + cone.angle_ > angle_) { 00200 double t1 = sin(0.5 * (theta + angle_ - cone.angle_)); 00201 double t2 = sin(0.5 * (theta - angle_ + cone.angle_)); 00202 centre_ = centre_ * t1 + cone.centre_ * t2; 00203 centre_.normalize(); 00204 00205 angle_ = 0.5 * (theta + angle_); 00206 } 00207 check_angle(); 00208 return; 00209 } 00210 00211 00212 //=========================================================================== 00213 void DirectionCone::check_angle() const 00214 //=========================================================================== 00215 { 00216 ALWAYS_ERROR_IF(angle_ < 0.0, "Check failed -- negative angle."); 00217 00218 if (angle_ <= M_PI + zero_tol_) 00219 greater_than_pi_ = 0; 00220 else 00221 greater_than_pi_ = 1; 00222 00223 return; 00224 } 00225 00226 00227 //=========================================================================== 00228 void DirectionCone::read(std::istream& is) 00229 //=========================================================================== 00230 { 00231 ALWAYS_ERROR_IF(centre_.dimension() == 0, 00232 "DirectionCone has no set dimension yet - cannot read."); 00233 is >> centre_ >> angle_; 00234 check_angle(); 00235 return; 00236 } 00237 00238 00239 //=========================================================================== 00240 void DirectionCone::write(std::ostream& os) const 00241 //=========================================================================== 00242 { 00243 ALWAYS_ERROR_IF(greater_than_pi_ < 0, 00244 "Not initialized - cannot write."); 00245 00246 os << centre_ << endl << angle_; 00247 return; 00248 }
Generated on Tue Sep 21 15:44:24 2010 for GoTools Core by  doxygen 1.6.3