Files
scummvm-cursorfix/engines/hpl1/engine/impl/PhysicsBodyNewton.cpp
2026-02-02 04:50:13 +01:00

508 lines
17 KiB
C++

/* ScummVM - Graphic Adventure Engine
*
* ScummVM is the legal property of its developers, whose names
* are too numerous to list here. Please refer to the COPYRIGHT
* file distributed with this source distribution.
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*
*/
/*
* Copyright (C) 2006-2010 - Frictional Games
*
* This file is part of HPL1 Engine.
*/
#include "hpl1/engine/impl/PhysicsBodyNewton.h"
#include "hpl1/engine/graphics/LowLevelGraphics.h"
#include "hpl1/engine/impl/CollideShapeNewton.h"
#include "hpl1/engine/impl/PhysicsMaterialNewton.h"
#include "hpl1/engine/impl/PhysicsWorldNewton.h"
#include "hpl1/engine/libraries/angelscript/angelscript.h"
#include "hpl1/engine/libraries/newton/Newton.h"
#include "hpl1/engine/math/Math.h"
#include "hpl1/engine/math/MathTypes.h"
#include "hpl1/engine/math/Vector3.h"
#include "hpl1/engine/scene/Node3D.h"
#include "hpl1/engine/system/low_level_system.h"
namespace hpl {
bool cPhysicsBodyNewton::mbUseCallback = true;
//////////////////////////////////////////////////////////////////////////
// CONSTRUCTORS
//////////////////////////////////////////////////////////////////////////
//-----------------------------------------------------------------------
cPhysicsBodyNewton::cPhysicsBodyNewton(const tString &asName, iPhysicsWorld *apWorld, iCollideShape *apShape)
: iPhysicsBody(asName, apWorld, apShape) {
cPhysicsWorldNewton *pWorldNewton = static_cast<cPhysicsWorldNewton *>(apWorld);
cCollideShapeNewton *pShapeNewton = static_cast<cCollideShapeNewton *>(apShape);
mpNewtonWorld = pWorldNewton->GetNewtonWorld();
mpNewtonBody = NewtonCreateBody(pWorldNewton->GetNewtonWorld(),
pShapeNewton->GetNewtonCollision(), cMatrixf::Identity.v);
mpCallback = hplNew(cPhysicsBodyNewtonCallback, ());
AddCallback(mpCallback);
// Setup the callbacks and set this body as user data
// This is so that the transform gets updated and
// to add gravity, forces and user sink.
NewtonBodySetForceAndTorqueCallback(mpNewtonBody, OnUpdateCallback);
NewtonBodySetTransformCallback(mpNewtonBody, OnTransformCallback);
NewtonBodySetUserData(mpNewtonBody, this);
// Set default property settings
mbGravity = true;
mfMaxLinearSpeed = 0;
mfMaxAngularSpeed = 0;
mfMass = 0;
mfAutoDisableLinearThreshold = 0.01f;
mfAutoDisableAngularThreshold = 0.01f;
mlAutoDisableNumSteps = 10;
// Log("Creating newton body '%s' %d\n",msName.c_str(),(size_t)this);
}
//-----------------------------------------------------------------------
cPhysicsBodyNewton::~cPhysicsBodyNewton() {
// Log(" Destroying newton body '%s' %d\n",msName.c_str(),(size_t)this);
}
//-----------------------------------------------------------------------
void cPhysicsBodyNewton::DeleteLowLevel() {
// Log(" Newton body %d\n", (size_t)mpNewtonBody);
NewtonDestroyBody(mpNewtonWorld, mpNewtonBody);
// Log(" Callback\n");
hplDelete(mpCallback);
}
//-----------------------------------------------------------------------
//////////////////////////////////////////////////////////////////////////
// CALLBACK METHODS
//////////////////////////////////////////////////////////////////////////
//-----------------------------------------------------------------------
void cPhysicsBodyNewtonCallback::OnTransformUpdate(iEntity3D *apEntity) {
if (cPhysicsBodyNewton::mbUseCallback == false)
return;
cPhysicsBodyNewton *pRigidBody = static_cast<cPhysicsBodyNewton *>(apEntity);
cMatrixf mTemp = apEntity->GetLocalMatrix().GetTranspose();
NewtonBodySetMatrix(pRigidBody->mpNewtonBody, mTemp.v);
if (pRigidBody->mpNode)
pRigidBody->mpNode->SetMatrix(apEntity->GetLocalMatrix());
}
//-----------------------------------------------------------------------
//////////////////////////////////////////////////////////////////////////
// PUBLIC METHODS
//////////////////////////////////////////////////////////////////////////
//-----------------------------------------------------------------------
void cPhysicsBodyNewton::SetMaterial(iPhysicsMaterial *apMaterial) {
mpMaterial = apMaterial;
if (apMaterial == nullptr)
return;
cPhysicsMaterialNewton *pNewtonMat = static_cast<cPhysicsMaterialNewton *>(mpMaterial);
NewtonBodySetMaterialGroupID(mpNewtonBody, pNewtonMat->GetId());
}
//-----------------------------------------------------------------------
void cPhysicsBodyNewton::SetLinearVelocity(const cVector3f &avVel) {
VEC3_CONST_ARRAY(vel, avVel);
NewtonBodySetVelocity(mpNewtonBody, vel);
}
cVector3f cPhysicsBodyNewton::GetLinearVelocity() const {
float vel[3];
NewtonBodyGetVelocity(mpNewtonBody, vel);
return cVector3f::fromArray(vel);
}
//-----------------------------------------------------------------------
void cPhysicsBodyNewton::SetAngularVelocity(const cVector3f &avVel) {
VEC3_CONST_ARRAY(vel, avVel);
NewtonBodySetOmega(mpNewtonBody, vel);
}
cVector3f cPhysicsBodyNewton::GetAngularVelocity() const {
float vel[3];
NewtonBodyGetOmega(mpNewtonBody, vel);
return cVector3f::fromArray(vel);
}
//-----------------------------------------------------------------------
void cPhysicsBodyNewton::SetLinearDamping(float afDamping) {
NewtonBodySetLinearDamping(mpNewtonBody, afDamping);
}
float cPhysicsBodyNewton::GetLinearDamping() const {
return NewtonBodyGetLinearDamping(mpNewtonBody);
}
//-----------------------------------------------------------------------
void cPhysicsBodyNewton::SetAngularDamping(float afDamping) {
float damp[3] = {afDamping, afDamping, afDamping};
NewtonBodySetAngularDamping(mpNewtonBody, damp);
}
float cPhysicsBodyNewton::GetAngularDamping() const {
float fDamp[3];
NewtonBodyGetAngularDamping(mpNewtonBody, fDamp);
return fDamp[0];
}
//-----------------------------------------------------------------------
void cPhysicsBodyNewton::SetMaxLinearSpeed(float afSpeed) {
mfMaxLinearSpeed = afSpeed;
}
float cPhysicsBodyNewton::GetMaxLinearSpeed() const {
return mfMaxLinearSpeed;
}
//-----------------------------------------------------------------------
void cPhysicsBodyNewton::SetMaxAngularSpeed(float afDamping) {
mfMaxAngularSpeed = afDamping;
}
float cPhysicsBodyNewton::GetMaxAngularSpeed() const {
return mfMaxAngularSpeed;
}
//-----------------------------------------------------------------------
cMatrixf cPhysicsBodyNewton::GetInertiaMatrix() {
float fIxx, fIyy, fIzz, fMass;
NewtonBodyGetMassMatrix(mpNewtonBody, &fMass, &fIxx, &fIyy, &fIzz);
cMatrixf mtxRot = GetLocalMatrix().GetRotation();
cMatrixf mtxTransRot = mtxRot.GetTranspose();
cMatrixf mtxI(fIxx, 0, 0, 0,
0, fIyy, 0, 0,
0, 0, fIzz, 0,
0, 0, 0, 1);
return cMath::MatrixMul(cMath::MatrixMul(mtxRot, mtxI), mtxTransRot);
}
//-----------------------------------------------------------------------
void cPhysicsBodyNewton::SetMass(float afMass) {
cCollideShapeNewton *pShapeNewton = static_cast<cCollideShapeNewton *>(mpShape);
float inertia[3];
float offset[3];
NewtonConvexCollisionCalculateInertialMatrix(pShapeNewton->GetNewtonCollision(),
inertia, offset);
cVector3f vInertia = cVector3f::fromArray(inertia) * afMass; // = pShapeNewton->GetInertia(afMass);
NewtonBodySetCentreOfMass(mpNewtonBody, offset);
NewtonBodySetMassMatrix(mpNewtonBody, afMass, vInertia.x, vInertia.y, vInertia.z);
mfMass = afMass;
}
float cPhysicsBodyNewton::GetMass() const {
return mfMass;
}
void cPhysicsBodyNewton::SetMassCentre(const cVector3f &avCentre) {
VEC3_CONST_ARRAY(ctr, avCentre);
NewtonBodySetCentreOfMass(mpNewtonBody, ctr);
}
cVector3f cPhysicsBodyNewton::GetMassCentre() const {
float center[3];
NewtonBodyGetCentreOfMass(mpNewtonBody, center);
return cVector3f::fromArray(center);
}
//-----------------------------------------------------------------------
void cPhysicsBodyNewton::AddForce(const cVector3f &avForce) {
mvTotalForce += avForce;
SetEnabled(true);
// Log("Added force %s\n",avForce.ToString().c_str());
}
void cPhysicsBodyNewton::AddForceAtPosition(const cVector3f &avForce, const cVector3f &avPos) {
mvTotalForce += avForce;
cVector3f vLocalPos = avPos - GetLocalPosition();
cVector3f vMassCentre = GetMassCentre();
if (vMassCentre != cVector3f(0, 0, 0)) {
vMassCentre = cMath::MatrixMul(GetLocalMatrix().GetRotation(), vMassCentre);
vLocalPos -= vMassCentre;
}
cVector3f vTorque = cMath::Vector3Cross(vLocalPos, avForce);
mvTotalTorque += vTorque;
SetEnabled(true);
// Log("Added force %s\n",avForce.ToString().c_str());
}
//-----------------------------------------------------------------------
void cPhysicsBodyNewton::AddTorque(const cVector3f &avTorque) {
mvTotalTorque += avTorque;
SetEnabled(true);
}
//-----------------------------------------------------------------------
void cPhysicsBodyNewton::AddImpulse(const cVector3f &avImpulse) {
cVector3f vMassCentre = GetMassCentre();
VEC3_CONST_ARRAY(impulse, avImpulse);
if (vMassCentre != cVector3f(0, 0, 0)) {
cVector3f vCentreOffset = cMath::MatrixMul(GetWorldMatrix().GetRotation(),
vMassCentre);
VEC3_CONST_ARRAY(worldPosition, (GetWorldPosition() + vCentreOffset));
NewtonBodyAddImpulse(mpNewtonBody, impulse, worldPosition);
} else {
VEC3_CONST_ARRAY(worldPosition, GetWorldPosition());
NewtonBodyAddImpulse(mpNewtonBody, impulse, worldPosition);
}
}
void cPhysicsBodyNewton::AddImpulseAtPosition(const cVector3f &avImpulse, const cVector3f &avPos) {
VEC3_CONST_ARRAY(impulse, avImpulse);
VEC3_CONST_ARRAY(pos, avPos);
NewtonBodyAddImpulse(mpNewtonBody, impulse, pos);
}
//-----------------------------------------------------------------------
void cPhysicsBodyNewton::SetEnabled(bool abEnabled) {
NewtonBodySetFreezeState(mpNewtonBody, !abEnabled);
}
bool cPhysicsBodyNewton::GetEnabled() const {
return NewtonBodyGetSleepState(mpNewtonBody) == 0 ? false : true;
}
//-----------------------------------------------------------------------
void cPhysicsBodyNewton::SetAutoDisable(bool abEnabled) {
NewtonBodySetAutoSleep(mpNewtonBody, abEnabled);
}
bool cPhysicsBodyNewton::GetAutoDisable() const {
return NewtonBodyGetAutoSleep(mpNewtonBody) == 0 ? false : true;
}
//-----------------------------------------------------------------------
#if 0
void cPhysicsBodyNewton::SetAutoDisableLinearThreshold(float afThresold) {
mfAutoDisableLinearThreshold = afThresold;
NewtonBodySetFreezeTreshold(mpNewtonBody, mfAutoDisableLinearThreshold,
mfAutoDisableAngularThreshold, mlAutoDisableNumSteps);
}
float cPhysicsBodyNewton::GetAutoDisableLinearThreshold() const {
return mfAutoDisableLinearThreshold;
}
//-----------------------------------------------------------------------
void cPhysicsBodyNewton::SetAutoDisableAngularThreshold(float afThresold) {
mfAutoDisableAngularThreshold = afThresold;
NewtonBodySetFreezeTreshold(mpNewtonBody, mfAutoDisableLinearThreshold,
mfAutoDisableAngularThreshold, mlAutoDisableNumSteps);
}
float cPhysicsBodyNewton::GetAutoDisableAngularThreshold() const {
return mfAutoDisableAngularThreshold;
}
//-----------------------------------------------------------------------
void cPhysicsBodyNewton::SetAutoDisableNumSteps(int anNum) {
mlAutoDisableNumSteps = anNum;
NewtonBodySetFreezeTreshold(mpNewtonBody, mfAutoDisableLinearThreshold,
mfAutoDisableAngularThreshold, mlAutoDisableNumSteps);
}
int cPhysicsBodyNewton::GetAutoDisableNumSteps() const {
return mlAutoDisableNumSteps;
}
#endif
//-----------------------------------------------------------------------
void cPhysicsBodyNewton::SetContinuousCollision(bool abOn) {
NewtonBodySetContinuousCollisionMode(mpNewtonBody, abOn ? 1 : 0);
}
bool cPhysicsBodyNewton::GetContinuousCollision() {
return NewtonBodyGetContinuousCollisionMode(mpNewtonBody) == 1 ? true : false;
}
//-----------------------------------------------------------------------
void cPhysicsBodyNewton::SetGravity(bool abEnabled) {
mbGravity = abEnabled;
}
bool cPhysicsBodyNewton::GetGravity() const {
return mbGravity;
}
//-----------------------------------------------------------------------
struct DrawParameters {
iLowLevelGraphics *lowLevelGraphics;
cColor drawColor;
};
///////////////////////////////////////////
static void RenderDebugPolygon(void *params, int alVertexCount, const dFloat *apFaceVertex, int alId) {
int i = alVertexCount - 1;
const DrawParameters *drawParams = static_cast<DrawParameters *>(params);
cVector3f vP0(apFaceVertex[i * 3 + 0], apFaceVertex[i * 3 + 1], apFaceVertex[i * 3 + 2]);
for (i = 0; i < alVertexCount; ++i) {
cVector3f vP1(apFaceVertex[i * 3 + 0], apFaceVertex[i * 3 + 1], apFaceVertex[i * 3 + 2]);
drawParams->lowLevelGraphics->DrawLine(vP0, vP1, drawParams->drawColor);
vP0 = vP1;
}
}
////////////////////////////////////////////
void cPhysicsBodyNewton::RenderDebugGeometry(iLowLevelGraphics *apLowLevel, const cColor &aColor) {
DrawParameters params{apLowLevel, aColor};
NewtonCollisionForEachPolygonDo(NewtonBodyGetCollision(mpNewtonBody), GetLocalMatrix().GetTranspose().v, RenderDebugPolygon, static_cast<void *>(&params));
}
//-----------------------------------------------------------------------
void cPhysicsBodyNewton::ClearForces() {
mvTotalForce = cVector3f(0, 0, 0);
mvTotalTorque = cVector3f(0, 0, 0);
}
//-----------------------------------------------------------------------
//////////////////////////////////////////////////////////////////////////
// STATIC NEWTON CALLBACKS
//////////////////////////////////////////////////////////////////////////
//-----------------------------------------------------------------------
void cPhysicsBodyNewton::OnTransformCallback(const NewtonBody *apBody, const dFloat *apMatrix, int32) {
cPhysicsBodyNewton *pRigidBody = (cPhysicsBodyNewton *)NewtonBodyGetUserData(apBody);
pRigidBody->m_mtxLocalTransform.FromTranspose(apMatrix);
mbUseCallback = false;
pRigidBody->SetTransformUpdated(true);
mbUseCallback = true;
if (pRigidBody->mpNode)
pRigidBody->mpNode->SetMatrix(pRigidBody->m_mtxLocalTransform);
}
//-----------------------------------------------------------------------
int cPhysicsBodyNewton::BuoyancyPlaneCallback(const int32 alCollisionID, void *apContext,
const float *afGlobalSpaceMatrix, float *afGlobalSpacePlane) {
cPlanef surfacePlane = static_cast<cPhysicsBodyNewton *>(apContext)->mBuoyancy.mSurface;
afGlobalSpacePlane[0] = surfacePlane.a;
afGlobalSpacePlane[1] = surfacePlane.b;
afGlobalSpacePlane[2] = surfacePlane.c;
afGlobalSpacePlane[3] = surfacePlane.d;
return 1;
}
void cPhysicsBodyNewton::OnUpdateCallback(NewtonBody *apBody, float, int32) {
float fMass;
float fX, fY, fZ;
cPhysicsBodyNewton *pRigidBody = (cPhysicsBodyNewton *)NewtonBodyGetUserData(apBody);
if (pRigidBody->IsActive() == false)
return;
cVector3f vGravity = pRigidBody->mpWorld->GetGravity();
// Create some gravity
if (pRigidBody->mbGravity) {
NewtonBodyGetMassMatrix(apBody, &fMass, &fX, &fY, &fZ);
VEC3_CONST_ARRAY(force, (vGravity * fMass));
NewtonBodyAddForce(apBody, force);
}
// Create Buoyancy
if (pRigidBody->mBuoyancy.mbActive) {
VEC3_CONST_ARRAY(gravity, vGravity);
NewtonBodyAddBuoyancyForce(apBody,
pRigidBody->mBuoyancy.mfDensity,
pRigidBody->mBuoyancy.mfLinearViscosity,
pRigidBody->mBuoyancy.mfAngularViscosity,
gravity, BuoyancyPlaneCallback,
pRigidBody);
}
// Add forces from calls to Addforce(..), etc
VEC3_CONST_ARRAY(totForce, pRigidBody->mvTotalForce);
NewtonBodyAddForce(apBody, totForce);
VEC3_CONST_ARRAY(totTorque, pRigidBody->mvTotalTorque);
NewtonBodyAddTorque(apBody, totTorque);
// Check so that all speeds are within thresholds
// Linear
if (pRigidBody->mfMaxLinearSpeed > 0) {
cVector3f vVel = pRigidBody->GetLinearVelocity();
float fSpeed = vVel.Length();
if (fSpeed > pRigidBody->mfMaxLinearSpeed) {
vVel = cMath::Vector3Normalize(vVel) * pRigidBody->mfMaxLinearSpeed;
pRigidBody->SetLinearVelocity(vVel);
}
}
// Angular
if (pRigidBody->mfMaxAngularSpeed > 0) {
cVector3f vVel = pRigidBody->GetAngularVelocity();
float fSpeed = vVel.Length();
if (fSpeed > pRigidBody->mfMaxAngularSpeed) {
vVel = cMath::Vector3Normalize(vVel) * pRigidBody->mfMaxAngularSpeed;
pRigidBody->SetAngularVelocity(vVel);
}
}
// cVector3f vForce;
// NewtonBodyGetForce(apBody,vForce.v);
// Log("Engine force %s\n",pRigidBody->mvTotalForce.ToString().c_str());
// Log("Engine force %s, body force: %s \n",pRigidBody->mvTotalForce.ToString().c_str(),
// vForce.ToString().c_str());
}
//-----------------------------------------------------------------------
} // namespace hpl