From e10bbb9ad7b1439c6bbc11274763f2375973c3bf Mon Sep 17 00:00:00 2001 From: elsid Date: Thu, 4 Nov 2021 19:48:18 +0100 Subject: [PATCH] Shift heightfield and water in navigator tests --- .../detournavigator/navigator.cpp | 861 +++++++++--------- .../detournavigator/operators.hpp | 2 +- components/detournavigator/makenavmesh.cpp | 1 + 3 files changed, 448 insertions(+), 416 deletions(-) diff --git a/apps/openmw_test_suite/detournavigator/navigator.cpp b/apps/openmw_test_suite/detournavigator/navigator.cpp index 0f4f1e3345..367572d449 100644 --- a/apps/openmw_test_suite/detournavigator/navigator.cpp +++ b/apps/openmw_test_suite/detournavigator/navigator.cpp @@ -7,6 +7,7 @@ #include #include #include +#include #include @@ -35,8 +36,8 @@ namespace { Settings mSettings; std::unique_ptr mNavigator; - osg::Vec3f mPlayerPosition; - osg::Vec3f mAgentHalfExtents; + const osg::Vec3f mPlayerPosition; + const osg::Vec3f mAgentHalfExtents; osg::Vec3f mStart; osg::Vec3f mEnd; std::deque mPath; @@ -48,12 +49,13 @@ namespace const int mHeightfieldTileSize = ESM::Land::REAL_SIZE / (ESM::Land::LAND_SIZE - 1); const osg::Vec3f mShift {0, 0, 0}; const float mEndTolerance = 0; + const btTransform mTransform {btMatrix3x3::getIdentity(), btVector3(256, 256, 0)}; DetourNavigatorNavigatorTest() - : mPlayerPosition(0, 0, 0) + : mPlayerPosition(256, 256, 0) , mAgentHalfExtents(29, 29, 66) - , mStart(-204, 204, 1) - , mEnd(204, -204, 1) + , mStart(52, 460, 1) + , mEnd(460, 52, 1) , mOut(mPath) , mStepSize(28.333332061767578125f) { @@ -135,6 +137,11 @@ namespace osg::ref_ptr mInstance; }; + osg::Vec3f getHeightfieldShift(const osg::Vec2i& cellPosition, int cellSize, float minHeight, float maxHeight) + { + return Misc::Convert::toOsg(BulletHelpers::getHeightfieldShift(cellPosition.x(), cellPosition.x(), cellSize, minHeight, maxHeight)); + } + TEST_F(DetourNavigatorNavigatorTest, find_path_for_empty_should_return_empty) { EXPECT_EQ(findPath(*mNavigator, mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_walk, mAreaCosts, mEndTolerance, mOut), @@ -168,9 +175,11 @@ namespace 0, -25, -100, -100, -100, }}; const HeightfieldSurface surface = makeSquareHeightfieldSurface(heightfieldData); + const int cellSize = mHeightfieldTileSize * (surface.mSize - 1); + const osg::Vec3f shift = getHeightfieldShift(mCellPosition, cellSize, surface.mMinHeight, surface.mMaxHeight); mNavigator->addAgent(mAgentHalfExtents); - mNavigator->addHeightfield(mCellPosition, mHeightfieldTileSize * (surface.mSize - 1), mShift, surface); + mNavigator->addHeightfield(mCellPosition, cellSize, shift, surface); mNavigator->update(mPlayerPosition); mNavigator->wait(mListener, WaitConditionType::requiredTilesPresent); @@ -178,28 +187,28 @@ namespace Status::Success); EXPECT_THAT(mPath, ElementsAre( - Vec3fEq(-204.0000152587890625, 204, 1.99998295307159423828125), - Vec3fEq(-183.96533203125, 183.9653167724609375, 1.99998819828033447265625), - Vec3fEq(-163.930633544921875, 163.9306182861328125, 1.99999344348907470703125), - Vec3fEq(-143.8959503173828125, 143.89593505859375, -2.720611572265625), - Vec3fEq(-123.86126708984375, 123.86124420166015625, -13.1089687347412109375), - Vec3fEq(-103.82657623291015625, 103.8265533447265625, -23.497333526611328125), - Vec3fEq(-83.7918853759765625, 83.7918548583984375, -33.885692596435546875), - Vec3fEq(-63.757190704345703125, 63.757171630859375, -44.274051666259765625), - Vec3fEq(-43.722503662109375, 43.72248077392578125, -54.66241455078125), - Vec3fEq(-23.687808990478515625, 23.6877918243408203125, -65.05077362060546875), - Vec3fEq(-3.6531188488006591796875, 3.6531002521514892578125, -75.43914031982421875), - Vec3fEq(16.3815746307373046875, -16.381591796875, -69.74927520751953125), - Vec3fEq(36.416263580322265625, -36.416286468505859375, -60.4739532470703125), - Vec3fEq(56.450958251953125, -56.450977325439453125, -51.1986236572265625), - Vec3fEq(76.48564910888671875, -76.4856719970703125, -41.92330169677734375), - Vec3fEq(96.5203399658203125, -96.52036285400390625, -31.46941375732421875), - Vec3fEq(116.55503082275390625, -116.5550537109375, -19.597003936767578125), - Vec3fEq(136.5897216796875, -136.5897369384765625, -7.724592685699462890625), - Vec3fEq(156.624420166015625, -156.624420166015625, 1.99999535083770751953125), - Vec3fEq(176.6591033935546875, -176.65911865234375, 1.99999010562896728515625), - Vec3fEq(196.69378662109375, -196.6938018798828125, 1.99998486042022705078125), - Vec3fEq(204, -204.0000152587890625, 1.99998295307159423828125) + Vec3fEq(56.66666412353515625, 460, 1.99998295307159423828125), + Vec3fEq(76.70135498046875, 439.965301513671875, -0.9659786224365234375), + Vec3fEq(96.73604583740234375, 419.93060302734375, -4.002437114715576171875), + Vec3fEq(116.770751953125, 399.89593505859375, -7.0388965606689453125), + Vec3fEq(136.8054351806640625, 379.861236572265625, -11.5593852996826171875), + Vec3fEq(156.840118408203125, 359.826568603515625, -20.7333812713623046875), + Vec3fEq(176.8748016357421875, 339.7918701171875, -34.014251708984375), + Vec3fEq(196.90948486328125, 319.757171630859375, -47.2951202392578125), + Vec3fEq(216.944183349609375, 299.722503662109375, -59.4111785888671875), + Vec3fEq(236.9788665771484375, 279.68780517578125, -65.76436614990234375), + Vec3fEq(257.0135498046875, 259.65313720703125, -68.12311553955078125), + Vec3fEq(277.048248291015625, 239.618438720703125, -66.5666656494140625), + Vec3fEq(297.082916259765625, 219.583740234375, -60.305889129638671875), + Vec3fEq(317.11761474609375, 199.549041748046875, -49.181324005126953125), + Vec3fEq(337.15228271484375, 179.5143585205078125, -35.742702484130859375), + Vec3fEq(357.186981201171875, 159.47967529296875, -22.304073333740234375), + Vec3fEq(377.221649169921875, 139.4449920654296875, -12.65070629119873046875), + Vec3fEq(397.25634765625, 119.41030120849609375, -7.41098117828369140625), + Vec3fEq(417.291046142578125, 99.3756103515625, -4.382833957672119140625), + Vec3fEq(437.325714111328125, 79.340911865234375, -1.354687213897705078125), + Vec3fEq(457.360443115234375, 59.3062286376953125, 1.624610424041748046875), + Vec3fEq(460, 56.66666412353515625, 1.99998295307159423828125) )) << mPath; } @@ -213,12 +222,14 @@ namespace 0, -25, -100, -100, -100, }}; const HeightfieldSurface surface = makeSquareHeightfieldSurface(heightfieldData); + const int cellSize = mHeightfieldTileSize * (surface.mSize - 1); + const osg::Vec3f shift = getHeightfieldShift(mCellPosition, cellSize, surface.mMinHeight, surface.mMaxHeight); CollisionShapeInstance compound(std::make_unique()); compound.shape().addChildShape(btTransform(btMatrix3x3::getIdentity(), btVector3(0, 0, 0)), new btBoxShape(btVector3(20, 20, 100))); mNavigator->addAgent(mAgentHalfExtents); - mNavigator->addHeightfield(mCellPosition, mHeightfieldTileSize * (surface.mSize - 1), mShift, surface); + mNavigator->addHeightfield(mCellPosition, cellSize, shift, surface); mNavigator->update(mPlayerPosition); mNavigator->wait(mListener, WaitConditionType::allJobsDone); @@ -226,31 +237,31 @@ namespace Status::Success); EXPECT_THAT(mPath, ElementsAre( - Vec3fEq(-204, 204, 1.99998295307159423828125), - Vec3fEq(-183.965301513671875, 183.965301513671875, 1.99998819828033447265625), - Vec3fEq(-163.9306182861328125, 163.9306182861328125, 1.99999344348907470703125), - Vec3fEq(-143.89593505859375, 143.89593505859375, -2.7206256389617919921875), - Vec3fEq(-123.86124420166015625, 123.86124420166015625, -13.1089839935302734375), - Vec3fEq(-103.8265533447265625, 103.8265533447265625, -23.4973468780517578125), - Vec3fEq(-83.7918548583984375, 83.7918548583984375, -33.885707855224609375), - Vec3fEq(-63.75716400146484375, 63.75716400146484375, -44.27407073974609375), - Vec3fEq(-43.72247314453125, 43.72247314453125, -54.662433624267578125), - Vec3fEq(-23.6877803802490234375, 23.6877803802490234375, -65.0507965087890625), - Vec3fEq(-3.653090000152587890625, 3.653090000152587890625, -75.43915557861328125), - Vec3fEq(16.3816013336181640625, -16.3816013336181640625, -69.749267578125), - Vec3fEq(36.416290283203125, -36.416290283203125, -60.4739532470703125), - Vec3fEq(56.450984954833984375, -56.450984954833984375, -51.1986236572265625), - Vec3fEq(76.4856719970703125, -76.4856719970703125, -41.92330169677734375), - Vec3fEq(96.52036285400390625, -96.52036285400390625, -31.46941375732421875), - Vec3fEq(116.5550537109375, -116.5550537109375, -19.597003936767578125), - Vec3fEq(136.5897369384765625, -136.5897369384765625, -7.724592685699462890625), - Vec3fEq(156.6244354248046875, -156.6244354248046875, 1.99999535083770751953125), - Vec3fEq(176.6591339111328125, -176.6591339111328125, 1.99999010562896728515625), - Vec3fEq(196.693817138671875, -196.693817138671875, 1.99998486042022705078125), - Vec3fEq(204, -204, 1.99998295307159423828125) + Vec3fEq(56.66666412353515625, 460, 1.99998295307159423828125), + Vec3fEq(76.70135498046875, 439.965301513671875, -0.9659786224365234375), + Vec3fEq(96.73604583740234375, 419.93060302734375, -4.002437114715576171875), + Vec3fEq(116.770751953125, 399.89593505859375, -7.0388965606689453125), + Vec3fEq(136.8054351806640625, 379.861236572265625, -11.5593852996826171875), + Vec3fEq(156.840118408203125, 359.826568603515625, -20.7333812713623046875), + Vec3fEq(176.8748016357421875, 339.7918701171875, -34.014251708984375), + Vec3fEq(196.90948486328125, 319.757171630859375, -47.2951202392578125), + Vec3fEq(216.944183349609375, 299.722503662109375, -59.4111785888671875), + Vec3fEq(236.9788665771484375, 279.68780517578125, -65.76436614990234375), + Vec3fEq(257.0135498046875, 259.65313720703125, -68.12311553955078125), + Vec3fEq(277.048248291015625, 239.618438720703125, -66.5666656494140625), + Vec3fEq(297.082916259765625, 219.583740234375, -60.305889129638671875), + Vec3fEq(317.11761474609375, 199.549041748046875, -49.181324005126953125), + Vec3fEq(337.15228271484375, 179.5143585205078125, -35.742702484130859375), + Vec3fEq(357.186981201171875, 159.47967529296875, -22.304073333740234375), + Vec3fEq(377.221649169921875, 139.4449920654296875, -12.65070629119873046875), + Vec3fEq(397.25634765625, 119.41030120849609375, -7.41098117828369140625), + Vec3fEq(417.291046142578125, 99.3756103515625, -4.382833957672119140625), + Vec3fEq(437.325714111328125, 79.340911865234375, -1.354687213897705078125), + Vec3fEq(457.360443115234375, 59.3062286376953125, 1.624610424041748046875), + Vec3fEq(460, 56.66666412353515625, 1.99998295307159423828125) )) << mPath; - mNavigator->addObject(ObjectId(&compound.shape()), ObjectShapes(compound.instance()), btTransform::getIdentity()); + mNavigator->addObject(ObjectId(&compound.shape()), ObjectShapes(compound.instance()), mTransform); mNavigator->update(mPlayerPosition); mNavigator->wait(mListener, WaitConditionType::allJobsDone); @@ -260,29 +271,29 @@ namespace Status::Success); EXPECT_THAT(mPath, ElementsAre( - Vec3fEq(-204, 204, 1.99998295307159423828125), - Vec3fEq(-189.9427337646484375, 179.3997802734375, -3.622931003570556640625), - Vec3fEq(-175.8854522705078125, 154.7995452880859375, -9.24583911895751953125), - Vec3fEq(-161.82818603515625, 130.1993255615234375, -14.86874866485595703125), - Vec3fEq(-147.770904541015625, 105.5991058349609375, -20.4916591644287109375), - Vec3fEq(-133.7136383056640625, 80.99887847900390625, -26.1145648956298828125), - Vec3fEq(-119.65636444091796875, 56.39865875244140625, -31.7374725341796875), - Vec3fEq(-105.59909820556640625, 31.798435211181640625, -26.133396148681640625), - Vec3fEq(-91.54183197021484375, 7.1982135772705078125, -31.5624217987060546875), - Vec3fEq(-77.48455810546875, -17.402008056640625, -26.98972320556640625), - Vec3fEq(-63.427295684814453125, -42.00223541259765625, -19.9045581817626953125), - Vec3fEq(-42.193531036376953125, -60.761363983154296875, -20.4544773101806640625), - Vec3fEq(-20.9597682952880859375, -79.5204925537109375, -23.599918365478515625), - Vec3fEq(3.8312885761260986328125, -93.2384033203125, -30.7141361236572265625), - Vec3fEq(28.6223468780517578125, -106.95632171630859375, -24.8243885040283203125), - Vec3fEq(53.413402557373046875, -120.6742401123046875, -31.3303241729736328125), - Vec3fEq(78.20446014404296875, -134.39215087890625, -25.8431549072265625), - Vec3fEq(102.99552154541015625, -148.110076904296875, -20.3559894561767578125), - Vec3fEq(127.7865753173828125, -161.827972412109375, -14.868824005126953125), - Vec3fEq(152.57763671875, -175.5458984375, -9.3816623687744140625), - Vec3fEq(177.3686981201171875, -189.2638092041015625, -3.894496917724609375), - Vec3fEq(202.1597442626953125, -202.9817047119140625, 1.59266507625579833984375), - Vec3fEq(204, -204, 1.99998295307159423828125) + Vec3fEq(56.66666412353515625, 460, 1.99998295307159423828125), + Vec3fEq(69.5299530029296875, 434.754913330078125, -2.6775772571563720703125), + Vec3fEq(82.39324951171875, 409.50982666015625, -7.355137348175048828125), + Vec3fEq(95.25653839111328125, 384.2647705078125, -12.0326976776123046875), + Vec3fEq(108.11983489990234375, 359.019683837890625, -16.71025848388671875), + Vec3fEq(120.983123779296875, 333.774627685546875, -21.3878192901611328125), + Vec3fEq(133.8464202880859375, 308.529541015625, -26.0653781890869140625), + Vec3fEq(146.7097015380859375, 283.284454345703125, -30.7429370880126953125), + Vec3fEq(159.572998046875, 258.039398193359375, -35.420497894287109375), + Vec3fEq(172.4362945556640625, 232.7943115234375, -27.2731761932373046875), + Vec3fEq(185.2996063232421875, 207.54925537109375, -19.575878143310546875), + Vec3fEq(206.6449737548828125, 188.917236328125, -20.3511219024658203125), + Vec3fEq(227.9903564453125, 170.28521728515625, -22.9776935577392578125), + Vec3fEq(253.4362640380859375, 157.8239593505859375, -31.1692962646484375), + Vec3fEq(278.8822021484375, 145.3627166748046875, -30.253124237060546875), + Vec3fEq(304.328094482421875, 132.9014739990234375, -22.219127655029296875), + Vec3fEq(329.774017333984375, 120.44022369384765625, -13.2701435089111328125), + Vec3fEq(355.219940185546875, 107.97898101806640625, -5.330339908599853515625), + Vec3fEq(380.665863037109375, 95.51773834228515625, -3.5501649379730224609375), + Vec3fEq(406.111785888671875, 83.05649566650390625, -1.76998889446258544921875), + Vec3fEq(431.557708740234375, 70.5952606201171875, 0.01018683053553104400634765625), + Vec3fEq(457.003662109375, 58.134021759033203125, 1.79036080837249755859375), + Vec3fEq(460, 56.66666412353515625, 1.99998295307159423828125) )) << mPath; } @@ -296,13 +307,15 @@ namespace 0, -25, -100, -100, -100, }}; const HeightfieldSurface surface = makeSquareHeightfieldSurface(heightfieldData); + const int cellSize = mHeightfieldTileSize * (surface.mSize - 1); + const osg::Vec3f shift = getHeightfieldShift(mCellPosition, cellSize, surface.mMinHeight, surface.mMaxHeight); CollisionShapeInstance compound(std::make_unique()); compound.shape().addChildShape(btTransform(btMatrix3x3::getIdentity(), btVector3(0, 0, 0)), new btBoxShape(btVector3(20, 20, 100))); mNavigator->addAgent(mAgentHalfExtents); - mNavigator->addHeightfield(mCellPosition, mHeightfieldTileSize * (surface.mSize - 1), mShift, surface); - mNavigator->addObject(ObjectId(&compound.shape()), ObjectShapes(compound.instance()), btTransform::getIdentity()); + mNavigator->addHeightfield(mCellPosition, cellSize, shift, surface); + mNavigator->addObject(ObjectId(&compound.shape()), ObjectShapes(compound.instance()), mTransform); mNavigator->update(mPlayerPosition); mNavigator->wait(mListener, WaitConditionType::allJobsDone); @@ -310,34 +323,34 @@ namespace Status::Success); EXPECT_THAT(mPath, ElementsAre( - Vec3fEq(-204, 204, 1.99998295307159423828125), - Vec3fEq(-189.9427337646484375, 179.3997802734375, -3.622931003570556640625), - Vec3fEq(-175.8854522705078125, 154.7995452880859375, -9.24583911895751953125), - Vec3fEq(-161.82818603515625, 130.1993255615234375, -14.86874866485595703125), - Vec3fEq(-147.770904541015625, 105.5991058349609375, -20.4916591644287109375), - Vec3fEq(-133.7136383056640625, 80.99887847900390625, -26.1145648956298828125), - Vec3fEq(-119.65636444091796875, 56.39865875244140625, -31.7374725341796875), - Vec3fEq(-105.59909820556640625, 31.798435211181640625, -26.133396148681640625), - Vec3fEq(-91.54183197021484375, 7.1982135772705078125, -31.5624217987060546875), - Vec3fEq(-77.48455810546875, -17.402008056640625, -26.98972320556640625), - Vec3fEq(-63.427295684814453125, -42.00223541259765625, -19.9045581817626953125), - Vec3fEq(-42.193531036376953125, -60.761363983154296875, -20.4544773101806640625), - Vec3fEq(-20.9597682952880859375, -79.5204925537109375, -23.599918365478515625), - Vec3fEq(3.8312885761260986328125, -93.2384033203125, -30.7141361236572265625), - Vec3fEq(28.6223468780517578125, -106.95632171630859375, -24.8243885040283203125), - Vec3fEq(53.413402557373046875, -120.6742401123046875, -31.3303241729736328125), - Vec3fEq(78.20446014404296875, -134.39215087890625, -25.8431549072265625), - Vec3fEq(102.99552154541015625, -148.110076904296875, -20.3559894561767578125), - Vec3fEq(127.7865753173828125, -161.827972412109375, -14.868824005126953125), - Vec3fEq(152.57763671875, -175.5458984375, -9.3816623687744140625), - Vec3fEq(177.3686981201171875, -189.2638092041015625, -3.894496917724609375), - Vec3fEq(202.1597442626953125, -202.9817047119140625, 1.59266507625579833984375), - Vec3fEq(204, -204, 1.99998295307159423828125) + Vec3fEq(56.66666412353515625, 460, 1.99998295307159423828125), + Vec3fEq(69.5299530029296875, 434.754913330078125, -2.6775772571563720703125), + Vec3fEq(82.39324951171875, 409.50982666015625, -7.355137348175048828125), + Vec3fEq(95.25653839111328125, 384.2647705078125, -12.0326976776123046875), + Vec3fEq(108.11983489990234375, 359.019683837890625, -16.71025848388671875), + Vec3fEq(120.983123779296875, 333.774627685546875, -21.3878192901611328125), + Vec3fEq(133.8464202880859375, 308.529541015625, -26.0653781890869140625), + Vec3fEq(146.7097015380859375, 283.284454345703125, -30.7429370880126953125), + Vec3fEq(159.572998046875, 258.039398193359375, -35.420497894287109375), + Vec3fEq(172.4362945556640625, 232.7943115234375, -27.2731761932373046875), + Vec3fEq(185.2996063232421875, 207.54925537109375, -19.575878143310546875), + Vec3fEq(206.6449737548828125, 188.917236328125, -20.3511219024658203125), + Vec3fEq(227.9903564453125, 170.28521728515625, -22.9776935577392578125), + Vec3fEq(253.4362640380859375, 157.8239593505859375, -31.1692962646484375), + Vec3fEq(278.8822021484375, 145.3627166748046875, -30.253124237060546875), + Vec3fEq(304.328094482421875, 132.9014739990234375, -22.219127655029296875), + Vec3fEq(329.774017333984375, 120.44022369384765625, -13.2701435089111328125), + Vec3fEq(355.219940185546875, 107.97898101806640625, -5.330339908599853515625), + Vec3fEq(380.665863037109375, 95.51773834228515625, -3.5501649379730224609375), + Vec3fEq(406.111785888671875, 83.05649566650390625, -1.76998889446258544921875), + Vec3fEq(431.557708740234375, 70.5952606201171875, 0.01018683053553104400634765625), + Vec3fEq(457.003662109375, 58.134021759033203125, 1.79036080837249755859375), + Vec3fEq(460, 56.66666412353515625, 1.99998295307159423828125) )) << mPath; compound.shape().updateChildTransform(0, btTransform(btMatrix3x3::getIdentity(), btVector3(1000, 0, 0))); - mNavigator->updateObject(ObjectId(&compound.shape()), ObjectShapes(compound.instance()), btTransform::getIdentity()); + mNavigator->updateObject(ObjectId(&compound.shape()), ObjectShapes(compound.instance()), mTransform); mNavigator->update(mPlayerPosition); mNavigator->wait(mListener, WaitConditionType::allJobsDone); @@ -347,28 +360,28 @@ namespace Status::Success); EXPECT_THAT(mPath, ElementsAre( - Vec3fEq(-204, 204, 1.99998295307159423828125), - Vec3fEq(-183.965301513671875, 183.965301513671875, 1.99998819828033447265625), - Vec3fEq(-163.9306182861328125, 163.9306182861328125, 1.99999344348907470703125), - Vec3fEq(-143.89593505859375, 143.89593505859375, -2.7206256389617919921875), - Vec3fEq(-123.86124420166015625, 123.86124420166015625, -13.1089839935302734375), - Vec3fEq(-103.8265533447265625, 103.8265533447265625, -23.4973468780517578125), - Vec3fEq(-83.7918548583984375, 83.7918548583984375, -33.885707855224609375), - Vec3fEq(-63.75716400146484375, 63.75716400146484375, -44.27407073974609375), - Vec3fEq(-43.72247314453125, 43.72247314453125, -54.662433624267578125), - Vec3fEq(-23.6877803802490234375, 23.6877803802490234375, -65.0507965087890625), - Vec3fEq(-3.653090000152587890625, 3.653090000152587890625, -75.43915557861328125), - Vec3fEq(16.3816013336181640625, -16.3816013336181640625, -69.749267578125), - Vec3fEq(36.416290283203125, -36.416290283203125, -60.4739532470703125), - Vec3fEq(56.450984954833984375, -56.450984954833984375, -51.1986236572265625), - Vec3fEq(76.4856719970703125, -76.4856719970703125, -41.92330169677734375), - Vec3fEq(96.52036285400390625, -96.52036285400390625, -31.46941375732421875), - Vec3fEq(116.5550537109375, -116.5550537109375, -19.597003936767578125), - Vec3fEq(136.5897369384765625, -136.5897369384765625, -7.724592685699462890625), - Vec3fEq(156.6244354248046875, -156.6244354248046875, 1.99999535083770751953125), - Vec3fEq(176.6591339111328125, -176.6591339111328125, 1.99999010562896728515625), - Vec3fEq(196.693817138671875, -196.693817138671875, 1.99998486042022705078125), - Vec3fEq(204, -204, 1.99998295307159423828125) + Vec3fEq(56.66666412353515625, 460, 1.99998295307159423828125), + Vec3fEq(76.70135498046875, 439.965301513671875, -0.9659786224365234375), + Vec3fEq(96.73604583740234375, 419.93060302734375, -4.002437114715576171875), + Vec3fEq(116.770751953125, 399.89593505859375, -7.0388965606689453125), + Vec3fEq(136.8054351806640625, 379.861236572265625, -11.5593852996826171875), + Vec3fEq(156.840118408203125, 359.826568603515625, -20.7333812713623046875), + Vec3fEq(176.8748016357421875, 339.7918701171875, -34.014251708984375), + Vec3fEq(196.90948486328125, 319.757171630859375, -47.2951202392578125), + Vec3fEq(216.944183349609375, 299.722503662109375, -59.4111785888671875), + Vec3fEq(236.9788665771484375, 279.68780517578125, -65.76436614990234375), + Vec3fEq(257.0135498046875, 259.65313720703125, -68.12311553955078125), + Vec3fEq(277.048248291015625, 239.618438720703125, -66.5666656494140625), + Vec3fEq(297.082916259765625, 219.583740234375, -60.305889129638671875), + Vec3fEq(317.11761474609375, 199.549041748046875, -49.181324005126953125), + Vec3fEq(337.15228271484375, 179.5143585205078125, -35.742702484130859375), + Vec3fEq(357.186981201171875, 159.47967529296875, -22.304073333740234375), + Vec3fEq(377.221649169921875, 139.4449920654296875, -12.65070629119873046875), + Vec3fEq(397.25634765625, 119.41030120849609375, -7.41098117828369140625), + Vec3fEq(417.291046142578125, 99.3756103515625, -4.382833957672119140625), + Vec3fEq(437.325714111328125, 79.340911865234375, -1.354687213897705078125), + Vec3fEq(457.360443115234375, 59.3062286376953125, 1.624610424041748046875), + Vec3fEq(460, 56.66666412353515625, 1.99998295307159423828125) )) << mPath; } @@ -395,8 +408,8 @@ namespace heightfield2.shape().setLocalScaling(btVector3(128, 128, 1)); mNavigator->addAgent(mAgentHalfExtents); - mNavigator->addObject(ObjectId(&heightfield1.shape()), ObjectShapes(heightfield1.instance()), btTransform::getIdentity()); - mNavigator->addObject(ObjectId(&heightfield2.shape()), ObjectShapes(heightfield2.instance()), btTransform::getIdentity()); + mNavigator->addObject(ObjectId(&heightfield1.shape()), ObjectShapes(heightfield1.instance()), mTransform); + mNavigator->addObject(ObjectId(&heightfield2.shape()), ObjectShapes(heightfield2.instance()), mTransform); mNavigator->update(mPlayerPosition); mNavigator->wait(mListener, WaitConditionType::allJobsDone); @@ -404,28 +417,28 @@ namespace Status::Success); EXPECT_THAT(mPath, ElementsAre( - Vec3fEq(-204, 204, 1.999981403350830078125), - Vec3fEq(-183.965301513671875, 183.965301513671875, -0.428465187549591064453125), - Vec3fEq(-163.9306182861328125, 163.9306182861328125, -2.8569104671478271484375), - Vec3fEq(-143.89593505859375, 143.89593505859375, -5.28535556793212890625), - Vec3fEq(-123.86124420166015625, 123.86124420166015625, -7.7138004302978515625), - Vec3fEq(-103.8265533447265625, 103.8265533447265625, -10.142246246337890625), - Vec3fEq(-83.7918548583984375, 83.7918548583984375, -12.3704509735107421875), - Vec3fEq(-63.75716400146484375, 63.75716400146484375, -14.354084014892578125), - Vec3fEq(-43.72247314453125, 43.72247314453125, -16.3377170562744140625), - Vec3fEq(-23.6877803802490234375, 23.6877803802490234375, -18.32135009765625), - Vec3fEq(-3.653090000152587890625, 3.653090000152587890625, -20.3049831390380859375), - Vec3fEq(16.3816013336181640625, -16.3816013336181640625, -19.044734954833984375), - Vec3fEq(36.416290283203125, -36.416290283203125, -17.061100006103515625), - Vec3fEq(56.450984954833984375, -56.450984954833984375, -15.0774688720703125), - Vec3fEq(76.4856719970703125, -76.4856719970703125, -13.0938358306884765625), - Vec3fEq(96.52036285400390625, -96.52036285400390625, -11.02784252166748046875), - Vec3fEq(116.5550537109375, -116.5550537109375, -8.5993976593017578125), - Vec3fEq(136.5897369384765625, -136.5897369384765625, -6.170953273773193359375), - Vec3fEq(156.6244354248046875, -156.6244354248046875, -3.74250507354736328125), - Vec3fEq(176.6591339111328125, -176.6591339111328125, -1.314060688018798828125), - Vec3fEq(196.693817138671875, -196.693817138671875, 1.1143856048583984375), - Vec3fEq(204, -204, 1.9999811649322509765625) + Vec3fEq(56.66666412353515625, 460, 1.99998295307159423828125), + Vec3fEq(76.70135498046875, 439.965301513671875, -0.903246104717254638671875), + Vec3fEq(96.73604583740234375, 419.93060302734375, -3.8064472675323486328125), + Vec3fEq(116.770751953125, 399.89593505859375, -6.709649562835693359375), + Vec3fEq(136.8054351806640625, 379.861236572265625, -9.33333873748779296875), + Vec3fEq(156.840118408203125, 359.826568603515625, -9.33333873748779296875), + Vec3fEq(176.8748016357421875, 339.7918701171875, -9.33333873748779296875), + Vec3fEq(196.90948486328125, 319.757171630859375, -9.33333873748779296875), + Vec3fEq(216.944183349609375, 299.722503662109375, -9.33333873748779296875), + Vec3fEq(236.9788665771484375, 279.68780517578125, -9.33333873748779296875), + Vec3fEq(257.0135498046875, 259.65313720703125, -9.33333873748779296875), + Vec3fEq(277.048248291015625, 239.618438720703125, -9.33333873748779296875), + Vec3fEq(297.082916259765625, 219.583740234375, -9.33333873748779296875), + Vec3fEq(317.11761474609375, 199.549041748046875, -9.33333873748779296875), + Vec3fEq(337.15228271484375, 179.5143585205078125, -9.33333873748779296875), + Vec3fEq(357.186981201171875, 159.47967529296875, -9.33333873748779296875), + Vec3fEq(377.221649169921875, 139.4449920654296875, -9.33333873748779296875), + Vec3fEq(397.25634765625, 119.41030120849609375, -6.891522884368896484375), + Vec3fEq(417.291046142578125, 99.3756103515625, -4.053897380828857421875), + Vec3fEq(437.325714111328125, 79.340911865234375, -1.21627247333526611328125), + Vec3fEq(457.360443115234375, 59.3062286376953125, 1.621352672576904296875), + Vec3fEq(460, 56.66666412353515625, 1.99998295307159423828125) )) << mPath; } @@ -439,6 +452,8 @@ namespace 0, -25, -100, -100, -100, }}; const HeightfieldSurface surface1 = makeSquareHeightfieldSurface(heightfieldData1); + const int cellSize1 = mHeightfieldTileSize * (surface1.mSize - 1); + const osg::Vec3f shift1 = getHeightfieldShift(mCellPosition, cellSize1, surface1.mMinHeight, surface1.mMaxHeight); const std::array heightfieldData2 {{ -25, -25, -25, -25, -25, @@ -448,10 +463,12 @@ namespace -25, -25, -25, -25, -25, }}; const HeightfieldSurface surface2 = makeSquareHeightfieldSurface(heightfieldData2); + const int cellSize2 = mHeightfieldTileSize * (surface2.mSize - 1); + const osg::Vec3f shift2 = getHeightfieldShift(mCellPosition, cellSize2, surface2.mMinHeight, surface2.mMaxHeight); mNavigator->addAgent(mAgentHalfExtents); - EXPECT_TRUE(mNavigator->addHeightfield(mCellPosition, mHeightfieldTileSize * (surface1.mSize - 1), mShift, surface1)); - EXPECT_FALSE(mNavigator->addHeightfield(mCellPosition, mHeightfieldTileSize * (surface2.mSize - 1), mShift, surface2)); + EXPECT_TRUE(mNavigator->addHeightfield(mCellPosition, cellSize1, shift1, surface1)); + EXPECT_FALSE(mNavigator->addHeightfield(mCellPosition, cellSize2, shift2, surface2)); } TEST_F(DetourNavigatorNavigatorTest, path_should_be_around_avoid_shape) @@ -483,7 +500,7 @@ namespace osg::ref_ptr instance(new Resource::BulletShapeInstance(bulletShape)); mNavigator->addAgent(mAgentHalfExtents); - mNavigator->addObject(ObjectId(instance->mCollisionShape.get()), ObjectShapes(instance), btTransform::getIdentity()); + mNavigator->addObject(ObjectId(instance->mCollisionShape.get()), ObjectShapes(instance), mTransform); mNavigator->update(mPlayerPosition); mNavigator->wait(mListener, WaitConditionType::allJobsDone); @@ -491,29 +508,29 @@ namespace Status::Success); EXPECT_THAT(mPath, ElementsAre( - Vec3fEq(-204, 204, 1.99997997283935546875), - Vec3fEq(-191.328948974609375, 178.65789794921875, -0.815807759761810302734375), - Vec3fEq(-178.65789794921875, 153.3157806396484375, -3.6315968036651611328125), - Vec3fEq(-165.986846923828125, 127.9736785888671875, -6.4473857879638671875), - Vec3fEq(-153.3157806396484375, 102.6315765380859375, -9.26317310333251953125), - Vec3fEq(-140.6447296142578125, 77.28946685791015625, -12.07896137237548828125), - Vec3fEq(-127.9736785888671875, 51.947368621826171875, -14.894748687744140625), - Vec3fEq(-115.3026275634765625, 26.6052646636962890625, -17.7105388641357421875), - Vec3fEq(-102.63158416748046875, 1.2631585597991943359375, -20.5263233184814453125), - Vec3fEq(-89.9605712890625, -24.0789661407470703125, -19.591716766357421875), - Vec3fEq(-68.54410552978515625, -42.629238128662109375, -19.847625732421875), - Vec3fEq(-47.127635955810546875, -61.17951202392578125, -20.1035366058349609375), - Vec3fEq(-25.711170196533203125, -79.72978973388671875, -20.359447479248046875), - Vec3fEq(-4.294706821441650390625, -98.280059814453125, -20.6153545379638671875), - Vec3fEq(17.121753692626953125, -116.83034515380859375, -17.3710460662841796875), - Vec3fEq(42.7990570068359375, -128.80755615234375, -14.7094440460205078125), - Vec3fEq(68.4763641357421875, -140.7847747802734375, -12.0478420257568359375), - Vec3fEq(94.15366363525390625, -152.761993408203125, -9.3862361907958984375), - Vec3fEq(119.83097076416015625, -164.7392120361328125, -6.724635601043701171875), - Vec3fEq(145.508270263671875, -176.7164306640625, -4.06303119659423828125), - Vec3fEq(171.185577392578125, -188.69366455078125, -1.40142619609832763671875), - Vec3fEq(196.862884521484375, -200.6708831787109375, 1.2601754665374755859375), - Vec3fEq(204, -204, 1.999979496002197265625) + Vec3fEq(56.66666412353515625, 460, 1.99998295307159423828125), + Vec3fEq(69.013885498046875, 434.49853515625, -0.74384129047393798828125), + Vec3fEq(81.36110687255859375, 408.997100830078125, -3.4876689910888671875), + Vec3fEq(93.7083282470703125, 383.495635986328125, -6.2314929962158203125), + Vec3fEq(106.0555419921875, 357.99420166015625, -8.97531890869140625), + Vec3fEq(118.40276336669921875, 332.49273681640625, -11.7191448211669921875), + Vec3fEq(130.7499847412109375, 306.991302490234375, -14.4629726409912109375), + Vec3fEq(143.0972137451171875, 281.4898681640625, -17.206798553466796875), + Vec3fEq(155.4444122314453125, 255.9884033203125, -19.9506206512451171875), + Vec3fEq(167.7916412353515625, 230.4869537353515625, -19.91887664794921875), + Vec3fEq(189.053619384765625, 211.75982666015625, -20.1138629913330078125), + Vec3fEq(210.3155975341796875, 193.032684326171875, -20.3088512420654296875), + Vec3fEq(231.577606201171875, 174.3055419921875, -20.503841400146484375), + Vec3fEq(252.839599609375, 155.5784149169921875, -19.9803981781005859375), + Vec3fEq(278.407989501953125, 143.3704071044921875, -17.2675113677978515625), + Vec3fEq(303.976348876953125, 131.16241455078125, -14.55462360382080078125), + Vec3fEq(329.54473876953125, 118.9544219970703125, -11.84173583984375), + Vec3fEq(355.11309814453125, 106.74642181396484375, -9.12884807586669921875), + Vec3fEq(380.681488037109375, 94.538421630859375, -6.4159603118896484375), + Vec3fEq(406.249847412109375, 82.33042144775390625, -3.7030735015869140625), + Vec3fEq(431.8182373046875, 70.1224365234375, -0.990187108516693115234375), + Vec3fEq(457.38665771484375, 57.9144439697265625, 1.72269880771636962890625), + Vec3fEq(460, 56.66666412353515625, 1.99998295307159423828125) )) << mPath; } @@ -527,38 +544,40 @@ namespace 0, -50, -100, -100, -100, }}; const HeightfieldSurface surface = makeSquareHeightfieldSurface(heightfieldData); + const int cellSize = mHeightfieldTileSize * (surface.mSize - 1); + const osg::Vec3f shift = getHeightfieldShift(mCellPosition, cellSize, surface.mMinHeight, surface.mMaxHeight); mNavigator->addAgent(mAgentHalfExtents); - mNavigator->addWater(osg::Vec2i(0, 0), 128 * 4, osg::Vec3f(0, 0, 300)); - mNavigator->addHeightfield(mCellPosition, mHeightfieldTileSize * (surface.mSize - 1), mShift, surface); + mNavigator->addWater(mCellPosition, cellSize, osg::Vec3f(shift.x(), shift.y(), 300)); + mNavigator->addHeightfield(mCellPosition, cellSize, shift, surface); mNavigator->update(mPlayerPosition); mNavigator->wait(mListener, WaitConditionType::allJobsDone); - mStart.x() = 0; + mStart.x() = 256; mStart.z() = 300; - mEnd.x() = 0; + mEnd.x() = 256; mEnd.z() = 300; EXPECT_EQ(findPath(*mNavigator, mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_swim, mAreaCosts, mEndTolerance, mOut), Status::Success); EXPECT_THAT(mPath, ElementsAre( - Vec3fEq(0, 204, 185.33331298828125), - Vec3fEq(0, 175.6666717529296875, 185.33331298828125), - Vec3fEq(0, 147.3333282470703125, 185.33331298828125), - Vec3fEq(0, 119, 185.33331298828125), - Vec3fEq(0, 90.6666717529296875, 185.33331298828125), - Vec3fEq(0, 62.333339691162109375, 185.33331298828125), - Vec3fEq(0, 34.00000762939453125, 185.33331298828125), - Vec3fEq(0, 5.66667461395263671875, 185.33331298828125), - Vec3fEq(0, -22.6666584014892578125, 185.33331298828125), - Vec3fEq(0, -50.999988555908203125, 185.33331298828125), - Vec3fEq(0, -79.33332061767578125, 185.33331298828125), - Vec3fEq(0, -107.666656494140625, 185.33331298828125), - Vec3fEq(0, -135.9999847412109375, 185.33331298828125), - Vec3fEq(0, -164.33331298828125, 185.33331298828125), - Vec3fEq(0, -192.666656494140625, 185.33331298828125), - Vec3fEq(0, -204, 185.33331298828125) + Vec3fEq(256, 460, 185.33331298828125), + Vec3fEq(256, 431.666656494140625, 185.33331298828125), + Vec3fEq(256, 403.33331298828125, 185.33331298828125), + Vec3fEq(256, 375, 185.33331298828125), + Vec3fEq(256, 346.666656494140625, 185.33331298828125), + Vec3fEq(256, 318.33331298828125, 185.33331298828125), + Vec3fEq(256, 290, 185.33331298828125), + Vec3fEq(256, 261.666656494140625, 185.33331298828125), + Vec3fEq(256, 233.3333282470703125, 185.33331298828125), + Vec3fEq(256, 205, 185.33331298828125), + Vec3fEq(256, 176.6666717529296875, 185.33331298828125), + Vec3fEq(256, 148.3333282470703125, 185.33331298828125), + Vec3fEq(256, 120, 185.33331298828125), + Vec3fEq(256, 91.6666717529296875, 185.33331298828125), + Vec3fEq(255.999969482421875, 63.33333587646484375, 185.33331298828125), + Vec3fEq(255.999969482421875, 56.66666412353515625, 185.33331298828125) )) << mPath; } @@ -574,36 +593,38 @@ namespace 0, 0, 0, 0, 0, 0, 0, }}; const HeightfieldSurface surface = makeSquareHeightfieldSurface(heightfieldData); + const int cellSize = mHeightfieldTileSize * (surface.mSize - 1); + const osg::Vec3f shift = getHeightfieldShift(mCellPosition, cellSize, surface.mMinHeight, surface.mMaxHeight); mNavigator->addAgent(mAgentHalfExtents); - mNavigator->addWater(mCellPosition, mHeightfieldTileSize * (surface.mSize - 1), osg::Vec3f(0, 0, -25)); - mNavigator->addHeightfield(mCellPosition, mHeightfieldTileSize * (surface.mSize - 1), mShift, surface); + mNavigator->addWater(mCellPosition, cellSize, osg::Vec3f(shift.x(), shift.y(), -25)); + mNavigator->addHeightfield(mCellPosition, cellSize, shift, surface); mNavigator->update(mPlayerPosition); mNavigator->wait(mListener, WaitConditionType::allJobsDone); - mStart.x() = 0; - mEnd.x() = 0; + mStart.x() = 256; + mEnd.x() = 256; EXPECT_EQ(findPath(*mNavigator, mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_swim | Flag_walk, mAreaCosts, mEndTolerance, mOut), Status::Success); EXPECT_THAT(mPath, ElementsAre( - Vec3fEq(0, 204, -98.000030517578125), - Vec3fEq(0, 175.6666717529296875, -108.30306243896484375), - Vec3fEq(0, 147.3333282470703125, -118.6060791015625), - Vec3fEq(0, 119, -128.90911865234375), - Vec3fEq(0, 90.6666717529296875, -139.2121429443359375), - Vec3fEq(0, 62.333339691162109375, -143.3333587646484375), - Vec3fEq(0, 34.00000762939453125, -143.3333587646484375), - Vec3fEq(0, 5.66667461395263671875, -143.3333587646484375), - Vec3fEq(0, -22.6666584014892578125, -143.3333587646484375), - Vec3fEq(0, -50.999988555908203125, -143.3333587646484375), - Vec3fEq(0, -79.33332061767578125, -143.3333587646484375), - Vec3fEq(0, -107.666656494140625, -133.0303192138671875), - Vec3fEq(0, -135.9999847412109375, -122.72728729248046875), - Vec3fEq(0, -164.33331298828125, -112.4242706298828125), - Vec3fEq(0, -192.666656494140625, -102.12123870849609375), - Vec3fEq(0, -204, -98.00002288818359375) + Vec3fEq(256, 460, -129.4098663330078125), + Vec3fEq(256, 431.666656494140625, -129.6970062255859375), + Vec3fEq(256, 403.33331298828125, -129.6970062255859375), + Vec3fEq(256, 375, -129.4439239501953125), + Vec3fEq(256, 346.666656494140625, -129.02587890625), + Vec3fEq(256, 318.33331298828125, -128.6078338623046875), + Vec3fEq(256, 290, -128.1021728515625), + Vec3fEq(256, 261.666656494140625, -126.46875), + Vec3fEq(256, 233.3333282470703125, -119.4891357421875), + Vec3fEq(256, 205, -110.62021636962890625), + Vec3fEq(256, 176.6666717529296875, -101.7512969970703125), + Vec3fEq(256, 148.3333282470703125, -92.88237762451171875), + Vec3fEq(256, 120, -75.29378509521484375), + Vec3fEq(256, 91.6666717529296875, -55.201839447021484375), + Vec3fEq(256.000030517578125, 63.33333587646484375, -34.800380706787109375), + Vec3fEq(256.000030517578125, 56.66666412353515625, -30.00003814697265625) )) << mPath; } @@ -619,36 +640,38 @@ namespace 0, 0, 0, 0, 0, 0, 0, }}; const HeightfieldSurface surface = makeSquareHeightfieldSurface(heightfieldData); + const int cellSize = mHeightfieldTileSize * (surface.mSize - 1); + const osg::Vec3f shift = getHeightfieldShift(mCellPosition, cellSize, surface.mMinHeight, surface.mMaxHeight); mNavigator->addAgent(mAgentHalfExtents); - mNavigator->addHeightfield(mCellPosition, mHeightfieldTileSize * (surface.mSize - 1), mShift, surface); - mNavigator->addWater(osg::Vec2i(0, 0), std::numeric_limits::max(), osg::Vec3f(0, 0, -25)); + mNavigator->addHeightfield(mCellPosition, cellSize, shift, surface); + mNavigator->addWater(mCellPosition, std::numeric_limits::max(), osg::Vec3f(shift.x(), shift.y(), -25)); mNavigator->update(mPlayerPosition); mNavigator->wait(mListener, WaitConditionType::allJobsDone); - mStart.x() = 0; - mEnd.x() = 0; + mStart.x() = 256; + mEnd.x() = 256; EXPECT_EQ(findPath(*mNavigator, mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_swim | Flag_walk, mAreaCosts, mEndTolerance, mOut), Status::Success); EXPECT_THAT(mPath, ElementsAre( - Vec3fEq(0, 204, -98.000030517578125), - Vec3fEq(0, 175.6666717529296875, -108.30306243896484375), - Vec3fEq(0, 147.3333282470703125, -118.6060791015625), - Vec3fEq(0, 119, -128.90911865234375), - Vec3fEq(0, 90.6666717529296875, -139.2121429443359375), - Vec3fEq(0, 62.333339691162109375, -143.3333587646484375), - Vec3fEq(0, 34.00000762939453125, -143.3333587646484375), - Vec3fEq(0, 5.66667461395263671875, -143.3333587646484375), - Vec3fEq(0, -22.6666584014892578125, -143.3333587646484375), - Vec3fEq(0, -50.999988555908203125, -143.3333587646484375), - Vec3fEq(0, -79.33332061767578125, -143.3333587646484375), - Vec3fEq(0, -107.666656494140625, -133.0303192138671875), - Vec3fEq(0, -135.9999847412109375, -122.72728729248046875), - Vec3fEq(0, -164.33331298828125, -112.4242706298828125), - Vec3fEq(0, -192.666656494140625, -102.12123870849609375), - Vec3fEq(0, -204, -98.00002288818359375) + Vec3fEq(256, 460, -129.4098663330078125), + Vec3fEq(256, 431.666656494140625, -129.6970062255859375), + Vec3fEq(256, 403.33331298828125, -129.6970062255859375), + Vec3fEq(256, 375, -129.4439239501953125), + Vec3fEq(256, 346.666656494140625, -129.02587890625), + Vec3fEq(256, 318.33331298828125, -128.6078338623046875), + Vec3fEq(256, 290, -128.1021728515625), + Vec3fEq(256, 261.666656494140625, -126.46875), + Vec3fEq(256, 233.3333282470703125, -119.4891357421875), + Vec3fEq(256, 205, -110.62021636962890625), + Vec3fEq(256, 176.6666717529296875, -101.7512969970703125), + Vec3fEq(256, 148.3333282470703125, -92.88237762451171875), + Vec3fEq(256, 120, -75.29378509521484375), + Vec3fEq(256, 91.6666717529296875, -55.201839447021484375), + Vec3fEq(256.000030517578125, 63.33333587646484375, -34.800380706787109375), + Vec3fEq(256.000030517578125, 56.66666412353515625, -30.00003814697265625) )) << mPath; } @@ -664,37 +687,38 @@ namespace 0, 0, 0, 0, 0, 0, 0, }}; const HeightfieldSurface surface = makeSquareHeightfieldSurface(heightfieldData); + const int cellSize = mHeightfieldTileSize * (surface.mSize - 1); + const osg::Vec3f shift = getHeightfieldShift(mCellPosition, cellSize, surface.mMinHeight, surface.mMaxHeight); mNavigator->addAgent(mAgentHalfExtents); - mNavigator->addWater(osg::Vec2i(0, 0), 128 * 4, osg::Vec3f(0, 0, -25)); - mNavigator->addHeightfield(mCellPosition, mHeightfieldTileSize * (surface.mSize - 1), mShift, surface); + mNavigator->addWater(mCellPosition, cellSize, osg::Vec3f(shift.x(), shift.y(), -25)); + mNavigator->addHeightfield(mCellPosition, cellSize, shift, surface); mNavigator->update(mPlayerPosition); mNavigator->wait(mListener, WaitConditionType::allJobsDone); - mStart.x() = 0; - mEnd.x() = 0; + mStart.x() = 256; + mEnd.x() = 256; EXPECT_EQ(findPath(*mNavigator, mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_walk, mAreaCosts, mEndTolerance, mOut), Status::Success); EXPECT_THAT(mPath, ElementsAre( - Vec3fEq(0, 204, -98.000030517578125), - Vec3fEq(10.26930999755859375, 177.59320068359375, -107.4711456298828125), - Vec3fEq(20.5386199951171875, 151.1864166259765625, -116.9422607421875), - Vec3fEq(30.8079280853271484375, 124.77960968017578125, -126.41339111328125), - Vec3fEq(41.077239990234375, 98.37281036376953125, -135.8845062255859375), - Vec3fEq(51.346546173095703125, 71.96601104736328125, -138.2003936767578125), - Vec3fEq(61.615856170654296875, 45.559215545654296875, -140.0838470458984375), - Vec3fEq(71.88516998291015625, 19.1524181365966796875, -141.9673004150390625), - Vec3fEq(82.15447235107421875, -7.254379749298095703125, -142.3074798583984375), - Vec3fEq(81.04636383056640625, -35.56603240966796875, -142.7104339599609375), - Vec3fEq(79.93825531005859375, -63.877685546875, -143.1133880615234375), - Vec3fEq(78.83014678955078125, -92.18933868408203125, -138.7660675048828125), - Vec3fEq(62.50392913818359375, -115.3460235595703125, -130.237823486328125), - Vec3fEq(46.17771148681640625, -138.502716064453125, -121.8172149658203125), - Vec3fEq(29.85149383544921875, -161.6594085693359375, -113.39659881591796875), - Vec3fEq(13.52527523040771484375, -184.81610107421875, -104.97599029541015625), - Vec3fEq(0, -204, -98.00002288818359375) + Vec3fEq(256, 460, -129.4098663330078125), + Vec3fEq(256, 431.666656494140625, -129.6970062255859375), + Vec3fEq(256, 403.33331298828125, -129.6970062255859375), + Vec3fEq(256, 375, -129.4439239501953125), + Vec3fEq(256, 346.666656494140625, -129.02587890625), + Vec3fEq(256, 318.33331298828125, -128.6078338623046875), + Vec3fEq(256, 290, -128.1021728515625), + Vec3fEq(256, 261.666656494140625, -126.46875), + Vec3fEq(256, 233.3333282470703125, -119.4891357421875), + Vec3fEq(256, 205, -110.62021636962890625), + Vec3fEq(256, 176.6666717529296875, -101.7512969970703125), + Vec3fEq(256, 148.3333282470703125, -92.88237762451171875), + Vec3fEq(256, 120, -75.29378509521484375), + Vec3fEq(256, 91.6666717529296875, -55.201839447021484375), + Vec3fEq(256.000030517578125, 63.33333587646484375, -34.800380706787109375), + Vec3fEq(256.000030517578125, 56.66666412353515625, -30.00003814697265625) )) << mPath; } @@ -711,7 +735,7 @@ namespace heightfield.shape().setLocalScaling(btVector3(128, 128, 1)); mNavigator->addAgent(mAgentHalfExtents); - mNavigator->addObject(ObjectId(&heightfield.shape()), ObjectShapes(heightfield.instance()), btTransform::getIdentity()); + mNavigator->addObject(ObjectId(&heightfield.shape()), ObjectShapes(heightfield.instance()), mTransform); mNavigator->update(mPlayerPosition); mNavigator->wait(mListener, WaitConditionType::allJobsDone); @@ -719,7 +743,7 @@ namespace mNavigator->update(mPlayerPosition); mNavigator->wait(mListener, WaitConditionType::allJobsDone); - mNavigator->addObject(ObjectId(&heightfield.shape()), ObjectShapes(heightfield.instance()), btTransform::getIdentity()); + mNavigator->addObject(ObjectId(&heightfield.shape()), ObjectShapes(heightfield.instance()), mTransform); mNavigator->update(mPlayerPosition); mNavigator->wait(mListener, WaitConditionType::allJobsDone); @@ -727,28 +751,28 @@ namespace Status::Success); EXPECT_THAT(mPath, ElementsAre( - Vec3fEq(-204, 204, 1.99998295307159423828125), - Vec3fEq(-183.965301513671875, 183.965301513671875, 1.99998819828033447265625), - Vec3fEq(-163.9306182861328125, 163.9306182861328125, 1.99999344348907470703125), - Vec3fEq(-143.89593505859375, 143.89593505859375, -2.7206256389617919921875), - Vec3fEq(-123.86124420166015625, 123.86124420166015625, -13.1089839935302734375), - Vec3fEq(-103.8265533447265625, 103.8265533447265625, -23.4973468780517578125), - Vec3fEq(-83.7918548583984375, 83.7918548583984375, -33.885707855224609375), - Vec3fEq(-63.75716400146484375, 63.75716400146484375, -44.27407073974609375), - Vec3fEq(-43.72247314453125, 43.72247314453125, -54.662433624267578125), - Vec3fEq(-23.6877803802490234375, 23.6877803802490234375, -65.0507965087890625), - Vec3fEq(-3.653090000152587890625, 3.653090000152587890625, -75.43915557861328125), - Vec3fEq(16.3816013336181640625, -16.3816013336181640625, -69.749267578125), - Vec3fEq(36.416290283203125, -36.416290283203125, -60.4739532470703125), - Vec3fEq(56.450984954833984375, -56.450984954833984375, -51.1986236572265625), - Vec3fEq(76.4856719970703125, -76.4856719970703125, -41.92330169677734375), - Vec3fEq(96.52036285400390625, -96.52036285400390625, -31.46941375732421875), - Vec3fEq(116.5550537109375, -116.5550537109375, -19.597003936767578125), - Vec3fEq(136.5897369384765625, -136.5897369384765625, -7.724592685699462890625), - Vec3fEq(156.6244354248046875, -156.6244354248046875, 1.99999535083770751953125), - Vec3fEq(176.6591339111328125, -176.6591339111328125, 1.99999010562896728515625), - Vec3fEq(196.693817138671875, -196.693817138671875, 1.99998486042022705078125), - Vec3fEq(204, -204, 1.99998295307159423828125) + Vec3fEq(56.66666412353515625, 460, 1.99998295307159423828125), + Vec3fEq(76.70135498046875, 439.965301513671875, -0.9659786224365234375), + Vec3fEq(96.73604583740234375, 419.93060302734375, -4.002437114715576171875), + Vec3fEq(116.770751953125, 399.89593505859375, -7.0388965606689453125), + Vec3fEq(136.8054351806640625, 379.861236572265625, -11.5593852996826171875), + Vec3fEq(156.840118408203125, 359.826568603515625, -20.7333812713623046875), + Vec3fEq(176.8748016357421875, 339.7918701171875, -34.014251708984375), + Vec3fEq(196.90948486328125, 319.757171630859375, -47.2951202392578125), + Vec3fEq(216.944183349609375, 299.722503662109375, -59.4111785888671875), + Vec3fEq(236.9788665771484375, 279.68780517578125, -65.76436614990234375), + Vec3fEq(257.0135498046875, 259.65313720703125, -68.12311553955078125), + Vec3fEq(277.048248291015625, 239.618438720703125, -66.5666656494140625), + Vec3fEq(297.082916259765625, 219.583740234375, -60.305889129638671875), + Vec3fEq(317.11761474609375, 199.549041748046875, -49.181324005126953125), + Vec3fEq(337.15228271484375, 179.5143585205078125, -35.742702484130859375), + Vec3fEq(357.186981201171875, 159.47967529296875, -22.304073333740234375), + Vec3fEq(377.221649169921875, 139.4449920654296875, -12.65070629119873046875), + Vec3fEq(397.25634765625, 119.41030120849609375, -7.41098117828369140625), + Vec3fEq(417.291046142578125, 99.3756103515625, -4.382833957672119140625), + Vec3fEq(437.325714111328125, 79.340911865234375, -1.354687213897705078125), + Vec3fEq(457.360443115234375, 59.3062286376953125, 1.624610424041748046875), + Vec3fEq(460, 56.66666412353515625, 1.99998295307159423828125) )) << mPath; } @@ -762,9 +786,11 @@ namespace 0, -25, -100, -100, -100, }}; const HeightfieldSurface surface = makeSquareHeightfieldSurface(heightfieldData); + const int cellSize = mHeightfieldTileSize * (surface.mSize - 1); + const osg::Vec3f shift = getHeightfieldShift(mCellPosition, cellSize, surface.mMinHeight, surface.mMaxHeight); mNavigator->addAgent(mAgentHalfExtents); - mNavigator->addHeightfield(mCellPosition, mHeightfieldTileSize * (surface.mSize - 1), mShift, surface); + mNavigator->addHeightfield(mCellPosition, cellSize, shift, surface); mNavigator->update(mPlayerPosition); mNavigator->wait(mListener, WaitConditionType::allJobsDone); @@ -772,7 +798,7 @@ namespace mNavigator->update(mPlayerPosition); mNavigator->wait(mListener, WaitConditionType::allJobsDone); - mNavigator->addHeightfield(mCellPosition, mHeightfieldTileSize * (surface.mSize - 1), mShift, surface); + mNavigator->addHeightfield(mCellPosition, cellSize, shift, surface); mNavigator->update(mPlayerPosition); mNavigator->wait(mListener, WaitConditionType::allJobsDone); @@ -780,44 +806,47 @@ namespace Status::Success); EXPECT_THAT(mPath, ElementsAre( - Vec3fEq(-204, 204, 1.99998295307159423828125), - Vec3fEq(-183.965301513671875, 183.965301513671875, 1.99998819828033447265625), - Vec3fEq(-163.9306182861328125, 163.9306182861328125, 1.99999344348907470703125), - Vec3fEq(-143.89593505859375, 143.89593505859375, -2.7206256389617919921875), - Vec3fEq(-123.86124420166015625, 123.86124420166015625, -13.1089839935302734375), - Vec3fEq(-103.8265533447265625, 103.8265533447265625, -23.4973468780517578125), - Vec3fEq(-83.7918548583984375, 83.7918548583984375, -33.885707855224609375), - Vec3fEq(-63.75716400146484375, 63.75716400146484375, -44.27407073974609375), - Vec3fEq(-43.72247314453125, 43.72247314453125, -54.662433624267578125), - Vec3fEq(-23.6877803802490234375, 23.6877803802490234375, -65.0507965087890625), - Vec3fEq(-3.653090000152587890625, 3.653090000152587890625, -75.43915557861328125), - Vec3fEq(16.3816013336181640625, -16.3816013336181640625, -69.749267578125), - Vec3fEq(36.416290283203125, -36.416290283203125, -60.4739532470703125), - Vec3fEq(56.450984954833984375, -56.450984954833984375, -51.1986236572265625), - Vec3fEq(76.4856719970703125, -76.4856719970703125, -41.92330169677734375), - Vec3fEq(96.52036285400390625, -96.52036285400390625, -31.46941375732421875), - Vec3fEq(116.5550537109375, -116.5550537109375, -19.597003936767578125), - Vec3fEq(136.5897369384765625, -136.5897369384765625, -7.724592685699462890625), - Vec3fEq(156.6244354248046875, -156.6244354248046875, 1.99999535083770751953125), - Vec3fEq(176.6591339111328125, -176.6591339111328125, 1.99999010562896728515625), - Vec3fEq(196.693817138671875, -196.693817138671875, 1.99998486042022705078125), - Vec3fEq(204, -204, 1.99998295307159423828125) + Vec3fEq(56.66666412353515625, 460, 1.99998295307159423828125), + Vec3fEq(76.70135498046875, 439.965301513671875, -0.9659786224365234375), + Vec3fEq(96.73604583740234375, 419.93060302734375, -4.002437114715576171875), + Vec3fEq(116.770751953125, 399.89593505859375, -7.0388965606689453125), + Vec3fEq(136.8054351806640625, 379.861236572265625, -11.5593852996826171875), + Vec3fEq(156.840118408203125, 359.826568603515625, -20.7333812713623046875), + Vec3fEq(176.8748016357421875, 339.7918701171875, -34.014251708984375), + Vec3fEq(196.90948486328125, 319.757171630859375, -47.2951202392578125), + Vec3fEq(216.944183349609375, 299.722503662109375, -59.4111785888671875), + Vec3fEq(236.9788665771484375, 279.68780517578125, -65.76436614990234375), + Vec3fEq(257.0135498046875, 259.65313720703125, -68.12311553955078125), + Vec3fEq(277.048248291015625, 239.618438720703125, -66.5666656494140625), + Vec3fEq(297.082916259765625, 219.583740234375, -60.305889129638671875), + Vec3fEq(317.11761474609375, 199.549041748046875, -49.181324005126953125), + Vec3fEq(337.15228271484375, 179.5143585205078125, -35.742702484130859375), + Vec3fEq(357.186981201171875, 159.47967529296875, -22.304073333740234375), + Vec3fEq(377.221649169921875, 139.4449920654296875, -12.65070629119873046875), + Vec3fEq(397.25634765625, 119.41030120849609375, -7.41098117828369140625), + Vec3fEq(417.291046142578125, 99.3756103515625, -4.382833957672119140625), + Vec3fEq(437.325714111328125, 79.340911865234375, -1.354687213897705078125), + Vec3fEq(457.360443115234375, 59.3062286376953125, 1.624610424041748046875), + Vec3fEq(460, 56.66666412353515625, 1.99998295307159423828125) )) << mPath; } TEST_F(DetourNavigatorNavigatorTest, update_then_find_random_point_around_circle_should_return_position) { - const std::array heightfieldData {{ - 0, 0, 0, 0, 0, - 0, -25, -25, -25, -25, - 0, -25, -100, -100, -100, - 0, -25, -100, -100, -100, - 0, -25, -100, -100, -100, + const std::array heightfieldData {{ + 0, 0, 0, 0, 0, 0, + 0, -25, -25, -25, -25, -25, + 0, -25, -1000, -1000, -100, -100, + 0, -25, -1000, -1000, -100, -100, + 0, -25, -100, -100, -100, -100, + 0, -25, -100, -100, -100, -100, }}; const HeightfieldSurface surface = makeSquareHeightfieldSurface(heightfieldData); + const int cellSize = mHeightfieldTileSize * (surface.mSize - 1); + const osg::Vec3f shift = getHeightfieldShift(mCellPosition, cellSize, surface.mMinHeight, surface.mMaxHeight); mNavigator->addAgent(mAgentHalfExtents); - mNavigator->addHeightfield(mCellPosition, mHeightfieldTileSize * (surface.mSize - 1), mShift, surface); + mNavigator->addHeightfield(mCellPosition, cellSize, shift, surface); mNavigator->update(mPlayerPosition); mNavigator->wait(mListener, WaitConditionType::allJobsDone); @@ -825,12 +854,12 @@ namespace const auto result = findRandomPointAroundCircle(*mNavigator, mAgentHalfExtents, mStart, 100.0, Flag_walk); - ASSERT_THAT(result, Optional(Vec3fEq(-198.909332275390625, 123.06096649169921875, 1.99998414516448974609375))) + ASSERT_THAT(result, Optional(Vec3fEq(69.6253509521484375, 531.29852294921875, -2.6667339801788330078125))) << (result ? *result : osg::Vec3f()); const auto distance = (*result - mStart).length(); - EXPECT_FLOAT_EQ(distance, 81.105133056640625) << distance; + EXPECT_FLOAT_EQ(distance, 73.536231994628906) << distance; } TEST_F(DetourNavigatorNavigatorTest, multiple_threads_should_lock_tiles) @@ -846,17 +875,19 @@ namespace 0, -25, -100, -100, -100, }}; const HeightfieldSurface surface = makeSquareHeightfieldSurface(heightfieldData); + const int cellSize = mHeightfieldTileSize * (surface.mSize - 1); + const osg::Vec3f shift = getHeightfieldShift(mCellPosition, cellSize, surface.mMinHeight, surface.mMaxHeight); std::vector> boxes; std::generate_n(std::back_inserter(boxes), 100, [] { return std::make_unique(btVector3(20, 20, 100)); }); mNavigator->addAgent(mAgentHalfExtents); - mNavigator->addHeightfield(mCellPosition, mHeightfieldTileSize * (surface.mSize - 1), mShift, surface); + mNavigator->addHeightfield(mCellPosition, cellSize, shift, surface); for (std::size_t i = 0; i < boxes.size(); ++i) { - const btTransform transform(btMatrix3x3::getIdentity(), btVector3(i * 10, i * 10, i * 10)); + const btTransform transform(btMatrix3x3::getIdentity(), btVector3(shift.x() + i * 10, shift.y() + i * 10, i * 10)); mNavigator->addObject(ObjectId(&boxes[i].shape()), ObjectShapes(boxes[i].instance()), transform); } @@ -864,7 +895,7 @@ namespace for (std::size_t i = 0; i < boxes.size(); ++i) { - const btTransform transform(btMatrix3x3::getIdentity(), btVector3(i * 10 + 1, i * 10 + 1, i * 10 + 1)); + const btTransform transform(btMatrix3x3::getIdentity(), btVector3(shift.x() + i * 10 + 1, shift.y() + i * 10 + 1, i * 10 + 1)); mNavigator->updateObject(ObjectId(&boxes[i].shape()), ObjectShapes(boxes[i].instance()), transform); } @@ -875,29 +906,29 @@ namespace Status::Success); EXPECT_THAT(mPath, ElementsAre( - Vec3fEq(-204, 204, 1.99998295307159423828125), - Vec3fEq(-189.9427337646484375, 179.3997802734375, 1.9999866485595703125), - Vec3fEq(-175.8854522705078125, 154.7995452880859375, 1.99999034404754638671875), - Vec3fEq(-161.82818603515625, 130.1993255615234375, -3.701923847198486328125), - Vec3fEq(-147.770904541015625, 105.5991058349609375, -15.67664432525634765625), - Vec3fEq(-133.7136383056640625, 80.99887847900390625, -27.6513614654541015625), - Vec3fEq(-119.65636444091796875, 56.39865875244140625, -20.1209163665771484375), - Vec3fEq(-105.59909820556640625, 31.798435211181640625, -25.0669879913330078125), - Vec3fEq(-91.54183197021484375, 7.1982135772705078125, -31.5624217987060546875), - Vec3fEq(-77.48455810546875, -17.402008056640625, -26.98972320556640625), - Vec3fEq(-63.427295684814453125, -42.00223541259765625, -19.9045581817626953125), - Vec3fEq(-42.193531036376953125, -60.761363983154296875, -20.4544773101806640625), - Vec3fEq(-20.9597682952880859375, -79.5204925537109375, -23.599918365478515625), - Vec3fEq(3.8312885761260986328125, -93.2384033203125, -30.7141361236572265625), - Vec3fEq(28.6223468780517578125, -106.95632171630859375, -24.1782474517822265625), - Vec3fEq(53.413402557373046875, -120.6742401123046875, -19.4096889495849609375), - Vec3fEq(78.20446014404296875, -134.39215087890625, -27.6632633209228515625), - Vec3fEq(102.99552154541015625, -148.110076904296875, -15.8613681793212890625), - Vec3fEq(127.7865753173828125, -161.827972412109375, -4.059485912322998046875), - Vec3fEq(152.57763671875, -175.5458984375, 1.9999904632568359375), - Vec3fEq(177.3686981201171875, -189.2638092041015625, 1.9999866485595703125), - Vec3fEq(202.1597442626953125, -202.9817047119140625, 1.9999830722808837890625), - Vec3fEq(204, -204, 1.99998295307159423828125) + Vec3fEq(56.66666412353515625, 460, 1.99998295307159423828125), + Vec3fEq(69.5299530029296875, 434.754913330078125, -2.6775772571563720703125), + Vec3fEq(82.39324951171875, 409.50982666015625, -7.355137348175048828125), + Vec3fEq(95.25653839111328125, 384.2647705078125, -12.0326976776123046875), + Vec3fEq(108.11983489990234375, 359.019683837890625, -16.71025848388671875), + Vec3fEq(120.983123779296875, 333.774627685546875, -21.3878192901611328125), + Vec3fEq(133.8464202880859375, 308.529541015625, -26.0653781890869140625), + Vec3fEq(146.7097015380859375, 283.284454345703125, -30.7429370880126953125), + Vec3fEq(159.572998046875, 258.039398193359375, -35.420497894287109375), + Vec3fEq(172.4362945556640625, 232.7943115234375, -27.2731761932373046875), + Vec3fEq(185.2996063232421875, 207.54925537109375, -20.3612518310546875), + Vec3fEq(206.6449737548828125, 188.917236328125, -20.578319549560546875), + Vec3fEq(227.9903564453125, 170.28521728515625, -26.291717529296875), + Vec3fEq(253.4362640380859375, 157.8239593505859375, -34.784488677978515625), + Vec3fEq(278.8822021484375, 145.3627166748046875, -30.253124237060546875), + Vec3fEq(304.328094482421875, 132.9014739990234375, -25.72176361083984375), + Vec3fEq(329.774017333984375, 120.44022369384765625, -21.1904010772705078125), + Vec3fEq(355.219940185546875, 107.97898101806640625, -16.6590404510498046875), + Vec3fEq(380.665863037109375, 95.51773834228515625, -12.127681732177734375), + Vec3fEq(406.111785888671875, 83.05649566650390625, -7.5963191986083984375), + Vec3fEq(431.557708740234375, 70.5952606201171875, -3.0649592876434326171875), + Vec3fEq(457.003662109375, 58.134021759033203125, 1.4664003849029541015625), + Vec3fEq(460, 56.66666412353515625, 1.99998295307159423828125) )) << mPath; } @@ -949,15 +980,19 @@ namespace 0, -25, -100, -100, -100, }}; const HeightfieldSurface surface = makeSquareHeightfieldSurface(heightfieldData); + const int cellSize = mHeightfieldTileSize * (surface.mSize - 1); + const osg::Vec3f shift = getHeightfieldShift(mCellPosition, cellSize, surface.mMinHeight, surface.mMaxHeight); mNavigator->addAgent(mAgentHalfExtents); - mNavigator->addHeightfield(mCellPosition, mHeightfieldTileSize * (surface.mSize - 1), mShift, surface); + mNavigator->addHeightfield(mCellPosition, cellSize, shift, surface); mNavigator->update(mPlayerPosition); mNavigator->wait(mListener, WaitConditionType::allJobsDone); - const auto result = raycast(*mNavigator, mAgentHalfExtents, mStart, mEnd, Flag_walk); + const osg::Vec3f start(57, 460, 1); + const osg::Vec3f end(460, 57, 1); + const auto result = raycast(*mNavigator, mAgentHalfExtents, start, end, Flag_walk); - ASSERT_THAT(result, Optional(Vec3fEq(mEnd.x(), mEnd.y(), 1.99998295307159423828125))) + ASSERT_THAT(result, Optional(Vec3fEq(end.x(), end.y(), 1.95257937908172607421875))) << (result ? *result : osg::Vec3f()); } @@ -971,27 +1006,28 @@ namespace 0, -25, -100, -100, -100, }}; const HeightfieldSurface surface = makeSquareHeightfieldSurface(heightfieldData); + const int cellSize = mHeightfieldTileSize * (surface.mSize - 1); + const osg::Vec3f shift = getHeightfieldShift(mCellPosition, cellSize, surface.mMinHeight, surface.mMaxHeight); CollisionShapeInstance oscillatingBox(std::make_unique(btVector3(20, 20, 20))); - const btVector3 oscillatingBoxShapePosition(32, 32, 400); - CollisionShapeInstance boderBox(std::make_unique(btVector3(50, 50, 50))); + const btVector3 oscillatingBoxShapePosition(288, 288, 400); + CollisionShapeInstance borderBox(std::make_unique(btVector3(50, 50, 50))); mNavigator->addAgent(mAgentHalfExtents); - mNavigator->addHeightfield(mCellPosition, mHeightfieldTileSize * (surface.mSize - 1), mShift, surface); + mNavigator->addHeightfield(mCellPosition, cellSize, shift, surface); mNavigator->addObject(ObjectId(&oscillatingBox.shape()), ObjectShapes(oscillatingBox.instance()), btTransform(btMatrix3x3::getIdentity(), oscillatingBoxShapePosition)); // add this box to make navmesh bound box independent from oscillatingBoxShape rotations - mNavigator->addObject(ObjectId(&boderBox.shape()), ObjectShapes(boderBox.instance()), + mNavigator->addObject(ObjectId(&borderBox.shape()), ObjectShapes(borderBox.instance()), btTransform(btMatrix3x3::getIdentity(), oscillatingBoxShapePosition + btVector3(0, 0, 200))); mNavigator->update(mPlayerPosition); mNavigator->wait(mListener, WaitConditionType::allJobsDone); + const Version expectedVersion {1, 1}; + const auto navMeshes = mNavigator->getNavMeshes(); ASSERT_EQ(navMeshes.size(), 1); - { - const auto navMesh = navMeshes.begin()->second->lockConst(); - ASSERT_EQ(navMesh->getVersion(), (Version {1, 4})); - } + ASSERT_EQ(navMeshes.begin()->second->lockConst()->getVersion(), expectedVersion); for (int n = 0; n < 10; ++n) { @@ -1003,18 +1039,17 @@ namespace } ASSERT_EQ(navMeshes.size(), 1); - { - const auto navMesh = navMeshes.begin()->second->lockConst(); - ASSERT_EQ(navMesh->getVersion(), (Version {1, 4})); - } + ASSERT_EQ(navMeshes.begin()->second->lockConst()->getVersion(), expectedVersion); } TEST_F(DetourNavigatorNavigatorTest, should_provide_path_over_flat_heightfield) { const HeightfieldPlane plane {100}; + const int cellSize = mHeightfieldTileSize * 4; + const osg::Vec3f shift = getHeightfieldShift(mCellPosition, cellSize, plane.mHeight, plane.mHeight); mNavigator->addAgent(mAgentHalfExtents); - mNavigator->addHeightfield(mCellPosition, mHeightfieldTileSize * 4, mShift, plane); + mNavigator->addHeightfield(mCellPosition, cellSize, shift, plane); mNavigator->update(mPlayerPosition); mNavigator->wait(mListener, WaitConditionType::requiredTilesPresent); @@ -1022,28 +1057,28 @@ namespace Status::Success); EXPECT_THAT(mPath, ElementsAre( - Vec3fEq(-204, 204, 101.99999237060546875), - Vec3fEq(-183.965301513671875, 183.965301513671875, 101.99999237060546875), - Vec3fEq(-163.9306182861328125, 163.9306182861328125, 101.99999237060546875), - Vec3fEq(-143.89593505859375, 143.89593505859375, 101.99999237060546875), - Vec3fEq(-123.86124420166015625, 123.86124420166015625, 101.99999237060546875), - Vec3fEq(-103.8265533447265625, 103.8265533447265625, 101.99999237060546875), - Vec3fEq(-83.7918548583984375, 83.7918548583984375, 101.99999237060546875), - Vec3fEq(-63.75716400146484375, 63.75716400146484375, 101.99999237060546875), - Vec3fEq(-43.72247314453125, 43.72247314453125, 101.99999237060546875), - Vec3fEq(-23.6877803802490234375, 23.6877803802490234375, 101.99999237060546875), - Vec3fEq(-3.653090000152587890625, 3.653090000152587890625, 101.99999237060546875), - Vec3fEq(16.3816013336181640625, -16.3816013336181640625, 101.99999237060546875), - Vec3fEq(36.416290283203125, -36.416290283203125, 101.99999237060546875), - Vec3fEq(56.450984954833984375, -56.450984954833984375, 101.99999237060546875), - Vec3fEq(76.4856719970703125, -76.4856719970703125, 101.99999237060546875), - Vec3fEq(96.52036285400390625, -96.52036285400390625, 101.99999237060546875), - Vec3fEq(116.5550537109375, -116.5550537109375, 101.99999237060546875), - Vec3fEq(136.5897369384765625, -136.5897369384765625, 101.99999237060546875), - Vec3fEq(156.6244354248046875, -156.6244354248046875, 101.99999237060546875), - Vec3fEq(176.6591339111328125, -176.6591339111328125, 101.99999237060546875), - Vec3fEq(196.693817138671875, -196.693817138671875, 101.99999237060546875), - Vec3fEq(204, -204, 101.99999237060546875) + Vec3fEq(56.66666412353515625, 460, 203.9999847412109375), + Vec3fEq(76.70135498046875, 439.965301513671875, 203.9999847412109375), + Vec3fEq(96.73604583740234375, 419.93060302734375, 203.9999847412109375), + Vec3fEq(116.770751953125, 399.89593505859375, 203.9999847412109375), + Vec3fEq(136.8054351806640625, 379.861236572265625, 203.9999847412109375), + Vec3fEq(156.840118408203125, 359.826568603515625, 203.9999847412109375), + Vec3fEq(176.8748016357421875, 339.7918701171875, 203.9999847412109375), + Vec3fEq(196.90948486328125, 319.757171630859375, 203.9999847412109375), + Vec3fEq(216.944183349609375, 299.722503662109375, 203.9999847412109375), + Vec3fEq(236.9788665771484375, 279.68780517578125, 203.9999847412109375), + Vec3fEq(257.0135498046875, 259.65313720703125, 203.9999847412109375), + Vec3fEq(277.048248291015625, 239.618438720703125, 203.9999847412109375), + Vec3fEq(297.082916259765625, 219.583740234375, 203.9999847412109375), + Vec3fEq(317.11761474609375, 199.549041748046875, 203.9999847412109375), + Vec3fEq(337.15228271484375, 179.5143585205078125, 203.9999847412109375), + Vec3fEq(357.186981201171875, 159.47967529296875, 203.9999847412109375), + Vec3fEq(377.221649169921875, 139.4449920654296875, 203.9999847412109375), + Vec3fEq(397.25634765625, 119.41030120849609375, 203.9999847412109375), + Vec3fEq(417.291046142578125, 99.3756103515625, 203.9999847412109375), + Vec3fEq(437.325714111328125, 79.340911865234375, 203.9999847412109375), + Vec3fEq(457.360443115234375, 59.3062286376953125, 203.9999847412109375), + Vec3fEq(460, 56.66666412353515625, 203.9999847412109375) )) << mPath; } @@ -1057,14 +1092,16 @@ namespace 0, -25, -100, -100, -100, }}; const HeightfieldSurface surface = makeSquareHeightfieldSurface(heightfieldData); + const int cellSize = mHeightfieldTileSize * (surface.mSize - 1); + const osg::Vec3f shift = getHeightfieldShift(mCellPosition, cellSize, surface.mMinHeight, surface.mMaxHeight); CollisionShapeInstance compound(std::make_unique()); compound.shape().addChildShape(btTransform(btMatrix3x3::getIdentity(), btVector3(204, -204, 0)), new btBoxShape(btVector3(200, 200, 1000))); mNavigator->addAgent(mAgentHalfExtents); - mNavigator->addHeightfield(mCellPosition, mHeightfieldTileSize * (surface.mSize - 1), mShift, surface); - mNavigator->addObject(ObjectId(&compound.shape()), ObjectShapes(compound.instance()), btTransform::getIdentity()); + mNavigator->addHeightfield(mCellPosition, cellSize, shift, surface); + mNavigator->addObject(ObjectId(&compound.shape()), ObjectShapes(compound.instance()), mTransform); mNavigator->update(mPlayerPosition); mNavigator->wait(mListener, WaitConditionType::allJobsDone); @@ -1072,23 +1109,16 @@ namespace Status::PartialPath); EXPECT_THAT(mPath, ElementsAre( - Vec3fEq(-204, 204, -2.666739940643310546875), - Vec3fEq(-193.730682373046875, 177.59320068359375, -3.9535934925079345703125), - Vec3fEq(-183.4613800048828125, 151.1864166259765625, -5.240451335906982421875), - Vec3fEq(-173.1920623779296875, 124.77962493896484375, -6.527309417724609375), - Vec3fEq(-162.922760009765625, 98.37282562255859375, -7.814167022705078125), - Vec3fEq(-152.6534423828125, 71.96602630615234375, -9.898590087890625), - Vec3fEq(-142.384124755859375, 45.559230804443359375, -17.641445159912109375), - Vec3fEq(-132.1148223876953125, 19.152431488037109375, -25.3843059539794921875), - Vec3fEq(-121.8455047607421875, -7.254369258880615234375, -27.97742462158203125), - Vec3fEq(-111.57619476318359375, -33.66117095947265625, -16.974590301513671875), - Vec3fEq(-101.30689239501953125, -60.06797027587890625, -5.9717559814453125), - Vec3fEq(-91.0375823974609375, -86.47476959228515625, -2.6667339801788330078125), - Vec3fEq(-80.76827239990234375, -112.88156890869140625, -2.6667339801788330078125), - Vec3fEq(-70.49897003173828125, -139.2883758544921875, -2.6667339801788330078125), - Vec3fEq(-60.229663848876953125, -165.6951751708984375, -2.6667339801788330078125), - Vec3fEq(-49.96035003662109375, -192.1019744873046875, -2.6667339801788330078125), - Vec3fEq(-45.333343505859375, -204, -2.6667339801788330078125) + Vec3fEq(56.66666412353515625, 460, -2.5371043682098388671875), + Vec3fEq(76.4206390380859375, 439.688446044921875, -2.913421630859375), + Vec3fEq(96.17461395263671875, 419.37689208984375, -4.508244037628173828125), + Vec3fEq(115.9285888671875, 399.06536865234375, -6.103069305419921875), + Vec3fEq(135.68255615234375, 378.753814697265625, -7.69789028167724609375), + Vec3fEq(155.4365386962890625, 358.44232177734375, -20.9574832916259765625), + Vec3fEq(175.190521240234375, 338.130767822265625, -35.907501220703125), + Vec3fEq(194.944488525390625, 317.8192138671875, -50.8574981689453125), + Vec3fEq(214.6984710693359375, 297.507720947265625, -65.8075103759765625), + Vec3fEq(222.0000457763671875, 290.00006103515625, -71.3334197998046875) )) << mPath; } @@ -1102,14 +1132,16 @@ namespace 0, -25, -100, -100, -100, }}; const HeightfieldSurface surface = makeSquareHeightfieldSurface(heightfieldData); + const int cellSize = mHeightfieldTileSize * (surface.mSize - 1); + const osg::Vec3f shift = getHeightfieldShift(mCellPosition, cellSize, surface.mMinHeight, surface.mMaxHeight); CollisionShapeInstance compound(std::make_unique()); compound.shape().addChildShape(btTransform(btMatrix3x3::getIdentity(), btVector3(204, -204, 0)), new btBoxShape(btVector3(100, 100, 1000))); mNavigator->addAgent(mAgentHalfExtents); - mNavigator->addHeightfield(mCellPosition, mHeightfieldTileSize * (surface.mSize - 1), mShift, surface); - mNavigator->addObject(ObjectId(&compound.shape()), ObjectShapes(compound.instance()), btTransform::getIdentity()); + mNavigator->addHeightfield(mCellPosition, cellSize, shift, surface); + mNavigator->addObject(ObjectId(&compound.shape()), ObjectShapes(compound.instance()), mTransform); mNavigator->update(mPlayerPosition); mNavigator->wait(mListener, WaitConditionType::allJobsDone); @@ -1119,25 +1151,24 @@ namespace Status::Success); EXPECT_THAT(mPath, ElementsAre( - Vec3fEq(-204, 204, -2.666739940643310546875), - Vec3fEq(-188.745635986328125, 180.1236114501953125, -4.578275203704833984375), - Vec3fEq(-173.49127197265625, 156.247222900390625, -6.489814281463623046875), - Vec3fEq(-158.2369232177734375, 132.370819091796875, -8.4013538360595703125), - Vec3fEq(-142.9825592041015625, 108.49443817138671875, -10.31289386749267578125), - Vec3fEq(-127.7281951904296875, 84.6180419921875, -17.4810466766357421875), - Vec3fEq(-112.47383880615234375, 60.7416534423828125, -27.6026020050048828125), - Vec3fEq(-97.21947479248046875, 36.865261077880859375, -37.724163055419921875), - Vec3fEq(-81.965118408203125, 12.98886966705322265625, -47.84572601318359375), - Vec3fEq(-66.71075439453125, -10.887523651123046875, -46.691577911376953125), - Vec3fEq(-51.45639801025390625, -34.763916015625, -32.085445404052734375), - Vec3fEq(-36.202037811279296875, -58.640308380126953125, -28.5217914581298828125), - Vec3fEq(-20.947673797607421875, -82.5167083740234375, -32.16143035888671875), - Vec3fEq(-5.693310260772705078125, -106.393096923828125, -35.8010711669921875), - Vec3fEq(9.56105327606201171875, -130.2694854736328125, -29.6399688720703125), - Vec3fEq(24.8154163360595703125, -154.1458740234375, -17.6428318023681640625), - Vec3fEq(40.0697784423828125, -178.0222625732421875, -10.46006107330322265625), - Vec3fEq(55.3241424560546875, -201.8986663818359375, -3.297139644622802734375), - Vec3fEq(56.66666412353515625, -204, -2.6667373180389404296875) + Vec3fEq(56.66666412353515625, 460, -2.5371043682098388671875), + Vec3fEq(71.5649566650390625, 435.899810791015625, -5.817593097686767578125), + Vec3fEq(86.46324920654296875, 411.79962158203125, -9.66499996185302734375), + Vec3fEq(101.36154937744140625, 387.699462890625, -13.512401580810546875), + Vec3fEq(116.2598419189453125, 363.599273681640625, -17.359806060791015625), + Vec3fEq(131.1581268310546875, 339.499114990234375, -21.2072086334228515625), + Vec3fEq(146.056427001953125, 315.39892578125, -25.0546112060546875), + Vec3fEq(160.9547271728515625, 291.298736572265625, -28.9020137786865234375), + Vec3fEq(175.8530120849609375, 267.198577880859375, -32.749416351318359375), + Vec3fEq(190.751312255859375, 243.098388671875, -33.819454193115234375), + Vec3fEq(205.64959716796875, 218.9982147216796875, -31.020172119140625), + Vec3fEq(220.5478973388671875, 194.898040771484375, -26.844608306884765625), + Vec3fEq(235.446197509765625, 170.7978668212890625, -26.785541534423828125), + Vec3fEq(250.3444671630859375, 146.6976776123046875, -26.7264766693115234375), + Vec3fEq(265.242767333984375, 122.59751129150390625, -20.59339141845703125), + Vec3fEq(280.141021728515625, 98.4973297119140625, -14.040531158447265625), + Vec3fEq(295.039306640625, 74.39715576171875, -7.48766994476318359375), + Vec3fEq(306, 56.66666412353515625, -2.6667339801788330078125) )) << mPath; } } diff --git a/apps/openmw_test_suite/detournavigator/operators.hpp b/apps/openmw_test_suite/detournavigator/operators.hpp index 92740c65f1..fb6fcc5c39 100644 --- a/apps/openmw_test_suite/detournavigator/operators.hpp +++ b/apps/openmw_test_suite/detournavigator/operators.hpp @@ -48,7 +48,7 @@ namespace testing template <> inline testing::Message& Message::operator <<(const osg::Vec3f& value) { - return (*this) << "osg::Vec3f(" << std::setprecision(std::numeric_limits::max_exponent10) << value.x() + return (*this) << "Vec3fEq(" << std::setprecision(std::numeric_limits::max_exponent10) << value.x() << ", " << std::setprecision(std::numeric_limits::max_exponent10) << value.y() << ", " << std::setprecision(std::numeric_limits::max_exponent10) << value.z() << ')'; diff --git a/components/detournavigator/makenavmesh.cpp b/components/detournavigator/makenavmesh.cpp index c53fcdcc48..890e2c994b 100644 --- a/components/detournavigator/makenavmesh.cpp +++ b/components/detournavigator/makenavmesh.cpp @@ -357,6 +357,7 @@ namespace rcPolyMeshDetail& polyMeshDetail) { rcCompactHeightfield compact; + compact.dist = nullptr; buildCompactHeightfield(context, config.walkableHeight, config.walkableClimb, solid, compact); erodeWalkableArea(context, config.walkableRadius, compact);