1 /* 2 Copyright (c) 2013-2017 Timur Gafarov 3 4 Boost Software License - Version 1.0 - August 17th, 2003 5 6 Permission is hereby granted, free of charge, to any person or organization 7 obtaining a copy of the software and accompanying documentation covered by 8 this license (the "Software") to use, reproduce, display, distribute, 9 execute, and transmit the Software, and to prepare derivative works of the 10 Software, and to permit third-parties to whom the Software is furnished to 11 do so, all subject to the following: 12 13 The copyright notices in the Software and this entire statement, including 14 the above license grant, this restriction and the following disclaimer, 15 must be included in all copies of the Software, in whole or in part, and 16 all derivative works of the Software, unless such copies or derivative 17 works are solely in the form of machine-executable object code generated by 18 a source language processor. 19 20 THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 21 IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 22 FITNESS FOR A PARTICULAR PURPOSE, TITLE AND NON-INFRINGEMENT. IN NO EVENT 23 SHALL THE COPYRIGHT HOLDERS OR ANYONE DISTRIBUTING THE SOFTWARE BE LIABLE 24 FOR ANY DAMAGES OR OTHER LIABILITY, WHETHER IN CONTRACT, TORT OR OTHERWISE, 25 ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER 26 DEALINGS IN THE SOFTWARE. 27 */ 28 29 module dmech.geometry; 30 31 import std.math; 32 33 import dlib.core.memory; 34 import dlib.math.vector; 35 import dlib.math.matrix; 36 import dlib.math.transformation; 37 import dlib.math.utils; 38 import dlib.geometry.aabb; 39 40 enum GeomType 41 { 42 Undefined, 43 Sphere, 44 Box, 45 Cylinder, 46 Cone, 47 Ellipsoid, 48 Triangle, 49 UserDefined 50 } 51 52 abstract class Geometry: Freeable 53 { 54 GeomType type = GeomType.Undefined; 55 56 this() 57 { 58 } 59 60 Vector3f supportPoint(Vector3f dir) 61 { 62 return Vector3f(0.0f, 0.0f, 0.0f); 63 } 64 65 Matrix3x3f inertiaTensor(float mass) 66 { 67 return Matrix3x3f.identity * mass; 68 } 69 70 AABB boundingBox(Vector3f position) 71 { 72 return AABB(position, Vector3f(1.0f, 1.0f, 1.0f)); 73 } 74 } 75 76 class GeomSphere: Geometry 77 { 78 float radius; 79 80 this(float r) 81 { 82 super(); 83 type = GeomType.Sphere; 84 radius = r; 85 } 86 87 override Vector3f supportPoint(Vector3f dir) 88 { 89 return dir.normalized * radius; 90 } 91 92 override Matrix3x3f inertiaTensor(float mass) 93 { 94 float v = 0.4f * mass * radius * radius; 95 96 return matrixf( 97 v, 0, 0, 98 0, v, 0, 99 0, 0, v 100 ); 101 } 102 103 override AABB boundingBox(Vector3f position) 104 { 105 return AABB(position, Vector3f(radius, radius, radius)); 106 } 107 108 void free() 109 { 110 Delete(this); 111 } 112 } 113 114 class GeomBox: Geometry 115 { 116 Vector3f halfSize; 117 float bsphereRadius; 118 119 this(Vector3f hsize) 120 { 121 super(); 122 type = GeomType.Box; 123 halfSize = hsize; 124 bsphereRadius = halfSize.length; 125 } 126 127 override Vector3f supportPoint(Vector3f dir) 128 { 129 Vector3f result; 130 result.x = sign(dir.x) * halfSize.x; 131 result.y = sign(dir.y) * halfSize.y; 132 result.z = sign(dir.z) * halfSize.z; 133 return result; 134 } 135 136 override Matrix3x3f inertiaTensor(float mass) 137 { 138 float x2 = halfSize.x * halfSize.x; 139 float y2 = halfSize.y * halfSize.y; 140 float z2 = halfSize.z * halfSize.z; 141 142 return matrixf( 143 (y2 + z2)/3 * mass, 0, 0, 144 0, (x2 + z2)/3 * mass, 0, 145 0, 0, (x2 + y2)/3 * mass 146 ); 147 } 148 149 override AABB boundingBox(Vector3f position) 150 { 151 return AABB(position, 152 Vector3f(bsphereRadius, bsphereRadius, bsphereRadius)); 153 } 154 155 void free() 156 { 157 Delete(this); 158 } 159 } 160 161 class GeomCylinder: Geometry 162 { 163 float height; 164 float radius; 165 166 this(float h, float r) 167 { 168 super(); 169 type = GeomType.Cylinder; 170 height = h; 171 radius = r; 172 } 173 174 override Vector3f supportPoint(Vector3f dir) 175 { 176 Vector3f result; 177 float sigma = sqrt((dir.x * dir.x + dir.z * dir.z)); 178 179 if (sigma > 0.0f) 180 { 181 result.x = dir.x / sigma * radius; 182 result.y = sign(dir.y) * height * 0.5f; 183 result.z = dir.z / sigma * radius; 184 } 185 else 186 { 187 result.x = 0.0f; 188 result.y = sign(dir.y) * height * 0.5f; 189 result.z = 0.0f; 190 } 191 192 return result; 193 } 194 195 override Matrix3x3f inertiaTensor(float mass) 196 { 197 float r2 = radius * radius; 198 float h2 = height * height; 199 200 return matrixf( 201 (3*r2 + h2)/12 * mass, 0, 0, 202 0, (3*r2 + h2)/12 * mass, 0, 203 0, 0, r2/2 * mass 204 ); 205 } 206 207 override AABB boundingBox(Vector3f position) 208 { 209 float rsum = radius + radius; 210 float d = sqrt(rsum * rsum + height * height) * 0.5f; 211 return AABB(position, Vector3f(d, d, d)); 212 } 213 214 void free() 215 { 216 Delete(this); 217 } 218 } 219 220 class GeomCone: Geometry 221 { 222 float radius; 223 float height; 224 225 this(float h, float r) 226 { 227 super(); 228 type = GeomType.Cone; 229 height = h; 230 radius = r; 231 } 232 233 override Vector3f supportPoint(Vector3f dir) 234 { 235 float zdist = dir[0] * dir[0] + dir[1] * dir[1]; 236 float len = zdist + dir[2] * dir[2]; 237 zdist = sqrt(zdist); 238 len = sqrt(len); 239 float half_h = height * 0.5; 240 float radius = radius; 241 242 float sin_a = radius / sqrt(radius * radius + 4.0f * half_h * half_h); 243 244 if (dir[2] > len * sin_a) 245 return Vector3f(0.0f, 0.0f, half_h); 246 else if (zdist > 0.0f) 247 { 248 float rad = radius / zdist; 249 return Vector3f(rad * dir[0], rad * dir[1], -half_h); 250 } 251 else 252 return Vector3f(0.0f, 0.0f, -half_h); 253 } 254 255 override Matrix3x3f inertiaTensor(float mass) 256 { 257 float r2 = radius * radius; 258 float h2 = height * height; 259 260 return matrixf( 261 (3.0f/80.0f*h2 + 3.0f/20.0f*r2) * mass, 0, 0, 262 0, (3.0f/80.0f*h2 + 3.0f/20.0f*r2) * mass, 0, 263 0, 0, (3.0f/10.0f*r2) * mass 264 ); 265 } 266 267 override AABB boundingBox(Vector3f position) 268 { 269 float rsum = radius + radius; 270 float d = sqrt(rsum * rsum + height * height) * 0.5f; 271 return AABB(position, Vector3f(d, d, d)); 272 } 273 274 void free() 275 { 276 Delete(this); 277 } 278 } 279 280 class GeomEllipsoid: Geometry 281 { 282 Vector3f radii; 283 284 this(Vector3f r) 285 { 286 super(); 287 type = GeomType.Ellipsoid; 288 radii = r; 289 } 290 291 override Vector3f supportPoint(Vector3f dir) 292 { 293 return dir.normalized * radii; 294 } 295 296 override Matrix3x3f inertiaTensor(float mass) 297 { 298 float x2 = radii.x * radii.x; 299 float y2 = radii.y * radii.y; 300 float z2 = radii.z * radii.z; 301 302 return matrixf( 303 (y2 + z2)/5 * mass, 0, 0, 304 0, (x2 + z2)/5 * mass, 0, 305 0, 0, (x2 + y2)/5 * mass 306 ); 307 } 308 309 override AABB boundingBox(Vector3f position) 310 { 311 return AABB(position, radii); 312 } 313 314 void free() 315 { 316 Delete(this); 317 } 318 } 319 320 class GeomTriangle: Geometry 321 { 322 Vector3f[3] v; 323 324 this(Vector3f a, Vector3f b, Vector3f c) 325 { 326 super(); 327 type = GeomType.Triangle; 328 v[0] = a; 329 v[1] = b; 330 v[2] = c; 331 } 332 333 override Vector3f supportPoint(Vector3f dir) 334 { 335 float dota = dir.dot(v[0]); 336 float dotb = dir.dot(v[1]); 337 float dotc = dir.dot(v[2]); 338 339 if (dota > dotb) 340 { 341 if (dotc > dota) 342 return v[2]; 343 else 344 return v[0]; 345 } 346 else 347 { 348 if (dotc > dotb) 349 return v[2]; 350 else 351 return v[1]; 352 } 353 } 354 355 // TODO: boundingBox 356 357 void free() 358 { 359 Delete(this); 360 } 361 }