Strange behavior of rigid bodies (video) C++

Post Reply
romashka911
Posts: 7
Joined: Sun Oct 07, 2018 12:03 pm

Strange behavior of rigid bodies (video) C++

Post by romashka911 »

Hi! I've already had similar problem, but it's new, and i don't know why... You can see it in the video https://youtu.be/IYQTlWocjcQ.
I have some structure:
there is class Model3D which has only it's vertices and indices.
Than, i have class WorldObject3D, which has the reference to Model3D instance, it's coordinates, rotation, their matrices, render function, set and get methods etc
The class World has vector of WorldObject3D like vector<WorldObject3D*> m_sceneObjects.
Class World has bullet initialization function and step function, which updates coordinates and rotation of each member of vector<WorldObject3D*> m_sceneObjects.
Class Engine has Update function, where i call world's step function.
Class Engine has render function, where i pass each m_sceneObjects member and render it.
So, some code:

WorldObject3D.h

Code: Select all

#ifndef _WORLD_OBJECT_3D_H
#define _WORLD_OBJECT_3D_H

#include "Model3D.h"
#include "ResourceShader.h"
#include "Structs.h"
#include "btBulletDynamicsCommon.h"

class WorldObject3D
{
private:
	Model3D* m_model;
	float posX, posY, posZ;
	float rotX, rotY, rotZ;
	D3DXMATRIX m_translateMatrix;
	D3DXMATRIX m_rotationMatrix[3];
	D3DXMATRIX m_worldMatrix;
	Material m_currMaterial;
	ResourceShader* m_shader;

	btRigidBody* m_body;
	btCompoundShape* m_compoundShape;
	vector<btConvexHullShape*> m_convexArray;
	float m_mass;
	float m_friction;
	float m_restitution;
	int m_type;
public:
	WorldObject3D();
	~WorldObject3D();

	void Render(D3DXMATRIX camera, D3DXMATRIX projection);
	void SetShader(ResourceShader* shader);
	void SetModel3D(Model3D* modelRef);
	void SetPosition(float X, float Y, float Z);
	void SetRotation(float X, float Y, float Z);
	void GetPosition(float& X, float& Y, float& Z);
	void GetRotation(float& X, float& Y, float& Z);
	string GetName();
	void SetType(int type);
	int GetType();
	void SetMass(float mass);
	float GetMass();
	void SetFriction(float friction);
	float GetFriction();
	void SetRestitution(float restitution);
	float GetRestitution();

	void InitializePhysics();
	btRigidBody* GetRigidBody();
};
#endif
WorldObject3D.cpp

Code: Select all

#include "WorldObject3D.h"
#include "Engine.h"

#define SPHERE_TYPE 0
#define MESH_TYPE 1
#define PLANE_TYPE 2

WorldObject3D::WorldObject3D()
{
	m_model = NULL;
	m_body = NULL;
	m_compoundShape = NULL;
	m_mass = 0;
	m_friction = 1.0f;
	m_restitution = 0.0f;
	m_shader = NULL;

	posX = 0;
	posY = 0;
	posZ = 0;
	rotX = 0;
	rotY = 0;
	rotZ = 0;
}

WorldObject3D::~WorldObject3D()
{
}

