1
The code snippet shown below is used for coordinating multiple objects to move around without bumping into each other.
You may assume the pathfinder will give a straight path from one point to another.
The problem I am having is the orientation of the objects. The outcome of the code below make the objects swing left and right randomly.
My intention is the object must follow the "direction" of the pathfinder.
I should highlight this line
float angle = atan2(m_velocity.z, -m_velocity.x) + D3DX_PI * 0.5f;
The output is out of expected.
What change(s) should I make in order to achieve the original intention?
#include "stdafx.h"
#include "CrowdEntity.h"
#include "Crowd.h"
#include "Pathfinder\Thread.h"
extern IDirect3DDevice9 *g_pDevice;
extern ID3DXEffect *g_pEffect;
HANDLE g_hMutex;
void CrowdEntity::PathFind(D3DXVECTOR3 s, D3DXVECTOR3 d, std::vector<D3DXVECTOR3>& p)
{
static HANDLE hEvent;
hEvent = CreateEvent(NULL, FALSE, FALSE, NULL);
g_hMutex = CreateMutex(NULL, FALSE, NULL);
D3DXVECTOR2 s1,d1;
s1 = D3DXVECTOR2(s.x, s.z);
d1 = D3DXVECTOR2(d.x, d.z);
PFThread thr(1, hEvent, s1, d1, 0.78, false);
thr.SetRunning(true);
thr.GetAStar()->SetClearance(5);
thr.run();
std::vector<D3DXVECTOR2> path;
path = thr.GetPath();
for (int i = path.size()-1; i > 0; i--)
{
D3DXVECTOR3 v;
v.x = path[i].x;
v.y = 0;
v.z = path[i].y;
p.push_back(v);
}
}
CrowdEntity::CrowdEntity(Crowd *pCrowd)
{
//if (cm_CBMesh == NULL)
//{
cm_CBMesh = new CMesh();
cm_CBMesh->Load("cb.dat");
//}
m_pCrowd = pCrowd;
m_position = GetRandomLocation();
m_goal = GetRandomLocation();
PathFind(m_position, m_goal, m_Path);
if (m_Path.size() > 0)
m_position = m_Path[0];
D3DXVec3Normalize(&m_velocity, &m_position);
m_count = 0;
// m_pAnimController = sm_OperatorMesh->GetAnimController();
/// ID3DXAnimationSet *anim = NULL;
// m_pAnimController->GetAnimationSet(0, &anim);
// m_pAnimController->SetTrackAnimationSet(0, anim);
// anim->Release();
}
CrowdEntity::~CrowdEntity()
{
if (m_pAnimController != NULL)
m_pAnimController->Release();
}
void CrowdEntity::Update(float deltaTime)
{
const float ENTITY_INFLUENCE_RADIUS = 100.0f;
const float NEIGHBOR_REPULSION = 50.0f;
const float ENTITY_SPEED = 200.0f;
const float ENTITY_SIZE = 100.0f;
//Force towards goal
if (m_count < m_Path.size())
{
// m_position derived from getRandomPosition();
// now is dependent on m_Path[m_count];
m_position = m_Path[m_count];
D3DXVECTOR3 forceToGoal = m_goal - m_position;
//Has goal been reached?
if(D3DXVec3Length(&forceToGoal) < ENTITY_INFLUENCE_RADIUS)
{
//Pick a new random goal
m_goal = GetRandomLocation();
PathFind(m_position, m_goal, m_Path);
m_count = 0;
m_position = m_Path[0];
}
D3DXVec3Normalize(&forceToGoal, &forceToGoal);
//Get neighbors
vector<CrowdEntity*> neighbors;
m_pCrowd->GetNeighbors(this, ENTITY_INFLUENCE_RADIUS, neighbors);
//Avoid bumping into close neighbors
D3DXVECTOR3 forceAvoidNeighbors(0.0f, 0.0f, 0.0f);
for(int i=0; i<(int)neighbors.size(); i++)
{
D3DXVECTOR3 toNeighbor = neighbors[i]->m_position - m_position;
float distToNeighbor = D3DXVec3Length(&toNeighbor);
toNeighbor.y = 0.0f;
float force = 1.0f - (distToNeighbor / ENTITY_INFLUENCE_RADIUS);
forceAvoidNeighbors += -toNeighbor * NEIGHBOR_REPULSION * force;
//Force move intersecting entities
if(distToNeighbor < ENTITY_SIZE)
{
D3DXVECTOR3 center = (m_position + neighbors[i]->m_position) * 0.5f;
D3DXVECTOR3 dir = center - m_position;
D3DXVec3Normalize(&dir, &dir);
//Force move both entities
m_position = center - dir * ENTITY_SIZE * 0.5f;
neighbors[i]->m_position = center + dir * ENTITY_SIZE * 0.5f;
}
}
//Sum up forces
D3DXVECTOR3 acc = forceToGoal + forceAvoidNeighbors;
D3DXVec3Normalize(&acc, &acc);
//Update velocity & position
m_velocity += acc * deltaTime;
D3DXVec3Normalize(&m_velocity, &m_velocity);
m_position += m_velocity * ENTITY_SPEED * deltaTime;
m_count++;
}
//Update animation
// m_pAnimController->AdvanceTime(deltaTime, NULL);
}
void CrowdEntity::Render()
{
D3DXMATRIX world, pos, rot;
//Position
//D3DXMatrixTranslation(&pos, m_position.x, m_position.y, m_position.z);
D3DXMatrixTransformation(&pos, NULL, NULL, &D3DXVECTOR3(0.5,0.5,0.5), NULL, NULL, &m_position);
//Orientation
float angle = atan2(m_velocity.z, -m_velocity.x) + D3DX_PI * 0.5f;
D3DXMatrixRotationY(&rot, angle);
//Set pose and render
world = rot * pos;
g_pDevice->SetTransform(D3DTS_WORLD, &world);
cm_CBMesh->Render();
}
D3DXVECTOR3 CrowdEntity::GetRandomLocation()
{
return D3DXVECTOR3(((rand()%1000 / 10.0f)) * 20.0f - 10.0f,
0.0f,
((rand()%1000 / 10.0f)) * 20.0f - 10.0f);// /1000.0f
}
