SPGrid.cpp 22 KB

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