void WorldObject3D::Render(D3DXMATRIX camera, D3DXMATRIX projection)
{
	D3DXMatrixIdentity(&m_translateMatrix);
	D3DXMatrixTranslation(&m_translateMatrix, posX, posY, posZ);
	D3DXMatrixRotationX(&m_rotationMatrix[0], rotX);
	D3DXMatrixRotationY(&m_rotationMatrix[1], rotY);
	D3DXMatrixRotationZ(&m_rotationMatrix[2], rotZ);

	m_worldMatrix = m_translateMatrix * m_rotationMatrix[0] * m_rotationMatrix[1] * m_rotationMatrix[2];
	
	if (m_model->m_objModel->Subsets > 0)
	{
		for (int i = 0; i < m_model->m_objModel->Subsets; i++)
		{
			if (m_model->m_materials[m_model->m_objModel->SubsetMaterialID[i]].HasDiffTexture)
			{
				m_currMaterial.HasDiffuseTexture = true;
				m_currMaterial.DiffuseColor = D3DXVECTOR4(1.0f, 1.0f, 1.0f, 1.0f);
				ID3D11ShaderResourceView *view = Engine::GetEngine()->GetWorld()->GetRSM()->GetTextureVector()[m_model->m_materials[m_model->m_objModel->SubsetMaterialID[i]].DiffuseTextureID - 1]->GetTexture()->GetTexture();
				m_shader->GetShader()->SetShaderParameters(m_model->m_deviceContext, view, 1);
			}

			m_shader->GetShader()->SetShaderParameters(m_model->m_deviceContext, m_worldMatrix, camera, projection, m_currMaterial, Engine::GetEngine()->GetWorld()->GetLightVec()[0]->GetLightDesc());
			int indexStart = m_model->m_objModel->SubsetIndexStart[i];
			int indexDrawAmount = m_model->m_objModel->SubsetIndexStart[i + 1] - indexStart;
			m_model->m_vertexBuffer->SetShader(m_shader);
			m_model->m_vertexBuffer->Render(m_model->m_deviceContext, indexDrawAmount, indexStart);
		}
	}
}

void WorldObject3D::SetShader(ResourceShader* shader)
{
	m_shader = shader;
}

void WorldObject3D::SetModel3D(Model3D* modelRef)
{
	m_model = modelRef;
}

void WorldObject3D::SetPosition(float X, float Y, float Z)
{
	posX = X;
	posY = Y;
	posZ = Z;
}

void WorldObject3D::SetRotation(float X, float Y, float Z)
{
	rotX = X;
	rotY = Y;
	rotZ = Z;
}

void WorldObject3D::GetPosition(float& X, float& Y, float& Z)
{
	X = posX;
	Y = posY;
	Z = posZ;
}

void WorldObject3D::GetRotation(float& X, float& Y, float& Z)
{
	X = rotX;
	Y = rotY;
	Z = rotZ;
}

void WorldObject3D::SetType(int type)
{
	m_type = type;
}

int WorldObject3D::GetType()
{
	return m_type;
}

void WorldObject3D::SetMass(float mass)
{
	m_mass = mass;
}

float WorldObject3D::GetMass()
{
	return m_mass;
}

void WorldObject3D::SetFriction(float friction)
{
	m_friction = friction;
}

float WorldObject3D::GetFriction()
{
	return m_friction;
}

void WorldObject3D::SetRestitution(float restitution)
{
	m_restitution = restitution;
}

float WorldObject3D::GetRestitution()
{

	return m_restitution;
}

void WorldObject3D::InitializePhysics()
{
	if (m_type == SPHERE_TYPE)
	{
		float radius;
		float x, y, z;
		x = m_model->m_vertices[0].position.x;
		y = m_model->m_vertices[0].position.y;
		z = m_model->m_vertices[0].position.z;
		radius = max(x, y);
		if (radius == x)
			radius = max(x, z);
		else
			radius = max(y, z);
		btSphereShape* sphere = new btSphereShape(btScalar(radius));
		btTransform tr;
		tr.setIdentity();
		tr.setOrigin(btVector3(0, 0, 0));
		m_compoundShape = new btCompoundShape();
		m_compoundShape->addChildShape(tr, sphere);
	}
	else if (m_type == MESH_TYPE)
	{
		int sizeOfConvexHull = 5;
		for (size_t i = sizeOfConvexHull; i <= m_model->m_vertices.size(); i += sizeOfConvexHull)
		{
			if (i > m_model->m_vertices.size())
				i = m_model->m_vertices.size();

			m_convexArray.push_back(new btConvexHullShape());

			for (size_t j = i - 1; j >= i - sizeOfConvexHull; j--)
			{
				m_convexArray.back()->addPoint(btVector3(m_model->m_vertices[j].position.x, m_model->m_vertices[j].position.y, m_model->m_vertices[j].position.z));
			}
		}

		btTransform trans;
		trans.setIdentity();
		trans.setOrigin(btVector3(0, 0, 0));
		m_compoundShape = new btCompoundShape();
		for (size_t i = 0; i < m_convexArray.size(); i++)
			m_compoundShape->addChildShape(trans, m_convexArray[i]);
	}
	else if (m_type == PLANE_TYPE)
	{
		btStaticPlaneShape* plane = new btStaticPlaneShape(btVector3(m_model->m_vertices[0].normal.x, m_model->m_vertices[0].normal.y, m_model->m_vertices[0].normal.z), 0);
		btTransform tr;
		tr.setIdentity();
		tr.setOrigin(btVector3(0, 0, 0));
		m_compoundShape = new btCompoundShape();
		m_compoundShape->addChildShape(tr, plane);
	}
	btVector3 inertia = btVector3(0, 0, 0);
	if (m_mass != 0)
		m_compoundShape->calculateLocalInertia(m_mass, inertia);
	btTransform positionMatrix;
	positionMatrix.setIdentity();
	positionMatrix.setOrigin(btVector3(posX, posY, posZ));
	btQuaternion quat;
	quat.setEulerZYX(rotX, rotY, rotZ);
	positionMatrix.setRotation(quat);

	btMotionState* motionState = new btDefaultMotionState(positionMatrix);
	m_body = new btRigidBody(m_mass, motionState, m_compoundShape, inertia);
	m_body->setFriction(m_friction);
	m_body->setRestitution(m_restitution);
}

