SPGrid.cpp 21 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636637638639640641642643644645646647648649650651652653654655656657658659660661662663664665666667668669670671672673674675676677678679680681682683684685686687688689690691692693694695696697698699700701702703704705706707708709710711712713714715716717718719720721722723724725726727728729730731732733734735736737738739740
  1. /*
  2. EQ2Emulator: Everquest II Server Emulator
  3. Copyright (C) 2007 EQ2EMulator Development Team (http://www.eq2emulator.net)
  4. This file is part of EQ2Emulator.
  5. EQ2Emulator is free software: you can redistribute it and/or modify
  6. it under the terms of the GNU General Public License as published by
  7. the Free Software Foundation, either version 3 of the License, or
  8. (at your option) any later version.
  9. EQ2Emulator is distributed in the hope that it will be useful,
  10. but WITHOUT ANY WARRANTY; without even the implied warranty of
  11. MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
  12. GNU General Public License for more details.
  13. You should have received a copy of the GNU General Public License
  14. along with EQ2Emulator. If not, see <http://www.gnu.org/licenses/>.
  15. */
  16. #include "SPGrid.h"
  17. #include "../../common/Log.h"
  18. #include <cmath>
  19. SPGrid::SPGrid(string file, int32 cellSize) {
  20. m_ZoneFile = file;
  21. m_CellSize = cellSize;
  22. m_MinX = 0;
  23. m_MinZ = 0;
  24. m_MaxX = 0;
  25. m_MaxZ = 0;
  26. m_NumCellsX = 0;
  27. m_NumCellsZ = 0;
  28. m_NumFaceCellsX = 0;
  29. m_NumFaceCellsZ = 0;
  30. }
  31. SPGrid::~SPGrid() {
  32. vector<Cell>::iterator CellItr;
  33. map<int32, vector<Face*> >::iterator MapItr;
  34. vector<Face*>::iterator FaceItr;
  35. // Loop through the vector of cells
  36. /*for (CellItr = m_Cells.begin(); CellItr != m_Cells.end(); CellItr++) {
  37. // Loop through the map of vertices on this cell
  38. for (MapItr = (*CellItr).FaceList.begin(); MapItr != (*CellItr).FaceList.end(); MapItr++) {
  39. // Loop through the vector of faces in the map and delete the pointers
  40. for (FaceItr = (*MapItr).second.begin(); FaceItr != (*MapItr).second.end(); FaceItr++) {
  41. safe_delete((*FaceItr));
  42. }
  43. }
  44. }*/
  45. }
  46. bool SPGrid::Init() {
  47. // Make sure we have a zone file
  48. if (m_ZoneFile.empty()) {
  49. LogWrite(ZONE__ERROR, 0, "SPGrid", "SPGrid::Init() m_ZoneFile is empty.");
  50. return false;
  51. }
  52. // Make sure we have a cell size
  53. if (m_CellSize == 0)
  54. m_CellSize = CELLSIZEDEFAULT;
  55. // Open the map file for this zone
  56. string filePath = "Maps/" + m_ZoneFile + ".EQ2Map";
  57. FILE* file = fopen(filePath.c_str(), "rb");
  58. if (file == nullptr) {
  59. LogWrite(ZONE__WARNING, 0, "SPGrid", "SPGrid::Init() unable to open the map file for %s. (zoneserver will continue to run fine without it)", m_ZoneFile.c_str());
  60. return false;
  61. }
  62. // Read the string for the zone file name this was created for
  63. int8 strSize;
  64. char name[256];
  65. fread(&strSize, sizeof(int8), 1, file);
  66. LogWrite(ZONE__DEBUG, 0, "SPGrid", "strSize = %u", strSize);
  67. size_t len = fread(&name, sizeof(char), strSize, file);
  68. name[len] = '\0';
  69. LogWrite(ZONE__DEBUG, 0, "SPGrid", "name = %s", name);
  70. string fileName(name);
  71. std::size_t found = fileName.find(m_ZoneFile);
  72. // Make sure file contents are for the correct zone
  73. if (found == std::string::npos) {
  74. fclose(file);
  75. LogWrite(ZONE__ERROR, 0, "SPGrid", "SPGrid::Init() map contents (%s) do not match its name (%s).", &name, m_ZoneFile.c_str());
  76. return false;
  77. }
  78. // Read the min bounds
  79. fread(&m_MinX, sizeof(float), 1, file);
  80. fread(&m_MinZ, sizeof(float), 1, file);
  81. LogWrite(ZONE__DEBUG, 0, "SPGrid", "minx = %f, minz = %f", m_MinX, m_MinZ);
  82. // Read the max bounds
  83. fread(&m_MaxX, sizeof(float), 1, file);
  84. fread(&m_MaxZ, sizeof(float), 1, file);
  85. LogWrite(ZONE__DEBUG, 0, "SPGrid", "maxx = %f, maxz = %f", m_MaxX, m_MaxZ);
  86. // Calculate how many cells we need
  87. // in both the X and Z direction
  88. float width = m_MaxX - m_MinX;
  89. float height = m_MaxZ - m_MinZ;
  90. m_NumCellsX = ceil(width / m_CellSize);
  91. m_NumCellsZ = ceil(height / m_CellSize);
  92. LogWrite(ZONE__DEBUG, 0, "SPGrid", "CellSize = %u, x cells = %u, z cells = %u", m_CellSize, m_NumCellsX, m_NumCellsZ);
  93. // Allocate all the cells
  94. m_Cells.resize(m_NumCellsZ * m_NumCellsX);
  95. m_NumFaceCellsX = ceil(width / FACECELLSIZEDEFAULT);
  96. m_NumFaceCellsZ = ceil(height / FACECELLSIZEDEFAULT);
  97. m_FaceCells.resize(m_NumFaceCellsX * m_NumFaceCellsZ);
  98. // Read the number of grids
  99. int32 NumGrids;
  100. fread(&NumGrids, sizeof(int32), 1, file);
  101. LogWrite(ZONE__DEBUG, 0, "SPGrid", "NumGrids = %u", NumGrids);
  102. // Loop through the grids loading the face list
  103. for (int32 i = 0; i < NumGrids; i++) {
  104. // Read the grid id
  105. int32 GridID;
  106. fread(&GridID, sizeof(int32), 1, file);
  107. LogWrite(ZONE__DEBUG, 0, "SPGrid", "GridID = %u", GridID);
  108. // Read the number of vertices
  109. int32 NumFaces;
  110. fread(&NumFaces, sizeof(int32), 1, file);
  111. LogWrite(ZONE__DEBUG, 0, "SPGrid", "NumFaces = %u", NumFaces);
  112. // Loop through the vertices list reading
  113. // 3 at a time to creat a triangle (face)
  114. for (int32 y = 0; y < NumFaces; ) {
  115. // Each vertex need an x,y,z coordinate and
  116. // we will be reading 3 to create the face
  117. float x1, x2, x3;
  118. float y1, y2, y3;
  119. float z1, z2, z3;
  120. // Read the first vertex
  121. fread(&x1, sizeof(float), 1, file);
  122. fread(&y1, sizeof(float), 1, file);
  123. fread(&z1, sizeof(float), 1, file);
  124. y++;
  125. // Read the second vertex
  126. fread(&x2, sizeof(float), 1, file);
  127. fread(&y2, sizeof(float), 1, file);
  128. fread(&z2, sizeof(float), 1, file);
  129. y++;
  130. // Read the third (final) vertex
  131. fread(&x3, sizeof(float), 1, file);
  132. fread(&y3, sizeof(float), 1, file);
  133. fread(&z3, sizeof(float), 1, file);
  134. y++;
  135. // Create the face and add it to the grid
  136. Face* face = new Face;
  137. face->Vertex1[0] = x1;
  138. face->Vertex1[1] = y1;
  139. face->Vertex1[2] = z1;
  140. face->Vertex2[0] = x2;
  141. face->Vertex2[1] = y2;
  142. face->Vertex2[2] = z2;
  143. face->Vertex3[0] = x3;
  144. face->Vertex3[1] = y3;
  145. face->Vertex3[2] = z3;
  146. AddFace(face, GridID);
  147. }
  148. }
  149. fclose(file);
  150. /*map<int32, vector<Face*> >::iterator itr;
  151. vector<Face*>::iterator itr2;
  152. for (int32 i = 0; i < m_Cells.size(); i++) {
  153. Cell& cell = m_Cells[i];
  154. for (itr = cell.FaceList.begin(); itr != cell.FaceList.end(); itr++) {
  155. float min_x = 0.0f;
  156. float min_y = 0.0f;
  157. float min_z = 0.0f;
  158. float max_x = 0.0f;
  159. float max_y = 0.0f;
  160. float max_z = 0.0f;
  161. for (itr2 = (*itr).second.begin(); itr2 != (*itr).second.end(); itr2++) {
  162. Face* face = (*itr2);
  163. if (min_x == 0.0f || face->Vertex1[0] < min_x)
  164. min_x = face->Vertex1[0];
  165. if (face->Vertex2[0] < min_x)
  166. min_x = face->Vertex2[0];
  167. if (face->Vertex3[0] < min_x)
  168. min_x = face->Vertex3[0];
  169. if (min_y == 0.0f || face->Vertex1[1] < min_y)
  170. min_y = face->Vertex1[1];
  171. if (face->Vertex2[1] < min_y)
  172. min_y = face->Vertex2[1];
  173. if (face->Vertex3[1] < min_y)
  174. min_y = face->Vertex3[1];
  175. if (min_z == 0.0f || face->Vertex1[2] < min_z)
  176. min_z = face->Vertex1[2];
  177. if (face->Vertex2[2] < min_z)
  178. min_z = face->Vertex2[2];
  179. if (face->Vertex3[2] < min_z)
  180. min_z = face->Vertex3[2];
  181. // Max bounds
  182. if (max_x == 0.0f || face->Vertex1[0] > max_x)
  183. max_x = face->Vertex1[0];
  184. if (face->Vertex2[0] > max_x)
  185. max_x = face->Vertex2[0];
  186. if (face->Vertex3[0] > max_x)
  187. max_x = face->Vertex3[0];
  188. if (max_y == 0.0f || face->Vertex1[1] > max_y)
  189. max_y = face->Vertex1[1];
  190. if (face->Vertex2[1] > max_y)
  191. max_y = face->Vertex2[1];
  192. if (face->Vertex3[1] > max_y)
  193. max_y = face->Vertex3[1];
  194. if (max_z == 0.0f || face->Vertex1[2] > max_z)
  195. max_z = face->Vertex1[2];
  196. if (face->Vertex2[2] > max_z)
  197. max_z = face->Vertex2[2];
  198. if (face->Vertex3[2] > max_z)
  199. max_z = face->Vertex3[2];
  200. }
  201. GridBounds* bounds = new GridBounds;
  202. bounds->MinBounds[0] = min_x;
  203. bounds->MinBounds[1] = min_y;
  204. bounds->MinBounds[2] = min_z;
  205. bounds->MaxBounds[0] = max_x;
  206. bounds->MaxBounds[1] = max_y;
  207. bounds->MaxBounds[2] = max_z;
  208. cell.GridBounds[(*itr).first] = bounds;
  209. }
  210. }*/
  211. return true;
  212. }
  213. Cell* SPGrid::GetCell(int32 x, int32 z) {
  214. if (x >= m_NumCellsX)
  215. x = m_NumCellsX - 1;
  216. if (z >= m_NumCellsZ)
  217. z = m_NumCellsZ - 1;
  218. return &m_Cells[z * m_NumCellsX + x];
  219. }
  220. Cell* SPGrid::GetCell(float x, float z) {
  221. // As cell grid coordinates are all positive we need to
  222. // modify the coordinates by subtracting the min bounds
  223. float newX = x - m_MinX;
  224. float newZ = z - m_MinZ;
  225. // Get the cell coordinates by doing int division
  226. // with the modified coordinates and the cell size
  227. int32 CellX = (int32)(newX / m_CellSize);
  228. int32 CellZ = (int32)(newZ / m_CellSize);
  229. return GetCell(CellX, CellZ);
  230. }
  231. FaceCell* SPGrid::GetFaceCell(int32 x, int32 z) {
  232. if (x >= m_NumFaceCellsX)
  233. x = m_NumFaceCellsX - 1;
  234. if (z >= m_NumFaceCellsZ)
  235. z = m_NumFaceCellsZ - 1;
  236. return &m_FaceCells[z * m_NumFaceCellsX + x];
  237. }
  238. FaceCell* SPGrid::GetFaceCell(float x, float z) {
  239. // As cell grid coordinates are all positive we need to
  240. // modify the coordinates by subtracting the min bounds
  241. float newX = x - m_MinX;
  242. float newZ = z - m_MinZ;
  243. // Get the cell coordinates by doing int division
  244. // with the modified coordinates and the cell size
  245. int32 CellX = (int32)(newX / FACECELLSIZEDEFAULT);
  246. int32 CellZ = (int32)(newZ / FACECELLSIZEDEFAULT);
  247. return GetFaceCell(CellX, CellZ);
  248. }
  249. void SPGrid::AddFace(Face* face, int32 grid) {
  250. // As each face has three vertices we will need to check the cell
  251. // for all of them and add the face to each cell that it is within
  252. face->grid_id = grid;
  253. // Get the cell at the first vertex position (X and Z, Y is vertical in EQ2)
  254. // as this is the first check we will add it to this cell and compare it
  255. // to the other two cells we get for the other two verticies
  256. FaceCell* cell = GetFaceCell(face->Vertex1[0], face->Vertex1[2]);
  257. cell->FaceList[grid].push_back(face);
  258. // Get the cells for the other two verticies and compare
  259. FaceCell* cell2 = GetFaceCell(face->Vertex2[0], face->Vertex2[2]);
  260. FaceCell* cell3 = GetFaceCell(face->Vertex3[0], face->Vertex3[2]);
  261. // If cell 2 is not the same cell as the original cell then add the face to cell2
  262. if (cell2 != cell)
  263. cell2->FaceList[grid].push_back(face);
  264. // If cell 3 is not the same as the original cell AND not the same as cell 2 then add the face to cell 3
  265. if (cell3 != cell && cell3 != cell2)
  266. cell3->FaceList[grid].push_back(face);
  267. }
  268. float rayIntersectsTriangle(float *p, float *d, float *v0, float *v1, float *v2);
  269. int32 SPGrid::GetGridID(Spawn * spawn) {
  270. FaceCell* cell = GetFaceCell(spawn->GetX(), spawn->GetZ());
  271. /*if (cell->GridBounds.size() == 1)
  272. return cell->FaceList.begin()->first;*/
  273. // Create the starting point for the trace
  274. float point[3];
  275. point[0] = spawn->GetX();
  276. point[1] = spawn->GetY() + 3.0f; // Small bump to make sure we are above ground when we do the trace
  277. point[2] = spawn->GetZ();
  278. // Create the direction for the trace, as we want what
  279. // is below it will just be -1 in the y direction
  280. float direction[3];
  281. direction[0] = 0.0f;
  282. direction[1] = -1.0f;
  283. direction[2] = 0.0f;
  284. float MinDistance = 0.0f;
  285. int32 Grid = 0;
  286. /*map<int32, GridBounds*>::iterator itr;
  287. for (itr = cell->GridBounds.begin(); itr != cell->GridBounds.end(); itr++) {
  288. GridBounds* bounds = (*itr).second;
  289. if (point[0] >= bounds->MinBounds[0] && point[1] >= bounds->MinBounds[1] && point[2] >= bounds->MinBounds[2]
  290. && point[0] <= bounds->MaxBounds[0] && point[1] <= bounds->MaxBounds[1] && point[2] <= bounds->MaxBounds[2]) {
  291. vector<Face*>::iterator itr2;
  292. for (itr2 = cell->FaceList[(*itr).first].begin(); itr2 != cell->FaceList[(*itr).first].end(); itr2++) {
  293. Face* face = *itr2;
  294. float distance;
  295. if ((distance = rayIntersectsTriangle(point, direction, face->Vertex1, face->Vertex2, face->Vertex3)) != 0) {
  296. if (MinDistance == 0.0f || distance < MinDistance) {
  297. MinDistance = distance;
  298. Grid = (*itr).first;
  299. }
  300. }
  301. }
  302. }
  303. }*/
  304. map<int32, vector<Face*> >::iterator mapitr;
  305. for (mapitr = cell->FaceList.begin(); mapitr != cell->FaceList.end(); mapitr++) {
  306. vector<Face*>::iterator itr;
  307. for (itr = (*mapitr).second.begin(); itr != (*mapitr).second.end(); itr++) {
  308. Face* face = *itr;
  309. float distance;
  310. if ((distance = rayIntersectsTriangle(point, direction, face->Vertex1, face->Vertex2, face->Vertex3)) != 0) {
  311. if (MinDistance == 0.0f || distance < MinDistance) {
  312. MinDistance = distance;
  313. Grid = (*mapitr).first;
  314. }
  315. }
  316. }
  317. }
  318. return Grid;
  319. }
  320. void SPGrid::AddSpawn(Spawn * spawn) {
  321. Cell* cell = GetCell(spawn->GetX(), spawn->GetZ());
  322. AddSpawn(spawn, cell);
  323. }
  324. void SPGrid::AddSpawn(Spawn * spawn, Cell * cell) {
  325. cell->SpawnList.push_back(spawn);
  326. spawn->Cell_Info.CurrentCell = cell;
  327. spawn->Cell_Info.CellListIndex = cell->SpawnList.size() - 1;
  328. }
  329. void SPGrid::RemoveSpawnFromCell(Spawn * spawn) {
  330. if (spawn->Cell_Info.CurrentCell) {
  331. vector<Spawn*>& spawns = spawn->Cell_Info.CurrentCell->SpawnList;
  332. // Only do the vector swap if the vector has more than 1 spawn in it
  333. if (spawns.size() > 1) {
  334. // Swap the last spawn in this list to our position and update its stored index to match its new index
  335. spawns[spawn->Cell_Info.CellListIndex] = spawns.back();
  336. spawns[spawn->Cell_Info.CellListIndex]->Cell_Info.CellListIndex = spawn->Cell_Info.CellListIndex;
  337. }
  338. // Remove the last spawn from the list which should now be the spawn passed as a parameter
  339. spawns.pop_back();
  340. // Reset the spawns CellInfo to default values now that it is no longer in a cell
  341. spawn->Cell_Info.CellListIndex = -1;
  342. spawn->Cell_Info.CurrentCell = nullptr;
  343. }
  344. }
  345. float SPGrid::GetBestY(float x, float y, float z)
  346. {
  347. float temp_y = 0;
  348. float best_y = 999999.0f;
  349. FaceCell* startCell = GetFaceCell(x, z);
  350. float tmpY = y + 0.5f;
  351. float point[3];
  352. point[0] = x;
  353. point[1] = tmpY; // Small bump to make sure we are above ground when we do the trace
  354. point[2] = z;
  355. float MinDistance = 0.0f;
  356. // Create the direction for the trace, as we want what
  357. // is below it will just be -1 in the y direction
  358. float direction[3];
  359. direction[0] = 0.0f;
  360. direction[1] = -1.0f;
  361. direction[2] = 0.0f;
  362. Face* lastFace = 0;
  363. int32 Grid = 0;
  364. float BestZ = -999999.0f;
  365. map<int32, vector<Face*> >::iterator mapitr;
  366. for (mapitr = startCell->FaceList.begin(); mapitr != startCell->FaceList.end(); mapitr++) {
  367. vector<Face*>::iterator itr;
  368. for (itr = (*mapitr).second.begin(); itr != (*mapitr).second.end(); itr++) {
  369. Face* face = *itr;
  370. float distance;
  371. if ((distance = rayIntersectsTriangle(point, direction, face->Vertex1, face->Vertex2, face->Vertex3)) != 0) {
  372. if (MinDistance == 0.0f || distance < MinDistance) {
  373. BestZ = face->Vertex2[1];
  374. MinDistance = distance;
  375. lastFace = face;
  376. Grid = (*mapitr).first;
  377. }
  378. }
  379. }
  380. }
  381. printf("GridID: %i, BestZ: %f yIn:% f\n", Grid, BestZ, y);
  382. float endY = 999999.0f;
  383. if (lastFace)
  384. {
  385. /* for (int i = 0; i < 3; i++)
  386. {
  387. for (int z = 0; z < 3; z++)
  388. {
  389. if (i == 0)
  390. printf("Face%i-%i: %f\n", i, z, lastFace->Vertex1[z]);
  391. else if (i == 1)
  392. printf("Face%i-%i: %f\n", i, z, lastFace->Vertex2[z]);
  393. else if (i == 2)
  394. printf("Face%i-%i: %f\n", i, z, lastFace->Vertex3[z]);
  395. }
  396. }*/
  397. endY = lastFace->Vertex2[1];
  398. }
  399. return endY;
  400. }
  401. Face* SPGrid::GetClosestFace(float x, float y, float z)
  402. {
  403. float temp_y = 0;
  404. float best_y = 999999.0f;
  405. FaceCell* startCell = GetFaceCell(x, z);
  406. float tmpY = y + 0.5f;
  407. float point[3];
  408. point[0] = x;
  409. point[1] = tmpY; // Small bump to make sure we are above ground when we do the trace
  410. point[2] = z;
  411. float MinDistance = 0.0f;
  412. // Create the direction for the trace, as we want what
  413. // is below it will just be -1 in the y direction
  414. float direction[3];
  415. direction[0] = 0.0f;
  416. direction[1] = -1.0f;
  417. direction[2] = 0.0f;
  418. Face* lastFace = 0;
  419. int32 Grid = 0;
  420. float BestZ = -999999.0f;
  421. map<int32, vector<Face*> >::iterator mapitr;
  422. for (mapitr = startCell->FaceList.begin(); mapitr != startCell->FaceList.end(); mapitr++) {
  423. vector<Face*>::iterator itr;
  424. for (itr = (*mapitr).second.begin(); itr != (*mapitr).second.end(); itr++) {
  425. Face* face = *itr;
  426. float distance;
  427. if ((distance = rayIntersectsTriangle(point, direction, face->Vertex1, face->Vertex2, face->Vertex3)) != 0) {
  428. if (MinDistance == 0.0f || distance < MinDistance) {
  429. BestZ = face->Vertex2[1];
  430. MinDistance = distance;
  431. lastFace = face;
  432. Grid = (*mapitr).first;
  433. }
  434. }
  435. }
  436. }
  437. return lastFace;
  438. }
  439. Face* SPGrid::FindPath(float x, float y, float z, float targX, float targY, float targZ, bool forceEndCell)
  440. {
  441. float MinDistance = 0.0f;
  442. float MinDistanceEnd = 999999.0f;
  443. // Create the starting point for the trace
  444. float point[3];
  445. point[0] = x;
  446. point[1] = y + 1.0f; // Small bump to make sure we are above ground when we do the trace
  447. point[2] = z;
  448. float pointEnd[3];
  449. pointEnd[0] = targX;
  450. pointEnd[1] = y + 1.0f; // Small bump to make sure we are above ground when we do the trace
  451. pointEnd[2] = targZ;
  452. // Create the direction for the trace, as we want what
  453. // is below it will just be -1 in the y direction
  454. float direction[3];
  455. if (!forceEndCell)
  456. {
  457. if (targX > x)
  458. direction[0] = -0.5f;
  459. else
  460. direction[0] = 0.5f;
  461. }
  462. else
  463. {
  464. if (targX > x)
  465. direction[0] = 1.0f;
  466. else// if (targZ < z)
  467. direction[0] = -1.0f;
  468. }
  469. //if (targY < y)
  470. direction[1] = -1.0f;
  471. //else
  472. // direction[1] = .5f;
  473. //direction[1] = -1.0f;
  474. if (forceEndCell)
  475. {
  476. if (targZ > z)
  477. direction[2] = -0.5f;
  478. else
  479. direction[2] = 0.5f;
  480. }
  481. else
  482. {
  483. if (targZ > z)
  484. direction[2] = 1.0f;
  485. else// if ( targX < x )
  486. direction[2] = -1.0f;
  487. }
  488. FaceCell* startCell = GetFaceCell(x, z);
  489. FaceCell* endCell = GetFaceCell(x, z);
  490. Face* startFace = GetClosestFace(x, y, z);
  491. if (startFace == NULL)
  492. return 0;
  493. //float tmpDistance = rayIntersectsTriangle(pointEnd, direction, startFace->Vertex1, startFace->Vertex2, startFace->Vertex3);
  494. //if (tmpDistance != 0.0f && tmpDistance < 15.0f)
  495. // return 0;
  496. Face* nextFace = 0;
  497. Face* endFace = GetClosestFace(targX, targY, targZ);
  498. float distBetweenEachOther = 999999.0f;
  499. map<int32, vector<Face*> >::iterator mapitr;
  500. if (endFace != NULL && startCell->FaceList.count(endFace->grid_id))
  501. mapitr = startCell->FaceList.find(endFace->grid_id);
  502. else if (startFace != NULL)
  503. mapitr = startCell->FaceList.find(startFace->grid_id);
  504. else
  505. return 0;
  506. //FILE* pFile;
  507. //pFile = fopen("vertices.txt", "a+");
  508. char msg[256];
  509. //_snprintf(msg, 256, "%f %f %f - %f %f %f\n", x,y,z,targX,targY,targZ);
  510. //fwrite(msg, 1, strnlen(msg, 256), pFile);
  511. for (; mapitr != startCell->FaceList.end(); mapitr++) {
  512. vector<Face*>::iterator itr;
  513. for (itr = (*mapitr).second.begin(); itr != (*mapitr).second.end(); itr++) {
  514. Face* face = *itr;
  515. float distance;
  516. float distanceend;
  517. distance = rayIntersectsTriangle(point, direction, face->Vertex1, face->Vertex2, face->Vertex3);
  518. //distanceend = rayIntersectsTriangle(pointEnd, direction, face->Vertex1, face->Vertex2, face->Vertex3);
  519. float tmpx1 = face->Vertex1[0] - pointEnd[0];
  520. float tmpy1 = face->Vertex1[1] - pointEnd[1];
  521. float tmpz1 = face->Vertex1[2] - pointEnd[2];
  522. float tmpDistBetweenEachOther = sqrt(tmpx1 * tmpx1 + tmpy1 * tmpy1 + tmpz1 * tmpz1);
  523. _snprintf(msg, 256, "%f (%f): Face: %f %f %f\n", tmpDistBetweenEachOther, distance, face->Vertex1[0], face->Vertex1[1], face->Vertex1[2]);
  524. if (face == startFace)
  525. {
  526. printf("Hit Start Cell..%s\n",msg);
  527. break;
  528. }
  529. else if (face == endFace)
  530. {
  531. printf("Hit End Cell..%s\n",msg);
  532. //continue;
  533. }
  534. //fwrite(msg, 1, strnlen(msg,256), pFile);
  535. //printf("%f: Face: %f %f %f... distance: %f..\n", tmpDistBetweenEachOther, face->Vertex1[0], face->Vertex1[1], face->Vertex1[2],distance);
  536. if (distance > 0.0f && ((MinDistance == 0.0f || distance < MinDistance) || (tmpDistBetweenEachOther < distBetweenEachOther))) {
  537. printf("%f (%f): !HIT! Face: %f %f %f\n", tmpDistBetweenEachOther, distance, face->Vertex1[0], face->Vertex1[1], face->Vertex1[2]);
  538. distBetweenEachOther = tmpDistBetweenEachOther;
  539. nextFace = face;
  540. MinDistance = distance;
  541. }
  542. }
  543. }
  544. /*
  545. fwrite("\n", sizeof(char), 1, pFile);
  546. if (forceEndCell)
  547. fwrite("Y", sizeof(char), 1, pFile);
  548. fwrite("\n\n", sizeof(char), 2, pFile);
  549. fclose(pFile);*/
  550. Face* anotherAttempt = 0;
  551. if (!forceEndCell)
  552. {
  553. printf("ForceEndCellSet:\n");
  554. anotherAttempt = FindPath(x, y, z, targX, targY, targZ, true);
  555. }
  556. if (!nextFace)
  557. {
  558. if (anotherAttempt)
  559. nextFace = anotherAttempt;
  560. else
  561. nextFace = endFace;
  562. /*if (!forceEndCell)
  563. return FindPath(x, y, z, targX, targY, targZ, true);
  564. nextFace = endFace;*/
  565. }
  566. return nextFace;
  567. }
  568. /**********************************************************************
  569. Math functions/macros to test a ray intersection in 3D space
  570. **********************************************************************/
  571. /* a = b - c */
  572. #define vector(a,b,c) \
  573. (a)[0] = (b)[0] - (c)[0]; \
  574. (a)[1] = (b)[1] - (c)[1]; \
  575. (a)[2] = (b)[2] - (c)[2];
  576. #define crossProduct(a,b,c) \
  577. (a)[0] = (b)[1] * (c)[2] - (c)[1] * (b)[2]; \
  578. (a)[1] = (b)[2] * (c)[0] - (c)[2] * (b)[0]; \
  579. (a)[2] = (b)[0] * (c)[1] - (c)[0] * (b)[1];
  580. #define innerProduct(v,q) \
  581. ((v)[0] * (q)[0] + \
  582. (v)[1] * (q)[1] + \
  583. (v)[2] * (q)[2])
  584. // all parameters should be vectors (float[3])
  585. float rayIntersectsTriangle(float *p, float *d, float *v0, float *v1, float *v2) {
  586. float e1[3], e2[3], h[3], s[3], q[3];
  587. float a, f, u, v;
  588. vector(e1, v1, v0);
  589. vector(e2, v2, v0);
  590. crossProduct(h, d, e2);
  591. a = innerProduct(e1, h);
  592. if (a > -0.00001 && a < 0.00001)
  593. return 0;
  594. f = 1 / a;
  595. vector(s, p, v0);
  596. u = f * (innerProduct(s, h));
  597. if (u < 0.0 || u > 1.0)
  598. return 0;
  599. crossProduct(q, s, e1);
  600. v = f * innerProduct(d, q);
  601. if (v < 0.0 || u + v > 1.0)
  602. return 0;
  603. // at this stage we can compute t to find out where
  604. // the intersection point is on the line
  605. float t = f * innerProduct(e2, q);
  606. if (t > 0.00001) // ray intersection
  607. return t;
  608. else // this means that there is a line intersection
  609. // but not a ray intersection
  610. return 0;
  611. }