Initial commit
This commit is contained in:
@@ -0,0 +1,362 @@
|
||||
/* Copyright (c) <2003-2011> <Julio Jerez, Newton Game Dynamics>
|
||||
*
|
||||
* This software is provided 'as-is', without any express or implied
|
||||
* warranty. In no event will the authors be held liable for any damages
|
||||
* arising from the use of this software.
|
||||
*
|
||||
* Permission is granted to anyone to use this software for any purpose,
|
||||
* including commercial applications, and to alter it and redistribute it
|
||||
* freely, subject to the following restrictions:
|
||||
*
|
||||
* 1. The origin of this software must not be misrepresented; you must not
|
||||
* claim that you wrote the original software. If you use this software
|
||||
* in a product, an acknowledgment in the product documentation would be
|
||||
* appreciated but is not required.
|
||||
*
|
||||
* 2. Altered source versions must be plainly marked as such, and must not be
|
||||
* misrepresented as being the original software.
|
||||
*
|
||||
* 3. This notice may not be removed or altered from any source distribution.
|
||||
*/
|
||||
|
||||
#include "dgCollisionConvexModifier.h"
|
||||
#include "dgBody.h"
|
||||
#include "dgCollisionEllipse.h"
|
||||
#include "dgContact.h"
|
||||
#include "dgWorld.h"
|
||||
#include "hpl1/engine/libraries/newton/core/dg.h"
|
||||
|
||||
|
||||
//////////////////////////////////////////////////////////////////////
|
||||
// Construction/Destruction
|
||||
//////////////////////////////////////////////////////////////////////
|
||||
|
||||
dgCollisionConvexModifier::dgCollisionConvexModifier(
|
||||
dgCollisionConvex *convexChild, dgWorld *world) : dgCollisionConvex(world->GetAllocator(), 0, dgGetIdentityMatrix(),
|
||||
m_convexCollisionModifier),
|
||||
m_modifierMatrix(dgGetIdentityMatrix()), m_modifierInvMatrix(
|
||||
dgGetIdentityMatrix()) {
|
||||
m_world = world;
|
||||
m_det = dgFloat32(1.0f);
|
||||
m_convexCollision = convexChild;
|
||||
convexChild->AddRef();
|
||||
|
||||
m_rtti |= dgCollisionConvexModifier_RTTI;
|
||||
// hack to make the collision work
|
||||
m_vertexCount = 1;
|
||||
SetUserData(convexChild->GetUserData());
|
||||
|
||||
SetOffsetMatrix(m_convexCollision->GetOffsetMatrix());
|
||||
}
|
||||
|
||||
dgCollisionConvexModifier::dgCollisionConvexModifier(dgWorld *const world,
|
||||
dgDeserialize deserialization, void *const userData) : dgCollisionConvex(world, deserialization, userData) {
|
||||
dgMatrix matrix;
|
||||
|
||||
m_rtti |= dgCollisionConvexModifier_RTTI;
|
||||
|
||||
m_world = world;
|
||||
deserialization(userData, &matrix, sizeof(dgMatrix));
|
||||
m_convexCollision = (dgCollisionConvex *)world->CreateFromSerialization(
|
||||
deserialization, userData);
|
||||
|
||||
m_det = dgFloat32(1.0f);
|
||||
SetUserData(m_convexCollision->GetUserData());
|
||||
SetUserDataID(SetUserDataID());
|
||||
|
||||
// hack to make the collision work
|
||||
m_vertexCount = 1;
|
||||
ModifierSetMatrix(matrix);
|
||||
}
|
||||
|
||||
dgCollisionConvexModifier::~dgCollisionConvexModifier() {
|
||||
m_vertexCount = 0;
|
||||
m_world->ReleaseCollision(m_convexCollision);
|
||||
}
|
||||
|
||||
dgMatrix dgCollisionConvexModifier::ModifierGetMatrix() const {
|
||||
return m_offset.Inverse() * m_modifierMatrix * m_offset;
|
||||
}
|
||||
|
||||
dgFloat32 dgCollisionConvexModifier::GetBoxMinRadius() const {
|
||||
return m_convexCollision->GetBoxMinRadius();
|
||||
}
|
||||
dgFloat32 dgCollisionConvexModifier::GetBoxMaxRadius() const {
|
||||
return m_convexCollision->GetBoxMaxRadius();
|
||||
}
|
||||
|
||||
void dgCollisionConvexModifier::ModifierSetMatrix(const dgMatrix &matrix) {
|
||||
|
||||
dgInt32 i;
|
||||
dgInt32 j;
|
||||
dgInt32 k;
|
||||
dgFloat32 val;
|
||||
dgFloat32 mat[4][8];
|
||||
|
||||
// copy the matrix into the modifier
|
||||
m_modifierMatrix = m_offset * matrix * m_offset.Inverse();
|
||||
|
||||
// create a matrix matrix array set to identity
|
||||
for (i = 0; i < 4; i++) {
|
||||
for (j = 0; j < 4; j++) {
|
||||
mat[i][j] = m_modifierMatrix[i][j];
|
||||
mat[i][j + 4] = dgFloat32(0.0f);
|
||||
}
|
||||
mat[i][i + 4] = dgFloat32(1.0f);
|
||||
}
|
||||
|
||||
// calculate the inverse matrix of the modifier using full
|
||||
// Gauss Jordan pivoting method
|
||||
m_det = dgFloat32(1.0f);
|
||||
for (i = 0; i < 4; i++) {
|
||||
if (dgAbsf(mat[i][i]) < dgFloat32(1.0e-3f)) {
|
||||
for (j = i + 1; j < 4; j++) {
|
||||
if (dgAbsf(mat[j][i]) > dgFloat32(1.0e-3f)) {
|
||||
for (k = 0; k < 8; k++) {
|
||||
mat[i][k] += mat[j][k];
|
||||
}
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
val = dgFloat32(1.0f) / mat[i][i];
|
||||
m_det *= mat[i][i];
|
||||
for (j = 0; j < 8; j++) {
|
||||
mat[i][j] *= val;
|
||||
}
|
||||
mat[i][i] = dgFloat32(1.0f);
|
||||
|
||||
for (j = 0; j < 4; j++) {
|
||||
if (j != i) {
|
||||
val = mat[j][i];
|
||||
for (k = 0; k < 8; k++) {
|
||||
mat[j][k] -= val * mat[i][k];
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// copy the inverted matrix into the modifier
|
||||
for (i = 0; i < 4; i++) {
|
||||
for (j = 0; j < 4; j++) {
|
||||
m_modifierInvMatrix[i][j] = mat[i][j + 4];
|
||||
}
|
||||
}
|
||||
|
||||
#ifdef _DEBUG
|
||||
// check the the matrix is correctly inverted
|
||||
dgMatrix tmp(m_modifierInvMatrix * m_modifierMatrix);
|
||||
for (i = 0; i < 4; i++) {
|
||||
NEWTON_ASSERT(dgAbsf(tmp[i][i] - dgFloat32(1.0f)) < dgFloat32(1.0e-5f));
|
||||
for (j = i + 1; j < 4; j++) {
|
||||
NEWTON_ASSERT(dgAbsf(tmp[i][j]) < dgFloat32(1.0e-5f));
|
||||
NEWTON_ASSERT(dgAbsf(tmp[j][i]) < dgFloat32(1.0e-5f));
|
||||
}
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
||||
dgInt32 dgCollisionConvexModifier::CalculateSignature() const {
|
||||
return 0;
|
||||
}
|
||||
|
||||
void dgCollisionConvexModifier::SetCollisionBBox(const dgVector &p0__,
|
||||
const dgVector &p1__) {
|
||||
NEWTON_ASSERT(0);
|
||||
}
|
||||
|
||||
void dgCollisionConvexModifier::CalcAABB(const dgMatrix &matrix, dgVector &p0,
|
||||
dgVector &p1) const {
|
||||
dgMatrix trans(matrix.Transpose());
|
||||
|
||||
for (dgInt32 i = 0; i < 3; i++) {
|
||||
p0[i] =
|
||||
matrix.m_posit[i] + matrix.RotateVector(SupportVertex(trans[i].Scale(-dgFloat32(1.0f))))[i] - dgFloat32(5.0e-2f);
|
||||
p1[i] = matrix.m_posit[i] + matrix.RotateVector(SupportVertex(trans[i]))[i] + dgFloat32(5.0e-2f);
|
||||
}
|
||||
}
|
||||
|
||||
void dgCollisionConvexModifier::CalcAABBSimd(const dgMatrix &matrix,
|
||||
dgVector &p0, dgVector &p1) const {
|
||||
#ifdef DG_BUILD_SIMD_CODE
|
||||
dgInt32 i;
|
||||
dgMatrix trans(matrix.Transpose());
|
||||
for (i = 0; i < 3; i++) {
|
||||
p0[i] = matrix.m_posit[i] + matrix.RotateVectorSimd(SupportVertexSimd(trans[i].Scale(-dgFloat32(1.0f))))[i] - dgFloat32(5.0e-2f);
|
||||
p1[i] = matrix.m_posit[i] + matrix.RotateVectorSimd(SupportVertexSimd(trans[i]))[i] + dgFloat32(5.0e-2f);
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
||||
dgVector dgCollisionConvexModifier::SupportVertex(const dgVector &dir) const {
|
||||
NEWTON_ASSERT(dgAbsf(dir % dir - dgFloat32(1.0f)) < dgFloat32(1.0e-2f));
|
||||
|
||||
dgVector dir1(m_modifierMatrix.UnrotateVector(dir));
|
||||
dir1 = dir1.Scale(dgRsqrt(dir1 % dir1));
|
||||
|
||||
NEWTON_ASSERT(dgAbsf(dir1 % dir1 - dgFloat32(1.0f)) < dgFloat32(1.0e-2f));
|
||||
return m_modifierMatrix.TransformVector(
|
||||
m_convexCollision->SupportVertex(dir1));
|
||||
}
|
||||
|
||||
dgVector dgCollisionConvexModifier::SupportVertexSimd(const dgVector &dir) const {
|
||||
#ifdef DG_BUILD_SIMD_CODE
|
||||
simd_type tmp1;
|
||||
simd_type tmp0;
|
||||
dgVector localDir;
|
||||
dgVector dir1(m_modifierMatrix.UnrotateVectorSimd(dir));
|
||||
|
||||
NEWTON_ASSERT(dgAbsf(dir % dir - dgFloat32(1.0f)) < dgFloat32(1.0e-2f));
|
||||
// dir1 = dir1.Scale (dgRsqrt (dir1 % dir1));
|
||||
tmp1 =
|
||||
simd_mul_v((simd_type &)dir1, simd_and_v((simd_type &)dir1, (simd_type &)m_triplexMask));
|
||||
tmp1 = simd_add_v(tmp1, simd_move_hl_v(tmp1, tmp1));
|
||||
tmp1 = simd_add_s(tmp1, simd_permut_v(tmp1, tmp1, PURMUT_MASK(0, 0, 0, 1)));
|
||||
|
||||
tmp0 = simd_rsqrt_s(tmp1);
|
||||
tmp0 =
|
||||
simd_mul_s(simd_mul_s((simd_type &)m_nrh0p5, tmp0), simd_mul_sub_s((simd_type &)m_nrh3p0, simd_mul_s(tmp1, tmp0), tmp0));
|
||||
(simd_type &)localDir =
|
||||
simd_mul_v((simd_type &)dir1, simd_permut_v(tmp0, tmp0, PURMUT_MASK(3, 0, 0, 0)));
|
||||
|
||||
NEWTON_ASSERT(
|
||||
dgAbsf(localDir % localDir - dgFloat32(1.0f)) < dgFloat32(1.0e-2f));
|
||||
return m_modifierMatrix.TransformVectorSimd(
|
||||
m_convexCollision->SupportVertexSimd(localDir));
|
||||
|
||||
#else
|
||||
return dgVector(dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f));
|
||||
#endif
|
||||
}
|
||||
|
||||
dgInt32 dgCollisionConvexModifier::CalculatePlaneIntersection(
|
||||
const dgVector &normal, const dgVector &point,
|
||||
dgVector *const contactsOut) const {
|
||||
dgInt32 i;
|
||||
dgInt32 count;
|
||||
|
||||
dgVector n(m_modifierMatrix.UnrotateVector(normal));
|
||||
n = n.Scale(dgRsqrt(n % n));
|
||||
|
||||
dgVector p(m_modifierInvMatrix.TransformVector(point));
|
||||
count = m_convexCollision->CalculatePlaneIntersection(n, p, contactsOut);
|
||||
for (i = 0; i < count; i++) {
|
||||
contactsOut[i] = m_modifierMatrix.TransformVector(contactsOut[i]);
|
||||
}
|
||||
|
||||
return count;
|
||||
}
|
||||
|
||||
dgInt32 dgCollisionConvexModifier::CalculatePlaneIntersectionSimd(
|
||||
const dgVector &normal, const dgVector &point,
|
||||
dgVector *const contactsOut) const {
|
||||
return CalculatePlaneIntersection(normal, point, contactsOut);
|
||||
}
|
||||
|
||||
void dgCollisionConvexModifier::DebugCollision(const dgMatrix &matrixPtr,
|
||||
OnDebugCollisionMeshCallback callback, void *const userData) const {
|
||||
dgMatrix matrixInv(m_offset.Inverse());
|
||||
dgMatrix matrix(matrixInv * m_modifierMatrix * m_offset * matrixPtr);
|
||||
m_convexCollision->DebugCollision(matrix, callback, userData);
|
||||
}
|
||||
|
||||
dgFloat32 dgCollisionConvexModifier::RayCast(const dgVector &p0,
|
||||
const dgVector &p1, dgContactPoint &contactOut,
|
||||
OnRayPrecastAction preFilter, const dgBody *const body,
|
||||
void *const userData) const {
|
||||
dgFloat32 t;
|
||||
if (PREFILTER_RAYCAST(preFilter, reinterpret_cast<const NewtonBody *>(body), reinterpret_cast<const NewtonCollision *>(this), userData)) {
|
||||
return dgFloat32(1.2f);
|
||||
}
|
||||
|
||||
dgVector q0(m_modifierInvMatrix.TransformVector(p0));
|
||||
dgVector q1(m_modifierInvMatrix.TransformVector(p1));
|
||||
|
||||
t = m_convexCollision->RayCast(q0, q1, contactOut, NULL, NULL, NULL);
|
||||
if ((t >= dgFloat32(0.0f)) && (t <= dgFloat32(1.0f))) {
|
||||
dgVector n(m_modifierMatrix.RotateVector(contactOut.m_normal));
|
||||
contactOut.m_normal = n.Scale(dgRsqrt(n % n));
|
||||
}
|
||||
return t;
|
||||
}
|
||||
|
||||
dgFloat32 dgCollisionConvexModifier::RayCastSimd(const dgVector &p0,
|
||||
const dgVector &p1, dgContactPoint &contactOut,
|
||||
OnRayPrecastAction preFilter, const dgBody *const body,
|
||||
void *const userData) const {
|
||||
dgFloat32 t;
|
||||
if (PREFILTER_RAYCAST(preFilter, reinterpret_cast<const NewtonBody *>(body), reinterpret_cast<const NewtonCollision *>(this), userData)) {
|
||||
return dgFloat32(1.2f);
|
||||
}
|
||||
|
||||
dgVector q0(m_modifierInvMatrix.TransformVectorSimd(p0));
|
||||
dgVector q1(m_modifierInvMatrix.TransformVectorSimd(p1));
|
||||
|
||||
t = m_convexCollision->RayCastSimd(q0, q1, contactOut, NULL, NULL, NULL);
|
||||
if ((t >= dgFloat32(0.0f)) && (t <= dgFloat32(1.0f))) {
|
||||
dgVector n(m_modifierMatrix.RotateVectorSimd(contactOut.m_normal));
|
||||
contactOut.m_normal = n.Scale(dgRsqrt(n % n));
|
||||
}
|
||||
return t;
|
||||
}
|
||||
|
||||
dgFloat32 dgCollisionConvexModifier::GetVolume() const {
|
||||
return m_convexCollision->GetVolume() * m_det;
|
||||
}
|
||||
|
||||
dgVector dgCollisionConvexModifier::CalculateVolumeIntegral(
|
||||
const dgMatrix &globalMatrix, GetBuoyancyPlane bouyancyPlane,
|
||||
void *const context) const {
|
||||
dgFloat32 volume;
|
||||
|
||||
dgPlane plane(dgFloat32(0.0f), dgFloat32(1.0f), dgFloat32(0.0f),
|
||||
dgFloat32(-1.0e8f));
|
||||
if (bouyancyPlane) {
|
||||
dgPlane globalPlane;
|
||||
// user data is not used
|
||||
if (bouyancyPlane(0, context, &globalMatrix.m_front.m_x, &globalPlane.m_x)) {
|
||||
plane = globalMatrix.UntransformPlane(globalPlane);
|
||||
|
||||
dgVector point(plane.Scale(-plane.m_w));
|
||||
|
||||
dgVector n(m_modifierMatrix.UnrotateVector(plane));
|
||||
n = n.Scale(dgRsqrt(n % n));
|
||||
|
||||
dgVector p(m_modifierInvMatrix.TransformVector(point));
|
||||
|
||||
plane = dgPlane(n, -(n % p));
|
||||
}
|
||||
}
|
||||
|
||||
dgVector cg(m_convexCollision->CalculateVolumeIntegral(plane));
|
||||
|
||||
volume = cg.m_w;
|
||||
if (volume > dgFloat32(1.0e-8f)) {
|
||||
cg = cg.Scale(dgFloat32(0.5f) / cg.m_w);
|
||||
}
|
||||
|
||||
cg = globalMatrix.TransformVector(m_modifierMatrix.TransformVector(cg));
|
||||
cg.m_w = volume * m_det;
|
||||
|
||||
return cg;
|
||||
}
|
||||
|
||||
void dgCollisionConvexModifier::GetCollisionInfo(dgCollisionInfo *info) const {
|
||||
dgCollisionConvex::GetCollisionInfo(info);
|
||||
info->m_offsetMatrix = GetOffsetMatrix();
|
||||
info->m_convexModifierData.m_child = m_convexCollision;
|
||||
// strcpy (info->m_collisionType, "modifier");
|
||||
info->m_collisionType = m_collsionId;
|
||||
}
|
||||
|
||||
void dgCollisionConvexModifier::Serialize(dgSerialize callback,
|
||||
void *const userData) const {
|
||||
SerializeLow(callback, userData);
|
||||
callback(userData, &m_modifierMatrix, sizeof(dgMatrix));
|
||||
m_world->Serialize(m_convexCollision, callback, userData);
|
||||
}
|
||||
|
||||
bool dgCollisionConvexModifier::OOBBTest(const dgMatrix &matrix,
|
||||
const dgCollisionConvex *const shape, void *const cacheOrder) const {
|
||||
return true;
|
||||
}
|
||||
Reference in New Issue
Block a user