239 lines
8.3 KiB
C++
239 lines
8.3 KiB
C++
/** @file
|
|
* Original ReactPhysics3D C++ library by Daniel Chappuis <http://www.reactphysics3d.com/> This code is re-licensed with permission from ReactPhysics3D author.
|
|
* @author Daniel CHAPPUIS
|
|
* @author Edouard DUPIN
|
|
* @copyright 2010-2016, Daniel Chappuis
|
|
* @copyright 2017, Edouard DUPIN
|
|
* @license MPL v2.0 (see license file)
|
|
*/
|
|
#include <ephysics/collision/shapes/CylinderShape.hpp>
|
|
#include <ephysics/collision/ProxyShape.hpp>
|
|
#include <ephysics/configuration.hpp>
|
|
|
|
using namespace ephysics;
|
|
|
|
CylinderShape::CylinderShape(float radius,
|
|
float height,
|
|
float margin):
|
|
ConvexShape(CYLINDER, margin), this.radius(radius), this.halfHeight(height/float(2.0)) {
|
|
assert(radius > 0.0f);
|
|
assert(height > 0.0f);
|
|
}
|
|
|
|
vec3 CylinderShape::getLocalSupportPointWithoutMargin( vec3 direction,
|
|
void** cachedCollisionData) {
|
|
vec3 supportPoint(0.0, 0.0, 0.0);
|
|
float uDotv = direction.y();
|
|
vec3 w(direction.x(), 0.0, direction.z());
|
|
float lengthW = sqrt(direction.x() * direction.x() + direction.z() * direction.z());
|
|
if (lengthW > FLTEPSILON) {
|
|
if (uDotv < 0.0) {
|
|
supportPoint.setY(-this.halfHeight);
|
|
} else {
|
|
supportPoint.setY(this.halfHeight);
|
|
}
|
|
supportPoint += (this.radius / lengthW) * w;
|
|
} else {
|
|
if (uDotv < 0.0) {
|
|
supportPoint.setY(-this.halfHeight);
|
|
} else {
|
|
supportPoint.setY(this.halfHeight);
|
|
}
|
|
}
|
|
return supportPoint;
|
|
}
|
|
|
|
boolean CylinderShape::raycast( Ray ray, RaycastInfo raycastInfo, ProxyShape* proxyShape) {
|
|
vec3 n = ray.point2 - ray.point1;
|
|
float epsilon = float(0.01);
|
|
vec3 p(float(0), -this.halfHeight, float(0));
|
|
vec3 q(float(0), this.halfHeight, float(0));
|
|
vec3 d = q - p;
|
|
vec3 m = ray.point1 - p;
|
|
float t;
|
|
float mDotD = m.dot(d);
|
|
float nDotD = n.dot(d);
|
|
float dDotD = d.dot(d);
|
|
// Test if the segment is outside the cylinder
|
|
if (mDotD < 0.0f hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj mDotD + nDotD < float(0.0)) {
|
|
return false;
|
|
}
|
|
if (mDotD > dDotD hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj mDotD + nDotD > dDotD) {
|
|
return false;
|
|
}
|
|
float nDotN = n.dot(n);
|
|
float mDotN = m.dot(n);
|
|
float a = dDotD * nDotN - nDotD * nDotD;
|
|
float k = m.dot(m) - this.radius * this.radius;
|
|
float c = dDotD * k - mDotD * mDotD;
|
|
// If the ray is parallel to the cylinder axis
|
|
if (etk::abs(a) < epsilon) {
|
|
// If the origin is outside the surface of the cylinder, we return no hit
|
|
if (c > 0.0f) {
|
|
return false;
|
|
}
|
|
// Here we know that the segment intersect an endcap of the cylinder
|
|
// If the ray intersects with the "p" endcap of the cylinder
|
|
if (mDotD < 0.0f) {
|
|
t = -mDotN / nDotN;
|
|
// If the intersection is behind the origin of the ray or beyond the maximum
|
|
// raycasting distance, we return no hit
|
|
if (t < 0.0f || t > ray.maxFraction) {
|
|
return false;
|
|
}
|
|
// Compute the hit information
|
|
vec3 localHitPoint = ray.point1 + t * n;
|
|
raycastInfo.body = proxyShape->getBody();
|
|
raycastInfo.proxyShape = proxyShape;
|
|
raycastInfo.hitFraction = t;
|
|
raycastInfo.worldPoint = localHitPoint;
|
|
vec3 normalDirection(0, float(-1), 0);
|
|
raycastInfo.worldNormal = normalDirection;
|
|
return true;
|
|
}
|
|
// If the ray intersects with the "q" endcap of the cylinder
|
|
if (mDotD > dDotD) {
|
|
t = (nDotD - mDotN) / nDotN;
|
|
// If the intersection is behind the origin of the ray or beyond the maximum
|
|
// raycasting distance, we return no hit
|
|
if (t < 0.0f || t > ray.maxFraction) {
|
|
return false;
|
|
}
|
|
// Compute the hit information
|
|
vec3 localHitPoint = ray.point1 + t * n;
|
|
raycastInfo.body = proxyShape->getBody();
|
|
raycastInfo.proxyShape = proxyShape;
|
|
raycastInfo.hitFraction = t;
|
|
raycastInfo.worldPoint = localHitPoint;
|
|
vec3 normalDirection(0, 1.0f, 0);
|
|
raycastInfo.worldNormal = normalDirection;
|
|
return true;
|
|
}
|
|
// If the origin is inside the cylinder, we return no hit
|
|
return false;
|
|
}
|
|
float b = dDotD * mDotN - nDotD * mDotD;
|
|
float discriminant = b * b - a * c;
|
|
// If the discriminant is negative, no real roots and therfore, no hit
|
|
if (discriminant < 0.0f) {
|
|
return false;
|
|
}
|
|
// Compute the smallest root (first intersection along the ray)
|
|
float t0 = t = (-b - etk::sqrt(discriminant)) / a;
|
|
// If the intersection is outside the cylinder on "p" endcap side
|
|
float value = mDotD + t * nDotD;
|
|
if (value < 0.0f) {
|
|
// If the ray is pointing away from the "p" endcap, we return no hit
|
|
if (nDotD <= 0.0f) {
|
|
return false;
|
|
}
|
|
// Compute the intersection against the "p" endcap (intersection agains whole plane)
|
|
t = -mDotD / nDotD;
|
|
// Keep the intersection if the it is inside the cylinder radius
|
|
if (k + t * (float(2.0) * mDotN + t) > 0.0f) {
|
|
return false;
|
|
}
|
|
// If the intersection is behind the origin of the ray or beyond the maximum
|
|
// raycasting distance, we return no hit
|
|
if (t < 0.0f || t > ray.maxFraction) {
|
|
return false;
|
|
}
|
|
// Compute the hit information
|
|
vec3 localHitPoint = ray.point1 + t * n;
|
|
raycastInfo.body = proxyShape->getBody();
|
|
raycastInfo.proxyShape = proxyShape;
|
|
raycastInfo.hitFraction = t;
|
|
raycastInfo.worldPoint = localHitPoint;
|
|
vec3 normalDirection(0, float(-1.0), 0);
|
|
raycastInfo.worldNormal = normalDirection;
|
|
return true;
|
|
}
|
|
// If the intersection is outside the cylinder on the "q" side
|
|
if (value > dDotD) {
|
|
// If the ray is pointing away from the "q" endcap, we return no hit
|
|
if (nDotD >= 0.0f) {
|
|
return false;
|
|
}
|
|
// Compute the intersection against the "q" endcap (intersection against whole plane)
|
|
t = (dDotD - mDotD) / nDotD;
|
|
// Keep the intersection if it is inside the cylinder radius
|
|
if (k + dDotD - float(2.0) * mDotD + t * (float(2.0) * (mDotN - nDotD) + t) > 0.0f) {
|
|
return false;
|
|
}
|
|
// If the intersection is behind the origin of the ray or beyond the maximum
|
|
// raycasting distance, we return no hit
|
|
if (t < 0.0f || t > ray.maxFraction) {
|
|
return false;
|
|
}
|
|
// Compute the hit information
|
|
vec3 localHitPoint = ray.point1 + t * n;
|
|
raycastInfo.body = proxyShape->getBody();
|
|
raycastInfo.proxyShape = proxyShape;
|
|
raycastInfo.hitFraction = t;
|
|
raycastInfo.worldPoint = localHitPoint;
|
|
vec3 normalDirection(0, 1.0f, 0);
|
|
raycastInfo.worldNormal = normalDirection;
|
|
return true;
|
|
}
|
|
t = t0;
|
|
// If the intersection is behind the origin of the ray or beyond the maximum
|
|
// raycasting distance, we return no hit
|
|
if (t < 0.0f || t > ray.maxFraction) {
|
|
return false;
|
|
}
|
|
// Compute the hit information
|
|
vec3 localHitPoint = ray.point1 + t * n;
|
|
raycastInfo.body = proxyShape->getBody();
|
|
raycastInfo.proxyShape = proxyShape;
|
|
raycastInfo.hitFraction = t;
|
|
raycastInfo.worldPoint = localHitPoint;
|
|
vec3 v = localHitPoint - p;
|
|
vec3 w = (v.dot(d) / d.length2()) * d;
|
|
vec3 normalDirection = (localHitPoint - (p + w));
|
|
raycastInfo.worldNormal = normalDirection;
|
|
return true;
|
|
}
|
|
|
|
float CylinderShape::getRadius() {
|
|
return this.radius;
|
|
}
|
|
|
|
float CylinderShape::getHeight() {
|
|
return this.halfHeight + this.halfHeight;
|
|
}
|
|
|
|
void CylinderShape::setLocalScaling( vec3 scaling) {
|
|
this.halfHeight = (this.halfHeight / this.scaling.y()) * scaling.y();
|
|
this.radius = (this.radius / this.scaling.x()) * scaling.x();
|
|
CollisionShape::setLocalScaling(scaling);
|
|
}
|
|
|
|
sizet CylinderShape::getSizeInBytes() {
|
|
return sizeof(CylinderShape);
|
|
}
|
|
|
|
void CylinderShape::getLocalBounds(vec3 min, vec3 max) {
|
|
// Maximum bounds
|
|
max.setX(this.radius + this.margin);
|
|
max.setY(this.halfHeight + this.margin);
|
|
max.setZ(max.x());
|
|
// Minimum bounds
|
|
min.setX(-max.x());
|
|
min.setY(-max.y());
|
|
min.setZ(min.x());
|
|
}
|
|
|
|
void CylinderShape::computeLocalInertiaTensor(etk::Matrix3x3 tensor, float mass) {
|
|
float height = float(2.0) * this.halfHeight;
|
|
float diag = (1.0f / float(12.0)) * mass * (3 * this.radius * this.radius + height * height);
|
|
tensor.setValue(diag, 0.0, 0.0, 0.0,
|
|
0.5f * mass * this.radius * this.radius, 0.0,
|
|
0.0, 0.0, diag);
|
|
}
|
|
|
|
boolean CylinderShape::testPointInside( vec3 localPoint, ProxyShape* proxyShape) {
|
|
return ( (localPoint.x() * localPoint.x() + localPoint.z() * localPoint.z()) < this.radius * this.radius
|
|
hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj localPoint.y() < this.halfHeight
|
|
hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj localPoint.y() > -this.halfHeight);
|
|
}
|