btRigidBody* WorldObject3D::GetRigidBody()
{
	return m_body;
}

string WorldObject3D::GetName()
{
	return m_model->GetName();
}
World.h

Code: Select all

#ifndef _WORLD_H
#define _WORLD_H

#include "Camera.h"
#include "Light.h"
#include "ResourceManager.h"
#include "WorldObject3D.h"
#include "btBulletDynamicsCommon.h"

class World
{
private:
	Camera* m_camera;

	vector<Light*> m_lights;
	ResourceManager* m_rsManager;
	btDiscreteDynamicsWorld* m_dynamicsWorld;
	btDefaultCollisionConfiguration* m_collisionConfiguration;
	btCollisionDispatcher* m_dispatcher;
	btBroadphaseInterface* m_overlappingPairCache;
	btSequentialImpulseConstraintSolver* m_solver;

	vector<WorldObject3D*> m_sceneObjects;
public:
	World();
	~World();
	void StepSimulation(float time);
	void InitDynamicsWorld();
	void AddLightSource(UINT type, D3DXVECTOR4 color, bool enabled, float intense, float range, float angle, D3DXVECTOR3 pos, D3DXVECTOR3 dir);
	bool AddObjectToScene(string name, int type, float mass = 0.0f, float friction = 1.0f, float restitution = 0.0f, float posX = 0.0f, float posY = 0.0f, float posZ = 0.0f, float rotX = 0.0f, float rotY = 0.0f, float rotZ = 0.0f);
	bool DeleteObjectFromScene(int ID);
	bool DeleteObjectFromScene(string name);
	vector<WorldObject3D*>& GetSceneObjectsVector();
	size_t GetSceneObjectsCount();
	btDiscreteDynamicsWorld* GetDynamicsWorld();
	ResourceManager* GetRSM();
	Camera* GetCamera();
	vector<Light*>& GetLightVec();
};
#endif
World.cpp

Code: Select all

#include "World.h"

World::World()
{
	m_camera = new Camera();
	m_rsManager = ResourceManager::GetInstance();
	m_dynamicsWorld = NULL;
	m_collisionConfiguration = NULL;
	m_dispatcher = NULL;
	m_overlappingPairCache = NULL;
	m_solver = NULL;
}

World::~World()
{
	if (m_camera)
	{
		delete m_camera;
		m_camera = 0;
	}
	if (m_rsManager)
	{
		delete m_rsManager;
		m_rsManager = 0;
	}
	for (int i = m_dynamicsWorld->getNumCollisionObjects() - 1; i >= 0; i--)
	{
		btCollisionObject* obj = m_dynamicsWorld->getCollisionObjectArray()[i];
		btRigidBody* body = btRigidBody::upcast(obj);
		if (body && body->getMotionState())
			delete body->getMotionState();
		m_dynamicsWorld->removeCollisionObject(obj);
		delete obj;
	}
	if (m_collisionConfiguration)
	{
		delete m_collisionConfiguration;
		m_collisionConfiguration = 0;
	}
	if (m_dispatcher)
	{
		delete m_dispatcher;
		m_dispatcher = 0;
	}
	if (m_overlappingPairCache)
	{
		delete m_overlappingPairCache;
		m_overlappingPairCache = 0;
	}
	if (m_solver)
	{
		delete m_solver;
		m_solver = 0;
	}
	if (m_dynamicsWorld)
	{
		delete m_dynamicsWorld;
		m_dynamicsWorld = 0;
	}
}

