Panda3D
|
00001 // Filename: boundingLine.cxx 00002 // Created by: drose (04Jul00) 00003 // 00004 //////////////////////////////////////////////////////////////////// 00005 // 00006 // PANDA 3D SOFTWARE 00007 // Copyright (c) Carnegie Mellon University. All rights reserved. 00008 // 00009 // All use of this software is subject to the terms of the revised BSD 00010 // license. You should have received a copy of this license along 00011 // with this source code in a file named "LICENSE." 00012 // 00013 //////////////////////////////////////////////////////////////////// 00014 00015 #include "boundingLine.h" 00016 #include "boundingSphere.h" 00017 #include "config_mathutil.h" 00018 00019 #include <math.h> 00020 00021 TypeHandle BoundingLine::_type_handle; 00022 00023 //////////////////////////////////////////////////////////////////// 00024 // Function: BoundingLine::make_copy 00025 // Access: Public, Virtual 00026 // Description: 00027 //////////////////////////////////////////////////////////////////// 00028 BoundingVolume *BoundingLine:: 00029 make_copy() const { 00030 return new BoundingLine(*this); 00031 } 00032 00033 //////////////////////////////////////////////////////////////////// 00034 // Function: BoundingLine::get_approx_center 00035 // Access: Public, Virtual 00036 // Description: 00037 //////////////////////////////////////////////////////////////////// 00038 LPoint3 BoundingLine:: 00039 get_approx_center() const { 00040 nassertr(!is_empty(), LPoint3(0.0f, 0.0f, 0.0f)); 00041 nassertr(!is_infinite(), LPoint3(0.0f, 0.0f, 0.0f)); 00042 return (get_point_a() + get_point_b()) / 2.0; 00043 } 00044 00045 //////////////////////////////////////////////////////////////////// 00046 // Function: BoundingLine::xform 00047 // Access: Public, Virtual 00048 // Description: 00049 //////////////////////////////////////////////////////////////////// 00050 void BoundingLine:: 00051 xform(const LMatrix4 &mat) { 00052 nassertv(!mat.is_nan()); 00053 00054 if (!is_empty() && !is_infinite()) { 00055 _origin = _origin * mat; 00056 _vector = _vector * mat; 00057 if (!_vector.normalize()) { 00058 // If we just scaled the line down to nothing, it becomes an 00059 // empty volume. 00060 _flags |= F_empty; 00061 } 00062 } 00063 } 00064 00065 //////////////////////////////////////////////////////////////////// 00066 // Function: BoundingLine::output 00067 // Access: Public, Virtual 00068 // Description: 00069 //////////////////////////////////////////////////////////////////// 00070 void BoundingLine:: 00071 output(ostream &out) const { 00072 if (is_empty()) { 00073 out << "bline, empty"; 00074 } else if (is_infinite()) { 00075 out << "bline, infinite"; 00076 } else { 00077 out << "bline, (" << _origin << ") - (" << _origin + _vector << ")"; 00078 } 00079 } 00080 00081 //////////////////////////////////////////////////////////////////// 00082 // Function: BoundingLine::as_bounding_line 00083 // Access: Public, Virtual 00084 // Description: Virtual downcast method. Returns this object as a 00085 // pointer of the indicated type, if it is in fact that 00086 // type. Returns NULL if it is not that type. 00087 //////////////////////////////////////////////////////////////////// 00088 const BoundingLine *BoundingLine:: 00089 as_bounding_line() const { 00090 return this; 00091 } 00092 00093 //////////////////////////////////////////////////////////////////// 00094 // Function: BoundingLine::extend_other 00095 // Access: Protected, Virtual 00096 // Description: 00097 //////////////////////////////////////////////////////////////////// 00098 bool BoundingLine:: 00099 extend_other(BoundingVolume *other) const { 00100 return other->extend_by_line(this); 00101 } 00102 00103 //////////////////////////////////////////////////////////////////// 00104 // Function: BoundingLine::around_other 00105 // Access: Protected, Virtual 00106 // Description: 00107 //////////////////////////////////////////////////////////////////// 00108 bool BoundingLine:: 00109 around_other(BoundingVolume *other, 00110 const BoundingVolume **first, 00111 const BoundingVolume **last) const { 00112 return other->around_lines(first, last); 00113 } 00114 00115 //////////////////////////////////////////////////////////////////// 00116 // Function: BoundingLine::contains_other 00117 // Access: Protected, Virtual 00118 // Description: 00119 //////////////////////////////////////////////////////////////////// 00120 int BoundingLine:: 00121 contains_other(const BoundingVolume *other) const { 00122 return other->contains_line(this); 00123 } 00124 00125 //////////////////////////////////////////////////////////////////// 00126 // Function: BoundingLine::extend_by_line 00127 // Access: Protected, Virtual 00128 // Description: 00129 //////////////////////////////////////////////////////////////////// 00130 bool BoundingLine:: 00131 extend_by_line(const BoundingLine *line) { 00132 nassertr(!line->is_empty() && !line->is_infinite(), false); 00133 nassertr(!is_infinite(), false); 00134 00135 if (is_empty()) { 00136 _origin = line->_origin; 00137 _vector = line->_vector; 00138 _flags = 0; 00139 } else { 00140 _flags = F_infinite; 00141 } 00142 return true; 00143 } 00144 00145 //////////////////////////////////////////////////////////////////// 00146 // Function: BoundingLine::contains_sphere 00147 // Access: Protected, Virtual 00148 // Description: 00149 //////////////////////////////////////////////////////////////////// 00150 int BoundingLine:: 00151 contains_sphere(const BoundingSphere *sphere) const { 00152 nassertr(!is_empty() && !is_infinite(), 0); 00153 nassertr(!sphere->is_empty() && !sphere->is_infinite(), 0); 00154 00155 PN_stdfloat r = sphere->get_radius(); 00156 00157 if (r * r >= sqr_dist_to_line(sphere->get_center())) { 00158 return IF_possible | IF_some; 00159 } else { 00160 return IF_no_intersection; 00161 } 00162 } 00163 00164 //////////////////////////////////////////////////////////////////// 00165 // Function: BoundingLine::contains_box 00166 // Access: Protected, Virtual 00167 // Description: 00168 //////////////////////////////////////////////////////////////////// 00169 int BoundingLine:: 00170 contains_box(const BoundingBox *box) const { 00171 nassertr(!is_empty() && !is_infinite(), 0); 00172 nassertr(!box->is_empty() && !box->is_infinite(), 0); 00173 00174 LPoint3 center = (box->get_minq() + box->get_maxq()) * 0.5f; 00175 PN_stdfloat r2 = (box->get_maxq() - box->get_minq()).length_squared() * 0.25f; 00176 00177 if (r2 >= sqr_dist_to_line(center)) { 00178 return IF_possible; 00179 } else { 00180 return IF_no_intersection; 00181 } 00182 } 00183 00184 //////////////////////////////////////////////////////////////////// 00185 // Function: BoundingLine::sqr_dist_to_line 00186 // Access: Protected 00187 // Description: 00188 //////////////////////////////////////////////////////////////////// 00189 PN_stdfloat BoundingLine:: 00190 sqr_dist_to_line(const LPoint3 &point) const { 00191 nassertr(!point.is_nan(), 0.0f); 00192 nassertr(!is_empty() && !is_infinite(), 0.0f); 00193 nassertr(!_vector.almost_equal(LVector3(0.0f, 0.0f, 0.0f)), 0.0f); 00194 00195 // The formula for the distance from a point to the line based on 00196 // the quadratic equation. 00197 00198 PN_stdfloat A = dot(_vector, _vector); 00199 nassertr(A != 0.0f, 0.0f); 00200 LVector3 fc = _origin - point; 00201 PN_stdfloat B = 2.0 * dot(_vector, fc); 00202 PN_stdfloat fc_d2 = dot(fc, fc); 00203 00204 PN_stdfloat r2 = fc_d2 - B*B / 4.0*A; 00205 00206 nassertr(!cnan(r2), 0.0f); 00207 return r2; 00208 }