mirror of
https://github.com/copyrighttxt/watrbx-game-engine.git
synced 2026-09-04 20:57:49 +00:00
306 lines
9.0 KiB
C++
306 lines
9.0 KiB
C++
#include "stdafx.h"
|
|
|
|
#include "v8world/Cylinder.h"
|
|
|
|
#include "v8world/MegaClusterPoly.h"
|
|
|
|
#include "Util/Units.h"
|
|
#include "Util/Math.h"
|
|
|
|
DYNAMIC_FASTFLAGVARIABLE(CylinderSurfaceNormalHitFix, false)
|
|
|
|
|
|
namespace RBX {
|
|
|
|
Cylinder::Cylinder()
|
|
: realLength(0)
|
|
, realWidth(0)
|
|
{
|
|
bulletCollisionObject.reset(new btCollisionObject());
|
|
}
|
|
|
|
Cylinder::~Cylinder()
|
|
{
|
|
}
|
|
|
|
bool Cylinder::setUpBulletCollisionData()
|
|
{
|
|
if (!bulletCollisionObject)
|
|
updateBulletCollisionData();
|
|
|
|
return true;
|
|
}
|
|
|
|
void Cylinder::updateBulletCollisionData()
|
|
{
|
|
if (!bulletCollisionObject)
|
|
bulletCollisionObject.reset(new btCollisionObject());
|
|
|
|
bulletCylinderShape = BulletCylinderShapePool::getToken(Vector3(realLength, realWidth, realWidth));
|
|
bulletCollisionObject->setCollisionShape(const_cast<btCylinderShape*>(bulletCylinderShape->getShape()));
|
|
}
|
|
|
|
Vector3 Cylinder::getCenterToCorner(const Matrix3& rotation) const
|
|
{
|
|
// Approximate with a box
|
|
Vector3 size(realLength, realWidth, realWidth);
|
|
|
|
Vector3 newSize = Vector3(
|
|
fabs(rotation[0][0]) * size[0] + fabs(rotation[0][1]) * size[1] + fabs(rotation[0][2]) * size[2],
|
|
fabs(rotation[1][0]) * size[0] + fabs(rotation[1][1]) * size[1] + fabs(rotation[1][2]) * size[2],
|
|
fabs(rotation[2][0]) * size[0] + fabs(rotation[2][1]) * size[1] + fabs(rotation[2][2]) * size[2]);
|
|
|
|
return newSize * 0.5f;
|
|
}
|
|
|
|
Matrix3 Cylinder::getMoment(float mass) const
|
|
{
|
|
float radius2 = realWidth * realWidth * 0.25f;
|
|
float length2 = realLength * realLength;
|
|
|
|
float x = (1.f / 2.f) * mass * radius2;
|
|
float yz = (1.f / 4.f) * mass * radius2 + (1.f / 12.f) * mass * length2;
|
|
|
|
return Matrix3::fromDiagonal(Vector3(x, yz, yz));
|
|
}
|
|
|
|
float Cylinder::getVolume() const
|
|
{
|
|
return Math::pif() * realWidth * realWidth * realLength * 0.25f;
|
|
}
|
|
|
|
float Cylinder::getRadius() const
|
|
{
|
|
return (Vector3(realLength, realWidth, realWidth) * 0.5f).length();
|
|
}
|
|
|
|
int Cylinder::getNumSurfaces() const
|
|
{
|
|
return 6;
|
|
}
|
|
|
|
bool Cylinder::findTouchingSurfacesConvex(const CoordinateFrame& myCf, size_t& myFaceId, const Geometry& otherGeom, const CoordinateFrame& otherCf, size_t& otherFaceId) const
|
|
{
|
|
return false;
|
|
}
|
|
|
|
bool Cylinder::FacesOverlapped(const CoordinateFrame& myCf, size_t& myFaceId, const Geometry& otherGeom, const CoordinateFrame& otherCf, size_t& otherFaceId, float tol) const
|
|
{
|
|
return false;
|
|
}
|
|
|
|
bool Cylinder::FaceVerticesOverlapped(const CoordinateFrame& myCf, size_t& myFaceId, const Geometry& otherGeom, const CoordinateFrame& otherCf, size_t& otherFaceId, float tol) const
|
|
{
|
|
return false;
|
|
}
|
|
|
|
bool Cylinder::FaceEdgesOverlapped(const CoordinateFrame& myCf, size_t& myFaceId, const Geometry& otherGeom, const CoordinateFrame& otherCf, size_t& otherFaceId, float tol) const
|
|
{
|
|
return false;
|
|
}
|
|
|
|
bool Cylinder::hitTest(const RbxRay& rayInMe, Vector3& localHitPoint, Vector3& surfaceNormal)
|
|
{
|
|
if (DFFlag::CylinderSurfaceNormalHitFix)
|
|
{
|
|
Vector3 halfSize = getSize() * 0.5f;
|
|
double radius = std::min<double>(halfSize.y, halfSize.z);
|
|
|
|
if (rayInMe.origin().x < -halfSize.x)
|
|
{
|
|
if (rayInMe.direction().x > 0)
|
|
{
|
|
double distanceToPlane = (-halfSize.x - rayInMe.origin().x) / rayInMe.direction().x;
|
|
Vector3 intersectionPoint = rayInMe.origin() + (rayInMe.direction() * distanceToPlane);
|
|
|
|
double distance = (intersectionPoint - Vector3(-halfSize.x, 0, 0)).magnitude();
|
|
|
|
if (distance <= radius)
|
|
{
|
|
surfaceNormal = Vector3(-1, 0, 0);
|
|
localHitPoint = intersectionPoint;
|
|
return true;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
return false;
|
|
}
|
|
|
|
}
|
|
else if (rayInMe.origin().x > halfSize.x)
|
|
{
|
|
if (rayInMe.direction().x < 0)
|
|
{
|
|
double distanceToPlane = (halfSize.x - rayInMe.origin().x) / rayInMe.direction().x;
|
|
Vector3 intersectionPoint = rayInMe.origin() + (rayInMe.direction() * distanceToPlane);
|
|
|
|
double distance = (intersectionPoint - Vector3(halfSize.x, 0, 0)).magnitude();
|
|
|
|
if (distance <= radius)
|
|
{
|
|
surfaceNormal = Vector3(1, 0, 0);
|
|
localHitPoint = intersectionPoint;
|
|
return true;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
return false;
|
|
}
|
|
}
|
|
|
|
Vector3 projectedDirection = rayInMe.direction() * Vector3(0, 1, 1);
|
|
Vector3 projectedOrigin = rayInMe.origin() * Vector3(0, 1, 1);
|
|
|
|
double a = projectedDirection.dot(projectedDirection);
|
|
double b = 2 * projectedOrigin.dot(projectedDirection);
|
|
double c = projectedOrigin.dot(projectedOrigin) - (radius * radius);
|
|
|
|
double discriminant = (b*b)-(4*a*c);
|
|
|
|
if (discriminant < 0)
|
|
return false;
|
|
|
|
double distance = (-b - std::sqrt(discriminant)) / (2*a);
|
|
|
|
if (distance > 0)
|
|
{
|
|
Vector3 intersectionPoint = rayInMe.origin() + (rayInMe.direction() * distance);
|
|
if (intersectionPoint.x < halfSize.x && intersectionPoint.x > -halfSize.x)
|
|
{
|
|
surfaceNormal = (intersectionPoint * Vector3(0, 1, 1)).unit();
|
|
localHitPoint = intersectionPoint;
|
|
return true;
|
|
}
|
|
}
|
|
|
|
return false;
|
|
}
|
|
else
|
|
{
|
|
if (!bulletCylinderShape)
|
|
return false;
|
|
|
|
const float maxDistance = MC_SEARCH_RAY_MAX;
|
|
|
|
btTransform identityTransform;
|
|
identityTransform.setIdentity();
|
|
|
|
Vector3 fromVector3 = rayInMe.origin();
|
|
btVector3 from(fromVector3.x, fromVector3.y, fromVector3.z);
|
|
Vector3 toVector3 = rayInMe.origin() + maxDistance * rayInMe.direction();
|
|
btVector3 to(toVector3.x, toVector3.y, toVector3.z);
|
|
|
|
btCollisionWorld::ClosestRayResultCallback resultCallback(from, to);
|
|
|
|
btCollisionObjectWrapper colObWrap(0, bulletCylinderShape->getShape(), getBulletCollisionObject(), identityTransform, -1, -1);
|
|
|
|
btTransform rayFromTrans(identityTransform.getBasis(), from);
|
|
btTransform rayToTrans(identityTransform.getBasis(), to);
|
|
btCollisionWorld::rayTestSingleInternal(rayFromTrans, rayToTrans, &colObWrap, resultCallback);
|
|
|
|
if (resultCallback.hasHit())
|
|
{
|
|
surfaceNormal = Vector3(resultCallback.m_hitNormalWorld.x(), resultCallback.m_hitNormalWorld.y(), resultCallback.m_hitNormalWorld.z());
|
|
localHitPoint = Vector3(resultCallback.m_hitPointWorld.x(), resultCallback.m_hitPointWorld.y(), resultCallback.m_hitPointWorld.z());
|
|
return true;
|
|
}
|
|
|
|
return false;
|
|
}
|
|
}
|
|
|
|
void Cylinder::setSize(const G3D::Vector3& _size)
|
|
{
|
|
Super::setSize(_size);
|
|
RBXASSERT(getSize() == _size);
|
|
|
|
realLength = _size.x;
|
|
realWidth = std::min(_size.y, _size.z);
|
|
|
|
if (bulletCollisionObject)
|
|
updateBulletCollisionData();
|
|
}
|
|
|
|
size_t Cylinder::closestSurfaceToPoint(const Vector3& pointInBody) const
|
|
{
|
|
Vector3 size(realLength, realWidth, realWidth);
|
|
|
|
size_t surface = 0;
|
|
|
|
for (size_t i = 1; i < 6; ++i)
|
|
{
|
|
// This is not very efficient but the function should only be used in dragger code so we don't care too much
|
|
float di = getPlaneFromSurface(i).distance(pointInBody);
|
|
float ds = getPlaneFromSurface(surface).distance(pointInBody);
|
|
|
|
if (fabsf(di) > fabsf(ds))
|
|
surface = i;
|
|
}
|
|
|
|
return surface;
|
|
}
|
|
|
|
Plane Cylinder::getPlaneFromSurface(const size_t surfaceId) const
|
|
{
|
|
Vector3 normal = Math::getWorldNormal(static_cast<NormalId>(surfaceId), Matrix3::identity());
|
|
float distance = dot(normal, Vector3(realLength, realWidth, realWidth) * 0.5f);
|
|
|
|
return Plane(normal, fabsf(distance));
|
|
}
|
|
|
|
Vector3 Cylinder::getSurfaceVertInBody(const size_t surfaceId, const int vertId) const
|
|
{
|
|
Vector3 size(realLength, realWidth, realWidth);
|
|
Extents extents(-size * 0.5f, size * 0.5f);
|
|
|
|
Vector3 surface[4];
|
|
extents.getFaceCorners(static_cast<NormalId>(surfaceId), surface[0], surface[1], surface[2], surface[3]);
|
|
|
|
return surface[vertId];
|
|
}
|
|
|
|
size_t Cylinder::getMostAlignedSurface(const Vector3& vecInWorld, const G3D::Matrix3& objectR) const
|
|
{
|
|
return Math::getClosestObjectNormalId(vecInWorld, objectR);
|
|
}
|
|
|
|
int Cylinder::getNumVertsInSurface(const size_t surfaceId) const
|
|
{
|
|
return 4;
|
|
}
|
|
|
|
bool Cylinder::vertOverlapsFace(const Vector3& pointInBody, const size_t surfaceId) const
|
|
{
|
|
if (surfaceId == NORM_X || surfaceId == NORM_X_NEG)
|
|
return pointInBody.yz().squaredLength() <= realWidth * realWidth * 0.25f;
|
|
else if (surfaceId == NORM_Y || surfaceId == NORM_Y_NEG)
|
|
return fabsf(pointInBody.x) <= realLength * 0.5f && fabsf(pointInBody.z) <= realWidth * 0.5f;
|
|
else if (surfaceId == NORM_Z || surfaceId == NORM_Z_NEG)
|
|
return fabsf(pointInBody.x) <= realLength * 0.5f && fabsf(pointInBody.y) <= realWidth * 0.5f;
|
|
else
|
|
return false;
|
|
}
|
|
|
|
CoordinateFrame Cylinder::getSurfaceCoordInBody(const size_t surfaceId) const
|
|
{
|
|
float sign = (surfaceId >= NORM_X_NEG) ? -1 : 1;
|
|
|
|
Vector3 faceCentroid;
|
|
faceCentroid.x = (surfaceId == NORM_X || surfaceId == NORM_X_NEG) ? sign * realLength * 0.5f : 0;
|
|
faceCentroid.y = (surfaceId == NORM_Y || surfaceId == NORM_Y_NEG) ? sign * realWidth * 0.5f : 0;
|
|
faceCentroid.z = (surfaceId == NORM_Z || surfaceId == NORM_Z_NEG) ? sign * realWidth * 0.5f : 0;
|
|
|
|
Matrix3 rotation = Math::getWellFormedRotForZVector(getSurfaceNormalInBody(surfaceId));
|
|
|
|
return CoordinateFrame(rotation, faceCentroid);
|
|
}
|
|
|
|
Vector3 Cylinder::getSurfaceNormalInBody(const size_t surfaceId) const
|
|
{
|
|
return Math::getWorldNormal(static_cast<NormalId>(surfaceId), Matrix3::identity());
|
|
}
|
|
|
|
} // namespace RBX
|