|
N-sim
Emulation and simulation of Wireless Sensor Networks |
|
|
|
Hosted by |
/home/brennan/n-sim/OrbisQuartus/server/ellipsoid.c
Go to the documentation of this file.00001 00014 /* 00015 * Copyright 2007. Los Alamos National Security, LLC. This material 00016 * was produced under U.S. Government contract DE-AC52-06NA25396 for 00017 * Los Alamos National Laboratory (LANL), which is operated by Los 00018 * Alamos National Security, LLC, for the Department of Energy. The 00019 * U.S. Government has rights to use, reproduce, and distribute this 00020 * software. NEITHER THE GOVERNMENT NOR LOS ALAMOS NATIONAL SECURITY, 00021 * LLC, MAKES ANY WARRANTY, EXPRESS OR IMPLIED, OR ASSUMES ANY LEGAL 00022 * LIABILITY FOR THE USE OF THIS SOFTWARE. If software is modified to 00023 * produce derivative works, such modified software should be clearly 00024 * marked, so as not to confuse it with the version available from LANL. 00025 * 00026 * Additionally, this program is free software; you can redistribute 00027 * it and/or modify it under the terms of the GNU General Public 00028 * License as published by the Free Software Foundation; version 2 of 00029 * the License. Accordingly, this program is distributed in the hope 00030 * it will be useful, but WITHOUT ANY WARRANTY; without even the 00031 * implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR 00032 * PURPOSE. See the GNU General Public License for more details. 00033 */ 00034 00035 00036 #include <math.h> 00037 #define __ellipse_implementation__ 00038 #include "ellipsoid.h" 00039 00040 00041 int dEllipsoidClass = -1; 00042 00043 00044 dColliderFn *dGetEllipsoidColliderFnFn(int num); 00045 void dCalcEllipsoidAABB(dGeomID g, dReal aabb[6]); 00046 00047 /* collision functions */ 00048 int dCollideEllipsoidEllipsoid(dGeomID o1, dGeomID o2, int flags, 00049 dContactGeom * contact, int skip); 00050 int dCollideEllipsoidSphere(dGeomID o1, dGeomID o2, int flags, 00051 dContactGeom * contact, int skip); 00052 int dCollideEllipsoidBox(dGeomID o1, dGeomID o2, int flags, 00053 dContactGeom * contact, int skip); 00054 int dCollideEllipsoidCCylinder(dGeomID o1, dGeomID o2, int flags, 00055 dContactGeom * contact, int skip); 00056 int dCollideEllipsoidPlane(dGeomID o1, dGeomID o2, int flags, 00057 dContactGeom * contact, int skip); 00058 int dCollideEllipsoidRay(dGeomID o1, dGeomID o2, int flags, 00059 dContactGeom * contact, int skip); 00060 00061 00062 00063 00064 00065 dGeomID dCreateEllipsoid(dSpaceID space, dReal major, dReal minor) 00066 { 00067 if (dEllipsoidClass == -1) { // register class once 00068 dGeomClass c; 00069 c.bytes = sizeof(dxEllipsoid); 00070 c.collider = &dGetEllipsoidColliderFnFn; 00071 c.aabb = &dCalcEllipsoidAABB; 00072 c.aabb_test = 0; // no aabb tester function (dAABBTestFn) 00073 c.dtor = 0; // no destructor (dGeomDtorFn) 00074 dEllipsoidClass = dCreateGeomClass(&c); 00075 } 00076 dGeomID g = dCreateGeom(dEllipsoidClass); 00077 if (space != 0) 00078 dSpaceAdd(space, g); 00079 dxEllipsoid *e = (dxEllipsoid*)dGeomGetClassData(g); 00080 e->semimajor_axis = major; 00081 e->semiminor_axis = minor; 00082 00083 return g; 00084 } 00085 00086 00087 void dCalcEllipsoidAABB(dGeomID g, dReal aabb[6]) 00088 { 00089 dUASSERT(g && dGeomGetClass(g) == dEllipsoidClass, 00090 "argument not an ellipsoid"); 00091 dxEllipsoid *e = (dxEllipsoid*)dGeomGetClassData(g); 00092 const dReal *pos = dGeomGetPosition(g); 00093 const dReal *rot = dGeomGetRotation(g); 00094 00095 dReal major_range = dFabs(rot[2] * e->semimajor_axis); 00096 dReal minor_range = dFabs(rot[6] * e->semiminor_axis); 00097 00098 aabb[0] = pos[0] - major_range; 00099 aabb[1] = pos[0] + major_range; 00100 aabb[2] = pos[1] - minor_range; 00101 aabb[3] = pos[1] + minor_range; 00102 aabb[4] = pos[2] - minor_range; 00103 aabb[5] = pos[2] + minor_range; 00104 } 00105 00106 00107 void dGeomEllipsoidSetParams(dGeomID g, dReal major, dReal minor) 00108 { 00109 dUASSERT(g && dGeomGetClass(g) == dEllipsoidClass, 00110 "argument not an ellipsoid"); 00111 dAASSERT(major > 0 && minor > 0); 00112 dxEllipsoid *e = (dxEllipsoid*)dGeomGetClassData(g); 00113 00114 e->semimajor_axis = major; 00115 e->semiminor_axis = minor; 00116 dGeomMoved(g); 00117 } 00118 00119 00120 void dGeomEllipsoidGetParams(dGeomID g, dReal* major, dReal* minor) 00121 { 00122 dUASSERT(g && dGeomGetClass(g) == dEllipsoidClass, 00123 "argument not an ellipsoid"); 00124 dxEllipsoid *e = (dxEllipsoid*)dGeomGetClassData(g); 00125 00126 *major = e->semimajor_axis; 00127 *minor = e->semiminor_axis; 00128 } 00129 00130 00131 dReal dGeomEllipsoidPointDepth(dGeomID g, dReal x, dReal y, dReal z) 00132 { 00133 dUASSERT(g && dGeomGetClass(g) == dEllipsoidClass, 00134 "argument not an ellipsoid"); 00135 //dxEllipsoid *e = (dxEllipsoid*)dGeomGetClassData(g); 00137 return 0; 00138 } 00139 00140 00141 00143 int dCollideEllipsoidEllipsoid(dGeomID o1, dGeomID o2, int flags, 00144 dContactGeom * contact, int skip) 00145 { 00146 dIASSERT(skip >= (int) sizeof(dContactGeom)); 00147 dIASSERT(dGeomGetClass(o1) == dEllipsoidClass); 00148 dIASSERT(dGeomGetClass(o2) == dEllipsoidClass); 00149 dxEllipsoid *ellipse1 = (dxEllipsoid *) dGeomGetClassData(o1); 00150 dxEllipsoid *ellipse2 = (dxEllipsoid *) dGeomGetClassData(o2); 00151 00152 contact->g1 = o1; 00153 contact->g2 = o2; 00154 00155 //dReal *pos_o1 = dGeomGetPosition(o1); 00156 //dReal *pos_o2 = dGeomGetPosition(o2); 00157 //dReal *rot_o1 = dGeomGetPosition(o1); 00158 //dReal *rot_o2 = dGeomGetPosition(o2); 00159 00160 dReal depth = -1; 00161 if (depth >= 0) { 00162 return (int)floor(ellipse1->semimajor_axis + 00163 ellipse2->semiminor_axis); 00164 } else 00165 return 0; 00166 } 00167 00168 00169 00170 00171 int dCollideEllipsoidSphere(dGeomID o1, dGeomID o2, int flags, 00172 dContactGeom * contact, int skip) 00173 { 00174 dIASSERT(skip >= (int) sizeof(dContactGeom)); 00175 dIASSERT(dGeomGetClass(o1) == dEllipsoidClass); 00176 dIASSERT(dGeomGetClass(o2) == dSphereClass); 00177 dxEllipsoid *ellipse = (dxEllipsoid *) dGeomGetClassData(o1); 00179 00180 contact->g1 = o1; 00181 contact->g2 = o2; 00182 00183 dReal depth = -1; 00184 if (depth >= 0) { 00185 return (int)floor(ellipse->semimajor_axis + 00186 ellipse->semiminor_axis); 00187 } else 00188 return 0; 00189 } 00190 00191 00192 int dCollideEllipsoidBox(dGeomID o1, dGeomID o2, int flags, 00193 dContactGeom * contact, int skip) 00194 { 00195 dIASSERT(skip >= (int) sizeof(dContactGeom)); 00196 dIASSERT(dGeomGetClass(o1) == dEllipsoidClass); 00197 dIASSERT(dGeomGetClass(o2) == dBoxClass); 00198 dxEllipsoid *ellipse = (dxEllipsoid *) dGeomGetClassData(o1); 00200 00201 contact->g1 = o1; 00202 contact->g2 = o2; 00203 00204 dReal depth = -1; 00205 if (depth >= 0) { 00206 return (int)floor(ellipse->semimajor_axis + 00207 ellipse->semiminor_axis); 00208 } else 00209 return 0; 00210 } 00211 00212 00213 int dCollideEllipsoidCCylinder(dGeomID o1, dGeomID o2, int flags, 00214 dContactGeom * contact, int skip) 00215 { 00216 dIASSERT(skip >= (int) sizeof(dContactGeom)); 00217 dIASSERT(dGeomGetClass(o1) == dEllipsoidClass); 00218 dIASSERT(dGeomGetClass(o2) == dCCylinderClass); 00219 dxEllipsoid *ellipse = (dxEllipsoid *) dGeomGetClassData(o1); 00221 00222 contact->g1 = o1; 00223 contact->g2 = o2; 00224 00225 dReal depth = -1; 00226 if (depth >= 0) { 00227 return (int)floor(ellipse->semimajor_axis + 00228 ellipse->semiminor_axis); 00229 } else 00230 return 0; 00231 } 00232 00233 00234 int dCollideEllipsoidPlane(dGeomID o1, dGeomID o2, int flags, 00235 dContactGeom * contact, int skip) 00236 { 00237 dIASSERT(skip >= (int) sizeof(dContactGeom)); 00238 dIASSERT(dGeomGetClass(o1) == dEllipsoidClass); 00239 dIASSERT(dGeomGetClass(o2) == dPlaneClass); 00240 dxEllipsoid *ellipse = (dxEllipsoid *) dGeomGetClassData(o1); 00242 00243 contact->g1 = o1; 00244 contact->g2 = o2; 00245 00246 dReal depth = -1; 00247 if (depth >= 0) { 00248 return (int)floor(ellipse->semimajor_axis + 00249 ellipse->semiminor_axis); 00250 } else 00251 return 0; 00252 } 00253 00254 00255 int dCollideEllipsoidRay(dGeomID o1, dGeomID o2, int flags, 00256 dContactGeom * contact, int skip) 00257 { 00258 dIASSERT(skip >= (int) sizeof(dContactGeom)); 00259 dIASSERT(dGeomGetClass(o1) == dEllipsoidClass); 00260 dIASSERT(dGeomGetClass(o2) == dRayClass); 00261 dxEllipsoid *ellipse = (dxEllipsoid *) dGeomGetClassData(o1); 00263 00264 contact->g1 = o1; 00265 contact->g2 = o2; 00266 00267 dReal depth = -1; 00268 if (depth >= 0) { 00269 return (int)floor(ellipse->semimajor_axis + 00270 ellipse->semiminor_axis); 00271 } else 00272 return 0; 00273 } 00274 00275 00276 dColliderFn *dGetEllipsoidColliderFnFn(int num) 00277 { 00278 if (num == dSphereClass) 00279 return *dCollideEllipsoidSphere; 00280 else if (num == dBoxClass) 00281 return *dCollideEllipsoidBox; 00282 else if (num == dCCylinderClass) 00283 return *dCollideEllipsoidCCylinder; 00284 else if (num == dPlaneClass) 00285 return *dCollideEllipsoidPlane; 00286 else if (num == dRayClass) 00287 return *dCollideEllipsoidRay; 00288 else if (num == dEllipsoidClass) 00289 return *dCollideEllipsoidEllipsoid; 00290 return NULL; 00291 }