Camera* World::GetCamera()
{
	return m_camera;
}

ResourceManager* World::GetRSM()
{
	return m_rsManager;
}

vector<Light*>& World::GetLightVec()
{
	return m_lights;
}

void World::AddLightSource(UINT type, D3DXVECTOR4 color, bool enabled, float intense, float range, float angle, D3DXVECTOR3 pos, D3DXVECTOR3 dir)
{
	LightDesc desc;
	ZeroMemory(&desc, sizeof(LightDesc));
	desc.Color = color;
	desc.Enabled = enabled;
	desc.Intensity = intense;
	desc.Range = range;
	desc.SpotlightAngle = angle;
	desc.Type = type;
	desc.DirectionVS = dir;
	desc.PositionVS = pos;
	Light* light = new Light();
	light->SetLightDesc(desc);
	m_lights.push_back(light);
}

btDiscreteDynamicsWorld* World::GetDynamicsWorld()
{
	return m_dynamicsWorld;
}

void World::InitDynamicsWorld()
{
	m_collisionConfiguration = new btDefaultCollisionConfiguration();
	m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration);
	m_overlappingPairCache = new btDbvtBroadphase();
	m_solver = new btSequentialImpulseConstraintSolver;
	m_dynamicsWorld = new btDiscreteDynamicsWorld(m_dispatcher, m_overlappingPairCache, m_solver, m_collisionConfiguration);
	m_dynamicsWorld->setGravity(btVector3(0, -10, 0));
}

void World::StepSimulation(float time)
{
	m_dynamicsWorld->stepSimulation(time, 10);
	for (int i = 0; i < m_dynamicsWorld->getNumCollisionObjects(); i++)
	{
		if (m_dynamicsWorld->getNumCollisionObjects() != m_sceneObjects.size())
		{
			cout << "[World] - getNumCollisionObjects() != m_sceneObjects.size()" << endl;
			return;
		}
		btCollisionObject* obj = m_dynamicsWorld->getCollisionObjectArray()[i];
		btRigidBody* body = btRigidBody::upcast(obj);
		btTransform trans;

		if (body && body->getMotionState())
			body->getMotionState()->getWorldTransform(trans);
		else
			trans = obj->getWorldTransform();

		m_sceneObjects[i]->SetPosition(trans.getOrigin().getX(), trans.getOrigin().getY(), trans.getOrigin().getZ());
		btQuaternion quat = trans.getRotation();
		float X, Y, Z;
		quat.getEulerZYX(Z, Y, X);
		m_sceneObjects[i]->SetRotation(X, Y, Z);
	}
}

bool World::AddObjectToScene(string name, int type, float mass, float friction, float restitution, float posX, float posY, float posZ, float rotX, float rotY, float rotZ)
{
	if (GetRSM()->GetObjectsCount() == 0)
	{
		cout << "[World] - Error: there are no objects loaded by RSM" << endl;
		return false;
	}
	for (size_t i = 0; i < GetRSM()->GetObjectsCount(); i++)
	{
		if (strcmp(GetRSM()->GetObjectsVector()[i]->GetName().c_str(), name.c_str()) == 0)
		{
			WorldObject3D* obj = new WorldObject3D();
			obj->SetModel3D(GetRSM()->GetObjectsVector()[i]);
			obj->SetShader(GetRSM()->GetShadersVector()[0]);
			obj->SetPosition(posX, posY, posZ);
			obj->SetRotation(rotX, rotY, rotZ);
			obj->SetMass(mass);
			obj->SetFriction(friction);
			obj->SetRestitution(restitution);
			obj->SetType(type);
			obj->InitializePhysics();
			m_sceneObjects.push_back(obj);
			m_dynamicsWorld->addRigidBody(obj->GetRigidBody());
			cout << "[World] - Created object '" << name << "' ID: " << m_dynamicsWorld->getNumCollisionObjects() << endl;
			return true;
		}
	}
	cout << "[World] - Error: RSM has no object with name '" << name << "'" << endl;
	return false;
}

