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.ownership; 34 import dlib.core.memory; 35 import dlib.math.vector; 36 import dlib.math.matrix; 37 import dlib.math.transformation; 38 import dlib.math.utils; 39 import dlib.geometry.aabb; 40 41 import dmech.world; 42 43 enum GeomType 44 { 45 Undefined, 46 Sphere, 47 Box, 48 Cylinder, 49 Cone, 50 Ellipsoid, 51 Triangle, 52 UserDefined 53 } 54 55 abstract class Geometry: Owner 56 { 57 GeomType type = GeomType.Undefined; 58 59 this(Owner o) 60 { 61 super(o); 62 } 63 64 Vector3f supportPoint(Vector3f dir) 65 { 66 return Vector3f(0.0f, 0.0f, 0.0f); 67 } 68 69 Matrix3x3f inertiaTensor(float mass) 70 { 71 return Matrix3x3f.identity * mass; 72 } 73 74 AABB boundingBox(Vector3f position) 75 { 76 return AABB(position, Vector3f(1.0f, 1.0f, 1.0f)); 77 } 78 } 79 80 class GeomSphere: Geometry 81 { 82 float radius; 83 84 this(PhysicsWorld world, float r) 85 { 86 super(world); 87 type = GeomType.Sphere; 88 radius = r; 89 } 90 91 override Vector3f supportPoint(Vector3f dir) 92 { 93 return dir.normalized * radius; 94 } 95 96 override Matrix3x3f inertiaTensor(float mass) 97 { 98 float v = 0.4f * mass * radius * radius; 99 100 return matrixf( 101 v, 0, 0, 102 0, v, 0, 103 0, 0, v 104 ); 105 } 106 107 override AABB boundingBox(Vector3f position) 108 { 109 return AABB(position, Vector3f(radius, radius, radius)); 110 } 111 } 112 113 class GeomBox: Geometry 114 { 115 Vector3f halfSize; 116 float bsphereRadius; 117 118 this(PhysicsWorld world, Vector3f hsize) 119 { 120 super(world); 121 type = GeomType.Box; 122 halfSize = hsize; 123 bsphereRadius = halfSize.length; 124 } 125 126 override Vector3f supportPoint(Vector3f dir) 127 { 128 Vector3f result; 129 result.x = sign(dir.x) * halfSize.x; 130 result.y = sign(dir.y) * halfSize.y; 131 result.z = sign(dir.z) * halfSize.z; 132 return result; 133 } 134 135 override Matrix3x3f inertiaTensor(float mass) 136 { 137 float x2 = halfSize.x * halfSize.x; 138 float y2 = halfSize.y * halfSize.y; 139 float z2 = halfSize.z * halfSize.z; 140 141 return matrixf( 142 (y2 + z2)/3 * mass, 0, 0, 143 0, (x2 + z2)/3 * mass, 0, 144 0, 0, (x2 + y2)/3 * mass 145 ); 146 } 147 148 override AABB boundingBox(Vector3f position) 149 { 150 return AABB(position, 151 Vector3f(bsphereRadius, bsphereRadius, bsphereRadius)); 152 } 153 } 154 155 class GeomCylinder: Geometry 156 { 157 float height; 158 float radius; 159 160 this(PhysicsWorld world, float h, float r) 161 { 162 super(world); 163 type = GeomType.Cylinder; 164 height = h; 165 radius = r; 166 } 167 168 override Vector3f supportPoint(Vector3f dir) 169 { 170 Vector3f result; 171 float sigma = sqrt((dir.x * dir.x + dir.z * dir.z)); 172 173 if (sigma > 0.0f) 174 { 175 result.x = dir.x / sigma * radius; 176 result.y = sign(dir.y) * height * 0.5f; 177 result.z = dir.z / sigma * radius; 178 } 179 else 180 { 181 result.x = 0.0f; 182 result.y = sign(dir.y) * height * 0.5f; 183 result.z = 0.0f; 184 } 185 186 return result; 187 } 188 189 override Matrix3x3f inertiaTensor(float mass) 190 { 191 float r2 = radius * radius; 192 float h2 = height * height; 193 194 return matrixf( 195 (3*r2 + h2)/12 * mass, 0, 0, 196 0, (3*r2 + h2)/12 * mass, 0, 197 0, 0, r2/2 * mass 198 ); 199 } 200 201 override AABB boundingBox(Vector3f position) 202 { 203 float rsum = radius + radius; 204 float d = sqrt(rsum * rsum + height * height) * 0.5f; 205 return AABB(position, Vector3f(d, d, d)); 206 } 207 } 208 209 class GeomCone: Geometry 210 { 211 float radius; 212 float height; 213 214 this(PhysicsWorld world, float h, float r) 215 { 216 super(world); 217 type = GeomType.Cone; 218 height = h; 219 radius = r; 220 } 221 222 override Vector3f supportPoint(Vector3f dir) 223 { 224 float zdist = dir[0] * dir[0] + dir[1] * dir[1]; 225 float len = zdist + dir[2] * dir[2]; 226 zdist = sqrt(zdist); 227 len = sqrt(len); 228 float half_h = height * 0.5; 229 float radius = radius; 230 231 float sin_a = radius / sqrt(radius * radius + 4.0f * half_h * half_h); 232 233 if (dir[2] > len * sin_a) 234 return Vector3f(0.0f, 0.0f, half_h); 235 else if (zdist > 0.0f) 236 { 237 float rad = radius / zdist; 238 return Vector3f(rad * dir[0], rad * dir[1], -half_h); 239 } 240 else 241 return Vector3f(0.0f, 0.0f, -half_h); 242 } 243 244 override Matrix3x3f inertiaTensor(float mass) 245 { 246 float r2 = radius * radius; 247 float h2 = height * height; 248 249 return matrixf( 250 (3.0f/80.0f*h2 + 3.0f/20.0f*r2) * mass, 0, 0, 251 0, (3.0f/80.0f*h2 + 3.0f/20.0f*r2) * mass, 0, 252 0, 0, (3.0f/10.0f*r2) * mass 253 ); 254 } 255 256 override AABB boundingBox(Vector3f position) 257 { 258 float rsum = radius + radius; 259 float d = sqrt(rsum * rsum + height * height) * 0.5f; 260 return AABB(position, Vector3f(d, d, d)); 261 } 262 } 263 264 class GeomEllipsoid: Geometry 265 { 266 Vector3f radii; 267 268 this(PhysicsWorld world, Vector3f r) 269 { 270 super(world); 271 type = GeomType.Ellipsoid; 272 radii = r; 273 } 274 275 override Vector3f supportPoint(Vector3f dir) 276 { 277 return dir.normalized * radii; 278 } 279 280 override Matrix3x3f inertiaTensor(float mass) 281 { 282 float x2 = radii.x * radii.x; 283 float y2 = radii.y * radii.y; 284 float z2 = radii.z * radii.z; 285 286 return matrixf( 287 (y2 + z2)/5 * mass, 0, 0, 288 0, (x2 + z2)/5 * mass, 0, 289 0, 0, (x2 + y2)/5 * mass 290 ); 291 } 292 293 override AABB boundingBox(Vector3f position) 294 { 295 return AABB(position, radii); 296 } 297 } 298 299 class GeomTriangle: Geometry 300 { 301 Vector3f[3] v; 302 303 this(PhysicsWorld world, Vector3f a, Vector3f b, Vector3f c) 304 { 305 super(world); 306 type = GeomType.Triangle; 307 v[0] = a; 308 v[1] = b; 309 v[2] = c; 310 } 311 312 override Vector3f supportPoint(Vector3f dir) 313 { 314 float dota = dir.dot(v[0]); 315 float dotb = dir.dot(v[1]); 316 float dotc = dir.dot(v[2]); 317 318 if (dota > dotb) 319 { 320 if (dotc > dota) 321 return v[2]; 322 else 323 return v[0]; 324 } 325 else 326 { 327 if (dotc > dotb) 328 return v[2]; 329 else 330 return v[1]; 331 } 332 } 333 334 // TODO: boundingBox 335 }