bool World::DeleteObjectFromScene(int ID)
{
	if (ID > m_sceneObjects.size())
	{
		cout << "[World] - Error: there is no object with ID " << ID << endl;
		return false;
	}
	else if (m_sceneObjects[ID] != 0)
	{
		m_dynamicsWorld->removeRigidBody(m_sceneObjects[ID]->GetRigidBody());
		m_sceneObjects.erase(m_sceneObjects.begin() + ID);
	}
	else
	{
		cout << "[World] - Error: there is no object with ID " << ID << endl;
		return false;
	}
	return true;
}

bool World::DeleteObjectFromScene(string name)
{
	int count = 0;
	for (size_t i = 0; i < m_sceneObjects.size(); i++)
	{
		if (m_sceneObjects[i]->GetName() == name)
		{
			m_dynamicsWorld->removeRigidBody(m_sceneObjects[i]->GetRigidBody());
			m_sceneObjects.erase(m_sceneObjects.begin() + i);
			count++;
		}
	}
	if (count == 0)
	{
		cout << "[World] - Error: there are no objects with name '" << name << "'" << endl;
		return false;
	}
	return true;
}

size_t World::GetSceneObjectsCount()
{
	return m_sceneObjects.size();
}

vector<WorldObject3D*>& World::GetSceneObjectsVector()
{
	return m_sceneObjects;
}
Engine.cpp Render() function:

Code: Select all

void Engine::Render()
{
	m_graphics->BeginScene(0.0f, 0.3f, 0.0f, 1.0f);
	for (size_t i = 0; i < m_world->GetSceneObjectsCount(); i++)
		m_world->GetSceneObjectsVector()[i]->Render(m_world->GetCamera()->GetViewMatrix(), m_world->GetCamera()->GetProjectionMatrix());
	m_graphics->EndScene();
}
DannyChapman
Posts: 84
Joined: Sun Jan 07, 2007 4:29 pm
Location: Oxford, England
Contact:

Re: Strange behavior of rigid bodies (video) C++

Post by DannyChapman »

Just from the video - it looks like you're rendering your shapes offset (a long way) sideways from the actual physical objects. That would explain why initially they look OK when they fall - the real stack would be some distance to the side. But when the objects start falling and rotating, your rendered shapes will rotate around the offset physical objects.

I would look hard at the matrix maths code in WorldObject3D::Render and consider whether everything in there is correct - including the order of multiplication. You seem to be going via Euler angles - not sure that's a good idea.
romashka911
Posts: 7
Joined: Sun Oct 07, 2018 12:03 pm

Re: Strange behavior of rigid bodies (video) C++

Post by romashka911 »

DannyChapman wrote: Sat Oct 13, 2018 12:21 pm Just from the video - it looks like you're rendering your shapes offset (a long way) sideways from the actual physical objects. That would explain why initially they look OK when they fall - the real stack would be some distance to the side. But when the objects start falling and rotating, your rendered shapes will rotate around the offset physical objects.

I would look hard at the matrix maths code in WorldObject3D::Render and consider whether everything in there is correct - including the order of multiplication. You seem to be going via Euler angles - not sure that's a good idea.
Thank you for your reply.
I think, if i had problems because of Euler angles, it would look like correct position, but wrong angles. I had some problem with it, but in my previous topic i've solved it.
But here we can see also wrong position - bodies fly chaotically.
I suspect a mismatch between m_sceneObjects and btCollisionObjectArray in World::StepSimulation()... But i can't find certain mistake...
romashka911
Posts: 7
Joined: Sun Oct 07, 2018 12:03 pm

Re: Strange behavior of rigid bodies (video) C++

Post by romashka911 »

Can anybody help?
DannyChapman
Posts: 84
Joined: Sun Jan 07, 2007 4:29 pm
Location: Oxford, England
Contact:

Re: Strange behavior of rigid bodies (video) C++

Post by DannyChapman »

You didn't respond to my suggestion to check your maths.

The best ways to do that are to
1. Look at it and check that your code matches what you wrote down on paper
2. Feed it simple input, and check that the output is what you expect.

In this case, the output of your maths is a rendered object, and since Bullet works (unless you did something crazy to it) the problem in one of three places:
1. How you set the scene up in Bullet
2. How you convert from that to the rendering.
3. How you do the rendering.

From the video (and the fact that you even have Euler angles in there!) I'd suspect the second.

So - to debug that you can simply:
1. Render a transform/grid/reference object at the origin (so you can relate everything else to that).
2. Place Bullet physics objects at known positions. Disable the dynamics for the moment (e.g. make them kinematic) but keep the simulation.

For example:
* Does a box placed at the origin render at the origin?
* Does a box placed at (2, 0, 0) render at two units in the world x direction
* Does a box with Euler angles (30, 0, 0) [in degrees] render at the origin, rotated around the x axis by 30 degrees.
* Does a box placed at (2, 0, 0) and Eulers (30, 0, 0) render where you expect it.

Etc etc. just keep going until something doesn't work. Now you've found your problem.
romashka911
Posts: 7
Joined: Sun Oct 07, 2018 12:03 pm

Re: Strange behavior of rigid bodies (video) C++

Post by romashka911 »

DannyChapman wrote: Mon Oct 15, 2018 9:36 pm You didn't respond to my suggestion to check your maths.

The best ways to do that are to
1. Look at it and check that your code matches what you wrote down on paper
2. Feed it simple input, and check that the output is what you expect.

In this case, the output of your maths is a rendered object, and since Bullet works (unless you did something crazy to it) the problem in one of three places:
1. How you set the scene up in Bullet
2. How you convert from that to the rendering.
3. How you do the rendering.

From the video (and the fact that you even have Euler angles in there!) I'd suspect the second.

So - to debug that you can simply:
1. Render a transform/grid/reference object at the origin (so you can relate everything else to that).
2. Place Bullet physics objects at known positions. Disable the dynamics for the moment (e.g. make them kinematic) but keep the simulation.

For example:
* Does a box placed at the origin render at the origin?
* Does a box placed at (2, 0, 0) render at two units in the world x direction
* Does a box with Euler angles (30, 0, 0) [in degrees] render at the origin, rotated around the x axis by 30 degrees.
* Does a box placed at (2, 0, 0) and Eulers (30, 0, 0) render where you expect it.

Etc etc. just keep going until something doesn't work. Now you've found your problem.
Thanks fo reply.
So, i've taken one cube, made him kinematic and tried to change it's position and rotation, and i have very interesting results...

Position [0, 0, 0] Rotation [0, 0, 0]
Image
Position [0, 10, 0] Rotation [0, 0, 0]
Image

Then i've tried to rotate it and noticed, that cube has gone under the 0 Y-coordinate, this is the problem
Position [0, 10, 0] Rotation [10, 0, 0]
Image
Position [0, 10, 0] Rotation [30, 0, 0]
Image
Position [0, 10, 0] Rotation [60, 0, 0]
Image
DannyChapman
Posts: 84
Joined: Sun Jan 07, 2007 4:29 pm
Location: Oxford, England
Contact:

Re: Strange behavior of rigid bodies (video) C++

Post by DannyChapman »

Looks like your multiplication order is the wrong way round
romashka911
Posts: 7
Joined: Sun Oct 07, 2018 12:03 pm

Re: Strange behavior of rigid bodies (video) C++

Post by romashka911 »

DannyChapman wrote: Tue Oct 16, 2018 3:05 pm Looks like your multiplication order is the wrong way round
Thank you very much!
this code

Code: Select all

m_worldMatrix = m_translateMatrix * m_rotationMatrix[0] * m_rotationMatrix[1] * m_rotationMatrix[2];
i replaced with

Code: Select all

m_worldMatrix = m_rotationMatrix[0] * m_rotationMatrix[1] * m_rotationMatrix[2];
	m_worldMatrix *= m_translateMatrix;
Now it works correctly!
Post Reply