diff --git a/FishROV/Assets/Scripts/Benchmark/FrameworkAdapters/Rvo2Adapter.cs b/FishROV/Assets/Scripts/Benchmark/FrameworkAdapters/Rvo2Adapter.cs index 119018c..0945e67 100644 --- a/FishROV/Assets/Scripts/Benchmark/FrameworkAdapters/Rvo2Adapter.cs +++ b/FishROV/Assets/Scripts/Benchmark/FrameworkAdapters/Rvo2Adapter.cs @@ -12,7 +12,7 @@ namespace FishROV.AvoidanceBenchmark private AvoidanceBenchmarkConfig config; private bool initialized; - public string Name => "RVO2 / RVO2-3D 本地避障"; + public string Name => "RVO2 本地避障"; public bool IsAvailable => bridge != null && bridge.IsAvailable; public string Status => bridge != null ? bridge.Status @@ -52,6 +52,7 @@ namespace FishROV.AvoidanceBenchmark return; } + bridge.SetAgentPosition(id, agent.transform.position); bridge.SetPreferredVelocity(id, preferredVelocity); } @@ -104,6 +105,7 @@ namespace FishROV.AvoidanceBenchmark private readonly string status; private readonly MethodInfo addAgent; private readonly MethodInfo setAgentPrefVelocity; + private readonly MethodInfo setAgentPosition; private readonly MethodInfo doStep; private readonly MethodInfo getAgentVelocity; private readonly MethodInfo setTimeStep; @@ -119,6 +121,7 @@ namespace FishROV.AvoidanceBenchmark string status, MethodInfo addAgent, MethodInfo setAgentPrefVelocity, + MethodInfo setAgentPosition, MethodInfo doStep, MethodInfo getAgentVelocity, MethodInfo setTimeStep, @@ -133,6 +136,7 @@ namespace FishROV.AvoidanceBenchmark this.status = status; this.addAgent = addAgent; this.setAgentPrefVelocity = setAgentPrefVelocity; + this.setAgentPosition = setAgentPosition; this.doStep = doStep; this.getAgentVelocity = getAgentVelocity; this.setTimeStep = setTimeStep; @@ -168,6 +172,7 @@ namespace FishROV.AvoidanceBenchmark var addAgent = FindMethod(simulatorType, "addAgent", "AddAgent", new[] { agentVectorType }); var setAgentPrefVelocity = FindMethod(simulatorType, "setAgentPrefVelocity", "SetAgentPrefVelocity", new[] { typeof(int), agentVectorType }); + var setAgentPosition = FindMethod(simulatorType, "setAgentPosition", "SetAgentPosition", new[] { typeof(int), agentVectorType }); var getAgentVelocity = FindMethod(simulatorType, "getAgentVelocity", "GetAgentVelocity", new[] { typeof(int) }); var doStep = FindMethod(simulatorType, "doStep", "DoStep", Type.EmptyTypes); var setTimeStep = FindMethod(simulatorType, "setTimeStep", "SetTimeStep", new[] { typeof(float) }); @@ -188,6 +193,7 @@ namespace FishROV.AvoidanceBenchmark use3D ? "RVO2-3D 已通过反射接入。" : "RVO2 已通过反射接入。", addAgent, setAgentPrefVelocity, + setAgentPosition, doStep, getAgentVelocity, setTimeStep, @@ -212,6 +218,23 @@ namespace FishROV.AvoidanceBenchmark } } + public void SetAgentPosition(int id, Vector3 position) + { + if (setAgentPosition == null) + { + return; + } + + try + { + setAgentPosition.Invoke(simulator, new[] { id, ToRvoVector(position) }); + } + catch (Exception exception) + { + Debug.LogWarning($"RVO2 同步 Agent 位置失败：{exception.Message}"); + } + } + public void SetPreferredVelocity(int id, Vector3 preferredVelocity) { try @@ -435,7 +458,7 @@ namespace FishROV.AvoidanceBenchmark private static Rvo2ReflectionBridge Unavailable(string status = MissingPackageStatus) { - return new Rvo2ReflectionBridge(null, null, null, null, false, status, null, null, null, null, null, null, null); + return new Rvo2ReflectionBridge(null, null, null, null, false, status, null, null, null, null, null, null, null, null); } private static object CreateOrGetSimulator(Type type) diff --git a/FishROV/Assets/ThirdParty.meta b/FishROV/Assets/ThirdParty.meta new file mode 100644 index 0000000..a0c4a41 --- /dev/null +++ b/FishROV/Assets/ThirdParty.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: 08562b8a001a1ee459ff33a1ad143d23 +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/FishROV/Assets/ThirdParty/RVO2-CS.meta b/FishROV/Assets/ThirdParty/RVO2-CS.meta new file mode 100644 index 0000000..d111da2 --- /dev/null +++ b/FishROV/Assets/ThirdParty/RVO2-CS.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: 3d0866b55235ff948be97fbeb160eb12 +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/FishROV/Assets/ThirdParty/RVO2-CS/LICENSE b/FishROV/Assets/ThirdParty/RVO2-CS/LICENSE new file mode 100644 index 0000000..d645695 --- /dev/null +++ b/FishROV/Assets/ThirdParty/RVO2-CS/LICENSE @@ -0,0 +1,202 @@ + + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "[]" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright [yyyy] [name of copyright owner] + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. diff --git a/FishROV/Assets/ThirdParty/RVO2-CS/LICENSE.meta b/FishROV/Assets/ThirdParty/RVO2-CS/LICENSE.meta new file mode 100644 index 0000000..d61cc4c --- /dev/null +++ b/FishROV/Assets/ThirdParty/RVO2-CS/LICENSE.meta @@ -0,0 +1,7 @@ +fileFormatVersion: 2 +guid: 2e6d028a17551e240ae4528dcafd0158 +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/FishROV/Assets/ThirdParty/RVO2-CS/LICENSES.meta b/FishROV/Assets/ThirdParty/RVO2-CS/LICENSES.meta new file mode 100644 index 0000000..df242a2 --- /dev/null +++ b/FishROV/Assets/ThirdParty/RVO2-CS/LICENSES.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: cb86a7b56767e3b4e9c9b2614f8dfaf0 +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/FishROV/Assets/ThirdParty/RVO2-CS/LICENSES/Apache-2.0.txt b/FishROV/Assets/ThirdParty/RVO2-CS/LICENSES/Apache-2.0.txt new file mode 100644 index 0000000..0605966 --- /dev/null +++ b/FishROV/Assets/ThirdParty/RVO2-CS/LICENSES/Apache-2.0.txt @@ -0,0 +1,190 @@ + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + +TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + +1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, and + distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by the + copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all other + entities that control, are controlled by, or are under common control with + that entity. For the purposes of this definition, "control" means (i) the + power, direct or indirect, to cause the direction or management of such + entity, whether by contract or otherwise, or (ii) ownership of fifty percent + (50%) or more of the outstanding shares, or (iii) beneficial ownership of + such entity. + + "You" (or "Your") shall mean an individual or Legal Entity exercising + permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation source, and + configuration files. + + "Object" form shall mean any form resulting from mechanical transformation + or translation of a Source form, including but not limited to compiled + object code, generated documentation, and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or Object form, + made available under the License, as indicated by a copyright notice that is + included in or attached to the work (an example is provided in the Appendix + below). + + "Derivative Works" shall mean any work, whether in Source or Object form, + that is based on (or derived from) the Work and for which the editorial + revisions, annotations, elaborations, or other modifications represent, as a + whole, an original work of authorship. For the purposes of this License, + Derivative Works shall not include works that remain separable from, or + merely link (or bind by name) to the interfaces of, the Work and Derivative + Works thereof. + + "Contribution" shall mean any work of authorship, including the original + version of the Work and any modifications or additions to that Work or + Derivative Works thereof, that is intentionally submitted to Licensor for + inclusion in the Work by the copyright owner or by an individual or Legal + Entity authorized to submit on behalf of the copyright owner. For the + purposes of this definition, "submitted" means any form of electronic, + verbal, or written communication sent to the Licensor or its + representatives, including but not limited to communication on electronic + mailing lists, source code control systems, and issue tracking systems that + are managed by, or on behalf of, the Licensor for the purpose of discussing + and improving the Work, but excluding communication that is conspicuously + marked or otherwise designated in writing by the copyright owner as "Not a + Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity on + behalf of whom a Contribution has been received by Licensor and subsequently + incorporated within the Work. + +2. Grant of Copyright License. Subject to the terms and conditions of this + License, each Contributor hereby grants to You a perpetual, worldwide, + non-exclusive, no-charge, royalty-free, irrevocable copyright license to + reproduce, prepare Derivative Works of, publicly display, publicly perform, + sublicense, and distribute the Work and such Derivative Works in Source or + Object form. + +3. Grant of Patent License. Subject to the terms and conditions of this + License, each Contributor hereby grants to You a perpetual, worldwide, + non-exclusive, no-charge, royalty-free, irrevocable (except as stated in + this section) patent license to make, have made, use, offer to sell, sell, + import, and otherwise transfer the Work, where such license applies only to + those patent claims licensable by such Contributor that are necessarily + infringed by their Contribution(s) alone or by combination of their + Contribution(s) with the Work to which such Contribution(s) was submitted. + If You institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work or a + Contribution incorporated within the Work constitutes direct or contributory + patent infringement, then any patent licenses granted to You under this + License for that Work shall terminate as of the date such litigation is + filed. + +4. Redistribution. You may reproduce and distribute copies of the Work or + Derivative Works thereof in any medium, with or without modifications, and + in Source or Object form, provided that You meet the following conditions: + + (a) You must give any other recipients of the Work or Derivative Works a + copy of this License; and + + (b) You must cause any modified files to carry prominent notices stating + that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works that You + distribute, all copyright, patent, trademark, and attribution notices + from the Source form of the Work, excluding those notices that do not + pertain to any part of the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its distribution, + then any Derivative Works that You distribute must include a readable + copy of the attribution notices contained within such NOTICE file, + excluding those notices that do not pertain to any part of the + Derivative Works, in at least one of the following places: within a + NOTICE text file distributed as part of the Derivative Works; within the + Source form or documentation, if provided along with the Derivative + Works; or, within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents of the + NOTICE file are for informational purposes only and do not modify the + License. You may add Your own attribution notices within Derivative + Works that You distribute, alongside or as an addendum to the NOTICE + text from the Work, provided that such additional attribution notices + cannot be construed as modifying the License. + + You may add Your own copyright statement to Your modifications and may + provide additional or different license terms and conditions for use, + reproduction, or distribution of Your modifications, or for any such + Derivative Works as a whole, provided Your use, reproduction, and + distribution of the Work otherwise complies with the conditions stated in + this License. + +5. Submission of Contributions. Unless You explicitly state otherwise, any + Contribution intentionally submitted for inclusion in the Work by You to the + Licensor shall be under the terms and conditions of this License, without + any additional terms or conditions. Notwithstanding the above, nothing + herein shall supersede or modify the terms of any separate license agreement + you may have executed with Licensor regarding such Contributions. + +6. Trademarks. This License does not grant permission to use the trade names, + trademarks, service marks, or product names of the Licensor, except as + required for reasonable and customary use in describing the origin of the + Work and reproducing the content of the NOTICE file. + +7. Disclaimer of Warranty. Unless required by applicable law or agreed to in + writing, Licensor provides the Work (and each Contributor provides its + Contributions) on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY + KIND, either express or implied, including, without limitation, any + warranties or conditions of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or + FITNESS FOR A PARTICULAR PURPOSE. You are solely responsible for determining + the appropriateness of using or redistributing the Work and assume any risks + associated with Your exercise of permissions under this License. + +8. Limitation of Liability. In no event and under no legal theory, whether in + tort (including negligence), contract, or otherwise, unless required by + applicable law (such as deliberate and grossly negligent acts) or agreed to + in writing, shall any Contributor be liable to You for damages, including + any direct, indirect, special, incidental, or consequential damages of any + character arising as a result of this License or out of the use or inability + to use the Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all other + commercial damages or losses), even if such Contributor has been advised of + the possibility of such damages. + +9. Accepting Warranty or Additional Liability. While redistributing the Work or + Derivative Works thereof, You may choose to offer, and charge a fee for, + acceptance of support, warranty, indemnity, or other liability obligations + and/or rights consistent with this License. However, in accepting such + obligations, You may act only on Your own behalf and on Your sole + responsibility, not on behalf of any other Contributor, and only if You + agree to indemnify, defend, and hold each Contributor harmless for any + liability incurred by, or claims asserted against, such Contributor by + reason of your accepting any such warranty or additional liability. + +END OF TERMS AND CONDITIONS + +APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following boilerplate + notice, with the fields enclosed by brackets "[]" replaced with your own + identifying information. (Don't include the brackets!) The text should be + enclosed in the appropriate comment syntax for the file format. We also + recommend that a file or class name and description of purpose be included + on the same "printed page" as the copyright notice for easier identification + within third-party archives. + + Copyright [yyyy] [name of copyright owner] + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. diff --git a/FishROV/Assets/ThirdParty/RVO2-CS/LICENSES/Apache-2.0.txt.meta b/FishROV/Assets/ThirdParty/RVO2-CS/LICENSES/Apache-2.0.txt.meta new file mode 100644 index 0000000..db77208 --- /dev/null +++ b/FishROV/Assets/ThirdParty/RVO2-CS/LICENSES/Apache-2.0.txt.meta @@ -0,0 +1,7 @@ +fileFormatVersion: 2 +guid: a6d8b6920c5e24c40ac8c3b79ef24675 +TextScriptImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/FishROV/Assets/ThirdParty/RVO2-CS/README.md b/FishROV/Assets/ThirdParty/RVO2-CS/README.md new file mode 100644 index 0000000..253c95e --- /dev/null +++ b/FishROV/Assets/ThirdParty/RVO2-CS/README.md @@ -0,0 +1,111 @@ + + +Optimal Reciprocal Collision Avoidance for C# +============================================= + + + +[![DOI](https://zenodo.org/badge/45011155.svg)](https://zenodo.org/badge/latestdoi/45011155) + +We present a formal approach to reciprocal collision avoidance, where multiple +independent mobile robots or agents need to avoid collisions with each other +without communication among agents while moving in a common workspace. Our +formulation, optimal reciprocal collision avoidance (ORCA), provides sufficient +conditions for collision-free motion by letting each agent take half of the +responsibility of avoiding pairwise collisions. Selecting the optimal action for +each agent is reduced to solving a low-dimensional linear program, and we prove +that the resulting motions are smooth. We test our optimal reciprocal collision +avoidance approach on several dense and complex simulation scenarios workspaces +involving thousands of agents, and compute collision-free actions for all of +them in only a few milliseconds. + +RVO2 Library C# is an open-source C# .NET 10 implementation of our algorithm in +two dimensions. It has a simple API for third-party applications. The user +specifies static obstacles, agents, and the preferred velocities of the agents. +The simulation is performed step-by-step via a simple call to the library. The +simulation is fully accessible and manipulable during runtime. + +![Build Status](https://github.com/snape/RVO2-CS/actions/workflows/ci.yml/badge.svg?branch=main) + + +SPDX-FileCopyrightText: 2008 University of North Carolina at Chapel Hill +SPDX-License-Identifier: Apache-2.0 + +Licensed under the Apache License, Version 2.0 (the "License"); +you may not use this file except in compliance with the License. +You may obtain a copy of the License at + +   + +Unless required by applicable law or agreed to in writing, software +distributed under the License is distributed on an "AS IS" BASIS, +WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +See the License for the specific language governing permissions and +limitations under the License. + +Please send all bug reports to [geom@cs.unc.edu](mailto:geom@cs.unc.edu). + +The authors may be contacted via: + +Jur van den Berg, Stephen J. Guy, Jamie Snape, Ming C. Lin, Dinesh Manocha +Dept. of Computer Science +201 S. Columbia St. +Frederick P. Brooks, Jr. Computer Science Bldg. +Chapel Hill, N.C. 27599-3175 +United States of America + diff --git a/FishROV/Assets/ThirdParty/RVO2-CS/README.md.meta b/FishROV/Assets/ThirdParty/RVO2-CS/README.md.meta new file mode 100644 index 0000000..a48f417 --- /dev/null +++ b/FishROV/Assets/ThirdParty/RVO2-CS/README.md.meta @@ -0,0 +1,7 @@ +fileFormatVersion: 2 +guid: d4db5e3fc23bec64cb3ea454f330224a +TextScriptImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/FishROV/Assets/ThirdParty/RVO2-CS/RVOCS.meta b/FishROV/Assets/ThirdParty/RVO2-CS/RVOCS.meta new file mode 100644 index 0000000..1213a8c --- /dev/null +++ b/FishROV/Assets/ThirdParty/RVO2-CS/RVOCS.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: 6af893cb0842767489c05c232f3c840c +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/FishROV/Assets/ThirdParty/RVO2-CS/RVOCS/Agent.cs b/FishROV/Assets/ThirdParty/RVO2-CS/RVOCS/Agent.cs new file mode 100644 index 0000000..8fc11ff --- /dev/null +++ b/FishROV/Assets/ThirdParty/RVO2-CS/RVOCS/Agent.cs @@ -0,0 +1,719 @@ +/* + * Agent.cs + * RVO2 Library C# + * + * SPDX-FileCopyrightText: 2008 University of North Carolina at Chapel Hill + * SPDX-License-Identifier: Apache-2.0 + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + * Please send all bug reports to . + * + * The authors may be contacted via: + * + * Jur van den Berg, Stephen J. Guy, Jamie Snape, Ming C. Lin, Dinesh Manocha + * Dept. of Computer Science + * 201 S. Columbia St. + * Frederick P. Brooks, Jr. Computer Science Bldg. + * Chapel Hill, N.C. 27599-3175 + * United States of America + * + * + */ + +using System; +using System.Collections.Generic; + +namespace RVO +{ + /// Defines an agent in the simulation. + internal class Agent + { + internal IList> _agentNeighbors = new List>(); + internal IList> _obstacleNeighbors = new List>(); + internal IList _orcaLines = new List(); + internal Vector2 _position; + internal Vector2 _prefVelocity; + internal Vector2 _velocity; + internal int _id = 0; + internal int _maxNeighbors = 0; + internal float _maxSpeed = 0.0f; + internal float _neighborDist = 0.0f; + internal float _radius = 0.0f; + internal float _timeHorizon = 0.0f; + internal float _timeHorizonObst = 0.0f; + + private Vector2 _newVelocity; + + /// Computes the neighbors of this agent. + internal void ComputeNeighbors() + { + _obstacleNeighbors.Clear(); + float range = _timeHorizonObst * _maxSpeed + _radius; + float rangeSq = range * range; + Simulator.Instance._kdTree.ComputeObstacleNeighbors(this, rangeSq); + + _agentNeighbors.Clear(); + + if (_maxNeighbors > 0) + { + rangeSq = _neighborDist * _neighborDist; + Simulator.Instance._kdTree.ComputeAgentNeighbors(this, ref rangeSq); + } + } + + /// Computes the new velocity of this agent. + internal void ComputeNewVelocity() + { + _orcaLines.Clear(); + + float invTimeHorizonObst = 1.0f / _timeHorizonObst; + + /* Create obstacle ORCA lines. */ + for (int i = 0; i < _obstacleNeighbors.Count; ++i) + { + + Obstacle obstacle1 = _obstacleNeighbors[i].Value; + Obstacle obstacle2 = obstacle1._next; + + Vector2 relativePosition1 = obstacle1._point - _position; + Vector2 relativePosition2 = obstacle2._point - _position; + + /* + * Check if velocity obstacle of obstacle is already taken care + * of by previously constructed obstacle ORCA lines. + */ + bool alreadyCovered = false; + + for (int j = 0; j < _orcaLines.Count; ++j) + { + if (RVOMath.Det(invTimeHorizonObst * relativePosition1 - _orcaLines[j].Point, _orcaLines[j].Direction) - invTimeHorizonObst * _radius >= -RVOMath.RVO_EPSILON && RVOMath.Det(invTimeHorizonObst * relativePosition2 - _orcaLines[j].Point, _orcaLines[j].Direction) - invTimeHorizonObst * _radius >= -RVOMath.RVO_EPSILON) + { + alreadyCovered = true; + + break; + } + } + + if (alreadyCovered) + { + continue; + } + + /* Not yet covered. Check for collisions. */ + float distSq1 = RVOMath.AbsSq(relativePosition1); + float distSq2 = RVOMath.AbsSq(relativePosition2); + + float radiusSq = _radius * _radius; + + Vector2 obstacleVector = obstacle2._point - obstacle1._point; + float s = (-relativePosition1 * obstacleVector) / RVOMath.AbsSq(obstacleVector); + float distSqLine = RVOMath.AbsSq(-relativePosition1 - s * obstacleVector); + + Line line; + + if (s < 0.0f && distSq1 <= radiusSq) + { + /* Collision with left vertex. Ignore if non-convex. */ + if (obstacle1._convex) + { + line.Point = new Vector2(0.0f, 0.0f); + line.Direction = RVOMath.Normalize(new Vector2(-relativePosition1.Y, relativePosition1.X)); + _orcaLines.Add(line); + } + + continue; + } + else if (s > 1.0f && distSq2 <= radiusSq) + { + /* + * Collision with right vertex. Ignore if non-convex or if + * it will be taken care of by neighboring obstacle. + */ + if (obstacle2._convex && RVOMath.Det(relativePosition2, obstacle2._direction) >= 0.0f) + { + line.Point = new Vector2(0.0f, 0.0f); + line.Direction = RVOMath.Normalize(new Vector2(-relativePosition2.Y, relativePosition2.X)); + _orcaLines.Add(line); + } + + continue; + } + else if (s >= 0.0f && s <= 1.0f && distSqLine <= radiusSq) + { + /* Collision with obstacle segment. */ + line.Point = new Vector2(0.0f, 0.0f); + line.Direction = -obstacle1._direction; + _orcaLines.Add(line); + + continue; + } + + /* + * No collision. Compute legs. When obliquely viewed, both legs + * can come from a single vertex. Legs extend cut-off line when + * non-convex vertex. + */ + + Vector2 leftLegDirection, rightLegDirection; + + if (s < 0.0f && distSqLine <= radiusSq) + { + /* + * Obstacle viewed obliquely so that left vertex + * defines velocity obstacle. + */ + if (!obstacle1._convex) + { + /* Ignore obstacle. */ + continue; + } + + obstacle2 = obstacle1; + + float leg1 = MathF.Sqrt(distSq1 - radiusSq); + leftLegDirection = new Vector2(relativePosition1.X * leg1 - relativePosition1.Y * _radius, relativePosition1.X * _radius + relativePosition1.Y * leg1) / distSq1; + rightLegDirection = new Vector2(relativePosition1.X * leg1 + relativePosition1.Y * _radius, -relativePosition1.X * _radius + relativePosition1.Y * leg1) / distSq1; + } + else if (s > 1.0f && distSqLine <= radiusSq) + { + /* + * Obstacle viewed obliquely so that + * right vertex defines velocity obstacle. + */ + if (!obstacle2._convex) + { + /* Ignore obstacle. */ + continue; + } + + obstacle1 = obstacle2; + + float leg2 = MathF.Sqrt(distSq2 - radiusSq); + leftLegDirection = new Vector2(relativePosition2.X * leg2 - relativePosition2.Y * _radius, relativePosition2.X * _radius + relativePosition2.Y * leg2) / distSq2; + rightLegDirection = new Vector2(relativePosition2.X * leg2 + relativePosition2.Y * _radius, -relativePosition2.X * _radius + relativePosition2.Y * leg2) / distSq2; + } + else + { + /* Usual situation. */ + if (obstacle1._convex) + { + float leg1 = MathF.Sqrt(distSq1 - radiusSq); + leftLegDirection = new Vector2(relativePosition1.X * leg1 - relativePosition1.Y * _radius, relativePosition1.X * _radius + relativePosition1.Y * leg1) / distSq1; + } + else + { + /* Left vertex non-convex; left leg extends cut-off line. */ + leftLegDirection = -obstacle1._direction; + } + + if (obstacle2._convex) + { + float leg2 = MathF.Sqrt(distSq2 - radiusSq); + rightLegDirection = new Vector2(relativePosition2.X * leg2 + relativePosition2.Y * _radius, -relativePosition2.X * _radius + relativePosition2.Y * leg2) / distSq2; + } + else + { + /* Right vertex non-convex; right leg extends cut-off line. */ + rightLegDirection = obstacle1._direction; + } + } + + /* + * Legs can never point into neighboring edge when convex + * vertex, take cutoff-line of neighboring edge instead. If + * velocity projected on "foreign" leg, no constraint is added. + */ + + Obstacle leftNeighbor = obstacle1._previous; + + bool isLeftLegForeign = false; + bool isRightLegForeign = false; + + if (obstacle1._convex && RVOMath.Det(leftLegDirection, -leftNeighbor._direction) >= 0.0f) + { + /* Left leg points into obstacle. */ + leftLegDirection = -leftNeighbor._direction; + isLeftLegForeign = true; + } + + if (obstacle2._convex && RVOMath.Det(rightLegDirection, obstacle2._direction) <= 0.0f) + { + /* Right leg points into obstacle. */ + rightLegDirection = obstacle2._direction; + isRightLegForeign = true; + } + + /* Compute cut-off centers. */ + Vector2 leftCutOff = invTimeHorizonObst * (obstacle1._point - _position); + Vector2 rightCutOff = invTimeHorizonObst * (obstacle2._point - _position); + Vector2 cutOffVector = rightCutOff - leftCutOff; + + /* Project current velocity on velocity obstacle. */ + + /* Check if current velocity is projected on cutoff circles. */ + float t = obstacle1 == obstacle2 ? 0.5f : ((_velocity - leftCutOff) * cutOffVector) / RVOMath.AbsSq(cutOffVector); + float tLeft = (_velocity - leftCutOff) * leftLegDirection; + float tRight = (_velocity - rightCutOff) * rightLegDirection; + + if ((t < 0.0f && tLeft < 0.0f) || (obstacle1 == obstacle2 && tLeft < 0.0f && tRight < 0.0f)) + { + /* Project on left cut-off circle. */ + Vector2 unitW = RVOMath.Normalize(_velocity - leftCutOff); + + line.Direction = new Vector2(unitW.Y, -unitW.X); + line.Point = leftCutOff + _radius * invTimeHorizonObst * unitW; + _orcaLines.Add(line); + + continue; + } + else if (t > 1.0f && tRight < 0.0f) + { + /* Project on right cut-off circle. */ + Vector2 unitW = RVOMath.Normalize(_velocity - rightCutOff); + + line.Direction = new Vector2(unitW.Y, -unitW.X); + line.Point = rightCutOff + _radius * invTimeHorizonObst * unitW; + _orcaLines.Add(line); + + continue; + } + + /* + * Project on left leg, right leg, or cut-off line, whichever is + * closest to velocity. + */ + float distSqCutoff = (t < 0.0f || t > 1.0f || obstacle1 == obstacle2) ? float.PositiveInfinity : RVOMath.AbsSq(_velocity - (leftCutOff + t * cutOffVector)); + float distSqLeft = tLeft < 0.0f ? float.PositiveInfinity : RVOMath.AbsSq(_velocity - (leftCutOff + tLeft * leftLegDirection)); + float distSqRight = tRight < 0.0f ? float.PositiveInfinity : RVOMath.AbsSq(_velocity - (rightCutOff + tRight * rightLegDirection)); + + if (distSqCutoff <= distSqLeft && distSqCutoff <= distSqRight) + { + /* Project on cut-off line. */ + line.Direction = -obstacle1._direction; + line.Point = leftCutOff + _radius * invTimeHorizonObst * new Vector2(-line.Direction.Y, line.Direction.X); + _orcaLines.Add(line); + + continue; + } + + if (distSqLeft <= distSqRight) + { + /* Project on left leg. */ + if (isLeftLegForeign) + { + continue; + } + + line.Direction = leftLegDirection; + line.Point = leftCutOff + _radius * invTimeHorizonObst * new Vector2(-line.Direction.Y, line.Direction.X); + _orcaLines.Add(line); + + continue; + } + + /* Project on right leg. */ + if (isRightLegForeign) + { + continue; + } + + line.Direction = -rightLegDirection; + line.Point = rightCutOff + _radius * invTimeHorizonObst * new Vector2(-line.Direction.Y, line.Direction.X); + _orcaLines.Add(line); + } + + int numObstLines = _orcaLines.Count; + + float invTimeHorizon = 1.0f / _timeHorizon; + + /* Create agent ORCA lines. */ + for (int i = 0; i < _agentNeighbors.Count; ++i) + { + Agent other = _agentNeighbors[i].Value; + + Vector2 relativePosition = other._position - _position; + Vector2 relativeVelocity = _velocity - other._velocity; + float distSq = RVOMath.AbsSq(relativePosition); + float combinedRadius = _radius + other._radius; + float combinedRadiusSq = combinedRadius * combinedRadius; + + Line line; + Vector2 u; + + if (distSq > combinedRadiusSq) + { + /* No collision. */ + Vector2 w = relativeVelocity - invTimeHorizon * relativePosition; + + /* Vector from cutoff center to relative velocity. */ + float wLengthSq = RVOMath.AbsSq(w); + float dotProduct1 = w * relativePosition; + + if (dotProduct1 < 0.0f && dotProduct1 * dotProduct1 > combinedRadiusSq * wLengthSq) + { + /* Project on cut-off circle. */ + float wLength = MathF.Sqrt(wLengthSq); + Vector2 unitW = w / wLength; + + line.Direction = new Vector2(unitW.Y, -unitW.X); + u = (combinedRadius * invTimeHorizon - wLength) * unitW; + } + else + { + /* Project on legs. */ + float leg = MathF.Sqrt(distSq - combinedRadiusSq); + + if (RVOMath.Det(relativePosition, w) > 0.0f) + { + /* Project on left leg. */ + line.Direction = new Vector2(relativePosition.X * leg - relativePosition.Y * combinedRadius, relativePosition.X * combinedRadius + relativePosition.Y * leg) / distSq; + } + else + { + /* Project on right leg. */ + line.Direction = -new Vector2(relativePosition.X * leg + relativePosition.Y * combinedRadius, -relativePosition.X * combinedRadius + relativePosition.Y * leg) / distSq; + } + + float dotProduct2 = relativeVelocity * line.Direction; + u = dotProduct2 * line.Direction - relativeVelocity; + } + } + else + { + /* Collision. Project on cut-off circle of time timeStep. */ + float invTimeStep = 1.0f / Simulator.Instance.TimeStep; + + /* Vector from cutoff center to relative velocity. */ + Vector2 w = relativeVelocity - invTimeStep * relativePosition; + + float wLength = RVOMath.Abs(w); + Vector2 unitW = w / wLength; + + line.Direction = new Vector2(unitW.Y, -unitW.X); + u = (combinedRadius * invTimeStep - wLength) * unitW; + } + + line.Point = _velocity + 0.5f * u; + _orcaLines.Add(line); + } + + int lineFail = LinearProgram2(_orcaLines, _maxSpeed, _prefVelocity, false, out _newVelocity); + + if (lineFail < _orcaLines.Count) + { + LinearProgram3(_orcaLines, numObstLines, lineFail, _maxSpeed, ref _newVelocity); + } + } + + /// Inserts an agent neighbor into the set of neighbors of this + /// agent. + /// + /// A pointer to the agent to be inserted. + /// The squared range around this agent. + internal void InsertAgentNeighbor(Agent agent, ref float rangeSq) + { + if (this != agent) + { + float distSq = RVOMath.AbsSq(_position - agent._position); + + if (distSq < rangeSq) + { + if (_agentNeighbors.Count < _maxNeighbors) + { + _agentNeighbors.Add(new KeyValuePair(distSq, agent)); + } + + int i = _agentNeighbors.Count - 1; + + while (i != 0 && distSq < _agentNeighbors[i - 1].Key) + { + _agentNeighbors[i] = _agentNeighbors[i - 1]; + --i; + } + + _agentNeighbors[i] = new KeyValuePair(distSq, agent); + + if (_agentNeighbors.Count == _maxNeighbors) + { + rangeSq = _agentNeighbors[_agentNeighbors.Count - 1].Key; + } + } + } + } + + /// Inserts a static obstacle neighbor into the set of neighbors + /// of this agent. + /// + /// The number of the static obstacle to be + /// inserted. + /// The squared range around this agent. + internal void InsertObstacleNeighbor(Obstacle obstacle, float rangeSq) + { + Obstacle nextObstacle = obstacle._next; + + float distSq = RVOMath.DistSqPointLineSegment(obstacle._point, nextObstacle._point, _position); + + if (distSq < rangeSq) + { + _obstacleNeighbors.Add(new KeyValuePair(distSq, obstacle)); + + int i = _obstacleNeighbors.Count - 1; + + while (i != 0 && distSq < _obstacleNeighbors[i - 1].Key) + { + _obstacleNeighbors[i] = _obstacleNeighbors[i - 1]; + --i; + } + _obstacleNeighbors[i] = new KeyValuePair(distSq, obstacle); + } + } + + /// Updates the two-dimensional position and two-dimensional + /// velocity of this agent. + internal void Update() + { + _velocity = _newVelocity; + + if (RVOMath.AbsSq(_velocity) > _maxSpeed * _maxSpeed) + { + _velocity = RVOMath.Normalize(_velocity) * _maxSpeed; + } + + _position += _velocity * Simulator.Instance.TimeStep; + } + + /// Solves a one-dimensional linear program on a specified line + /// subject to linear constraints defined by lines and a circular + /// constraint. + /// + /// True if successful. + /// + /// Lines defining the linear constraints. + /// The specified line constraint. + /// The radius of the circular constraint. + /// The optimization velocity. + /// True if the direction should be optimized. + /// + /// A reference to the result of the linear program. + /// + private bool LinearProgram1(IList lines, int lineNo, float radius, Vector2 optVelocity, bool directionOpt, ref Vector2 result) + { + float dotProduct = lines[lineNo].Point * lines[lineNo].Direction; + float discriminant = dotProduct * dotProduct + radius * radius - RVOMath.AbsSq(lines[lineNo].Point); + + if (discriminant < 0.0f) + { + /* Max speed circle fully invalidates line lineNo. */ + return false; + } + + float sqrtDiscriminant = MathF.Sqrt(discriminant); + float tLeft = -dotProduct - sqrtDiscriminant; + float tRight = -dotProduct + sqrtDiscriminant; + + for (int i = 0; i < lineNo; ++i) + { + float denominator = RVOMath.Det(lines[lineNo].Direction, lines[i].Direction); + float numerator = RVOMath.Det(lines[i].Direction, lines[lineNo].Point - lines[i].Point); + + if (MathF.Abs(denominator) <= RVOMath.RVO_EPSILON) + { + /* Lines lineNo and i are (almost) parallel. */ + if (numerator < 0.0f) + { + return false; + } + + continue; + } + + float t = numerator / denominator; + + if (denominator >= 0.0f) + { + /* Line i bounds line lineNo on the right. */ + tRight = Math.Min(tRight, t); + } + else + { + /* Line i bounds line lineNo on the left. */ + tLeft = Math.Max(tLeft, t); + } + + if (tLeft > tRight) + { + return false; + } + } + + if (directionOpt) + { + /* Optimize direction. */ + if (optVelocity * lines[lineNo].Direction > 0.0f) + { + /* Take right extreme. */ + result = lines[lineNo].Point + tRight * lines[lineNo].Direction; + } + else + { + /* Take left extreme. */ + result = lines[lineNo].Point + tLeft * lines[lineNo].Direction; + } + } + else + { + /* Optimize closest point. */ + float t = lines[lineNo].Direction * (optVelocity - lines[lineNo].Point); + + if (t < tLeft) + { + result = lines[lineNo].Point + tLeft * lines[lineNo].Direction; + } + else if (t > tRight) + { + result = lines[lineNo].Point + tRight * lines[lineNo].Direction; + } + else + { + result = lines[lineNo].Point + t * lines[lineNo].Direction; + } + } + + return true; + } + + /// Solves a two-dimensional linear program subject to linear + /// constraints defined by lines and a circular constraint. + /// + /// The number of the line it fails on, and the number of lines + /// if successful. + /// + /// Lines defining the linear constraints. + /// The radius of the circular constraint. + /// The optimization velocity. + /// True if the direction should be optimized. + /// + /// A reference to the result of the linear program. + /// + private int LinearProgram2(IList lines, float radius, Vector2 optVelocity, bool directionOpt, out Vector2 result) + { + if (directionOpt) + { + /* + * Optimize direction. Note that the optimization velocity is of + * unit length in this case. + */ + result = optVelocity * radius; + } + else if (RVOMath.AbsSq(optVelocity) > radius * radius) + { + /* Optimize closest point and outside circle. */ + result = RVOMath.Normalize(optVelocity) * radius; + } + else + { + /* Optimize closest point and inside circle. */ + result = optVelocity; + } + + for (int i = 0; i < lines.Count; ++i) + { + if (RVOMath.Det(lines[i].Direction, lines[i].Point - result) > 0.0f) + { + /* Result does not satisfy constraint i. Compute new optimal result. */ + Vector2 tempResult = result; + if (!LinearProgram1(lines, i, radius, optVelocity, directionOpt, ref result)) + { + result = tempResult; + + return i; + } + } + } + + return lines.Count; + } + + /// Solves a two-dimensional linear program subject to linear + /// constraints defined by lines and a circular constraint. + /// + /// Lines defining the linear constraints. + /// Count of obstacle lines. + /// The line on which the 2-d linear program + /// failed. + /// The radius of the circular constraint. + /// A reference to the result of the linear program. + /// + private void LinearProgram3(IList lines, int numObstLines, int beginLine, float radius, ref Vector2 result) + { + float distance = 0.0f; + + for (int i = beginLine; i < lines.Count; ++i) + { + if (RVOMath.Det(lines[i].Direction, lines[i].Point - result) > distance) + { + /* Result does not satisfy constraint of line i. */ + IList projLines = new List(); + for (int ii = 0; ii < numObstLines; ++ii) + { + projLines.Add(lines[ii]); + } + + for (int j = numObstLines; j < i; ++j) + { + Line line; + + float determinant = RVOMath.Det(lines[i].Direction, lines[j].Direction); + + if (MathF.Abs(determinant) <= RVOMath.RVO_EPSILON) + { + /* Line i and line j are parallel. */ + if (lines[i].Direction * lines[j].Direction > 0.0f) + { + /* Line i and line j point in the same direction. */ + continue; + } + else + { + /* Line i and line j point in opposite direction. */ + line.Point = 0.5f * (lines[i].Point + lines[j].Point); + } + } + else + { + line.Point = lines[i].Point + (RVOMath.Det(lines[j].Direction, lines[i].Point - lines[j].Point) / determinant) * lines[i].Direction; + } + + line.Direction = RVOMath.Normalize(lines[j].Direction - lines[i].Direction); + projLines.Add(line); + } + + Vector2 tempResult = result; + if (LinearProgram2(projLines, radius, new Vector2(-lines[i].Direction.Y, lines[i].Direction.X), true, out result) < projLines.Count) + { + /* + * This should in principle not happen. The result is by + * definition already in the feasible region of this + * linear program. If it fails, it is due to small + * floating point error, and the current result is kept. + */ + result = tempResult; + } + + distance = RVOMath.Det(lines[i].Direction, lines[i].Point - result); + } + } + } + } +} diff --git a/FishROV/Assets/ThirdParty/RVO2-CS/RVOCS/Agent.cs.meta b/FishROV/Assets/ThirdParty/RVO2-CS/RVOCS/Agent.cs.meta new file mode 100644 index 0000000..af6681e --- /dev/null +++ b/FishROV/Assets/ThirdParty/RVO2-CS/RVOCS/Agent.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 8acc64d87f8b9c14a966b32cd9aab419 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/FishROV/Assets/ThirdParty/RVO2-CS/RVOCS/KdTree.cs b/FishROV/Assets/ThirdParty/RVO2-CS/RVOCS/KdTree.cs new file mode 100644 index 0000000..6aab627 --- /dev/null +++ b/FishROV/Assets/ThirdParty/RVO2-CS/RVOCS/KdTree.cs @@ -0,0 +1,590 @@ +/* + * KdTree.cs + * RVO2 Library C# + * + * SPDX-FileCopyrightText: 2008 University of North Carolina at Chapel Hill + * SPDX-License-Identifier: Apache-2.0 + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + * Please send all bug reports to . + * + * The authors may be contacted via: + * + * Jur van den Berg, Stephen J. Guy, Jamie Snape, Ming C. Lin, Dinesh Manocha + * Dept. of Computer Science + * 201 S. Columbia St. + * Frederick P. Brooks, Jr. Computer Science Bldg. + * Chapel Hill, N.C. 27599-3175 + * United States of America + * + * + */ + +using System; +using System.Collections.Generic; + +namespace RVO +{ + /// Defines k-D trees for agents and static obstacles in the + /// simulation. + internal class KdTree + { + /// Defines a node of an agent k-D tree. + private struct AgentTreeNode + { + internal int _begin; + internal int _end; + internal int _left; + internal int _right; + internal float _maxX; + internal float _maxY; + internal float _minX; + internal float _minY; + } + + /// Defines a pair of scalar values. + private struct FloatPair + { + private readonly float _a; + private readonly float _b; + + /// Constructs and initializes a pair of scalar + /// values. + /// + /// The first scalar value. + /// The second scalar value. + internal FloatPair(float a, float b) + { + _a = a; + _b = b; + } + + /// Returns true if the first pair of scalar values is less + /// than the second pair of scalar values. + /// + /// True if the first pair of scalar values is less than the + /// second pair of scalar values. + /// + /// The first pair of scalar values. + /// The second pair of scalar values. + public static bool operator <(FloatPair pair1, FloatPair pair2) + { + return pair1._a < pair2._a || pair1._a == pair2._a && pair1._b < pair2._b; + } + + /// Returns true if the first pair of scalar values is less + /// than or equal to the second pair of scalar values. + /// + /// True if the first pair of scalar values is less than or + /// equal to the second pair of scalar values. + /// + /// The first pair of scalar values. + /// The second pair of scalar values. + public static bool operator <=(FloatPair pair1, FloatPair pair2) + { + return (pair1._a == pair2._a && pair1._b == pair2._b) || pair1 < pair2; + } + + /// Returns true if the first pair of scalar values is + /// greater than the second pair of scalar values. + /// + /// True if the first pair of scalar values is greater than + /// the second pair of scalar values. + /// + /// The first pair of scalar values. + /// The second pair of scalar values. + public static bool operator >(FloatPair pair1, FloatPair pair2) + { + return !(pair1 <= pair2); + } + + /// Returns true if the first pair of scalar values is + /// greater than or equal to the second pair of scalar values. + /// + /// + /// True if the first pair of scalar values is greater than + /// or equal to the second pair of scalar values. + /// + /// The first pair of scalar values. + /// The second pair of scalar values. + public static bool operator >=(FloatPair pair1, FloatPair pair2) + { + return !(pair1 < pair2); + } + } + + /// Defines a node of an obstacle k-D tree. + private class ObstacleTreeNode + { + internal Obstacle _obstacle; + internal ObstacleTreeNode _left; + internal ObstacleTreeNode _right; + }; + + /// The maximum size of an agent k-D tree leaf. + /* Empirically chosen; balances tree depth against per-leaf work. */ + private const int MAX_LEAF_SIZE = 10; + + private Agent[] _agents; + private AgentTreeNode[] _agentTree; + private ObstacleTreeNode _obstacleTree; + + /// Builds an agent k-D tree. + internal void BuildAgentTree() + { + if (_agents is null || _agents.Length != Simulator.Instance._agents.Count) + { + _agents = new Agent[Simulator.Instance._agents.Count]; + + for (int i = 0; i < _agents.Length; ++i) + { + _agents[i] = Simulator.Instance._agents[i]; + } + + _agentTree = new AgentTreeNode[2 * _agents.Length]; + + for (int i = 0; i < _agentTree.Length; ++i) + { + _agentTree[i] = new AgentTreeNode(); + } + } + + if (_agents.Length != 0) + { + BuildAgentTreeRecursive(0, _agents.Length, 0); + } + } + + /// Builds an obstacle k-D tree. + internal void BuildObstacleTree() + { + _obstacleTree = new ObstacleTreeNode(); + + IList obstacles = new List(Simulator.Instance._obstacles.Count); + + for (int i = 0; i < Simulator.Instance._obstacles.Count; ++i) + { + obstacles.Add(Simulator.Instance._obstacles[i]); + } + + _obstacleTree = BuildObstacleTreeRecursive(obstacles); + } + + /// Computes the agent neighbors of the specified agent. + /// + /// + /// The agent for which agent neighbors are to be + /// computed. + /// The squared range around the agent. + internal void ComputeAgentNeighbors(Agent agent, ref float rangeSq) + { + QueryAgentTreeRecursive(agent, ref rangeSq, 0); + } + + /// Computes the obstacle neighbors of the specified agent. + /// + /// + /// The agent for which obstacle neighbors are to be + /// computed. + /// The squared range around the agent. + internal void ComputeObstacleNeighbors(Agent agent, float rangeSq) + { + QueryObstacleTreeRecursive(agent, rangeSq, _obstacleTree); + } + + /// Queries the visibility between two points within a specified + /// radius. + /// + /// True if q1 and q2 are mutually visible within the radius; + /// false otherwise. + /// + /// The first point between which visibility is to be + /// tested. + /// The second point between which visibility is to be + /// tested. + /// The radius within which visibility is to be + /// tested. + internal bool QueryVisibility(Vector2 q1, Vector2 q2, float radius) + { + return QueryVisibilityRecursive(q1, q2, radius, _obstacleTree); + } + + /// Recursive method for building an agent k-D tree. + /// + /// The beginning agent k-D tree node node index. + /// + /// The ending agent k-D tree node index. + /// The current agent k-D tree node index. + private void BuildAgentTreeRecursive(int begin, int end, int node) + { + _agentTree[node]._begin = begin; + _agentTree[node]._end = end; + _agentTree[node]._minX = _agentTree[node]._maxX = _agents[begin]._position._x; + _agentTree[node]._minY = _agentTree[node]._maxY = _agents[begin]._position._y; + + for (int i = begin + 1; i < end; ++i) + { + _agentTree[node]._maxX = Math.Max(_agentTree[node]._maxX, _agents[i]._position._x); + _agentTree[node]._minX = Math.Min(_agentTree[node]._minX, _agents[i]._position._x); + _agentTree[node]._maxY = Math.Max(_agentTree[node]._maxY, _agents[i]._position._y); + _agentTree[node]._minY = Math.Min(_agentTree[node]._minY, _agents[i]._position._y); + } + + if (end - begin > MAX_LEAF_SIZE) + { + /* No leaf node. */ + bool isVertical = _agentTree[node]._maxX - _agentTree[node]._minX > _agentTree[node]._maxY - _agentTree[node]._minY; + float splitValue = 0.5f * (isVertical ? _agentTree[node]._maxX + _agentTree[node]._minX : _agentTree[node]._maxY + _agentTree[node]._minY); + + int left = begin; + int right = end; + + while (left < right) + { + while (left < right && (isVertical ? _agents[left]._position._x : _agents[left]._position._y) < splitValue) + { + ++left; + } + + while (right > left && (isVertical ? _agents[right - 1]._position._x : _agents[right - 1]._position._y) >= splitValue) + { + --right; + } + + if (left < right) + { + Agent tempAgent = _agents[left]; + _agents[left] = _agents[right - 1]; + _agents[right - 1] = tempAgent; + ++left; + --right; + } + } + + int leftSize = left - begin; + + if (leftSize == 0) + { + ++leftSize; + ++left; + } + + _agentTree[node]._left = node + 1; + _agentTree[node]._right = node + 2 * leftSize; + + BuildAgentTreeRecursive(begin, left, _agentTree[node]._left); + BuildAgentTreeRecursive(left, end, _agentTree[node]._right); + } + } + + /// Recursive method for building an obstacle k-D tree. + /// + /// + /// An obstacle k-D tree node. + /// + /// A list of obstacles. + private ObstacleTreeNode BuildObstacleTreeRecursive(IList obstacles) + { + if (obstacles.Count == 0) + { + return null; + } + + ObstacleTreeNode node = new(); + + int optimalSplit = 0; + int minLeft = obstacles.Count; + int minRight = obstacles.Count; + + for (int i = 0; i < obstacles.Count; ++i) + { + int leftSize = 0; + int rightSize = 0; + + Obstacle obstacleI1 = obstacles[i]; + Obstacle obstacleI2 = obstacleI1._next; + + /* Compute optimal split node. */ + for (int j = 0; j < obstacles.Count; ++j) + { + if (i == j) + { + continue; + } + + Obstacle obstacleJ1 = obstacles[j]; + Obstacle obstacleJ2 = obstacleJ1._next; + + float j1LeftOfI = RVOMath.LeftOf(obstacleI1._point, obstacleI2._point, obstacleJ1._point); + float j2LeftOfI = RVOMath.LeftOf(obstacleI1._point, obstacleI2._point, obstacleJ2._point); + + if (j1LeftOfI >= -RVOMath.RVO_EPSILON && j2LeftOfI >= -RVOMath.RVO_EPSILON) + { + ++leftSize; + } + else if (j1LeftOfI <= RVOMath.RVO_EPSILON && j2LeftOfI <= RVOMath.RVO_EPSILON) + { + ++rightSize; + } + else + { + ++leftSize; + ++rightSize; + } + + if (new FloatPair(Math.Max(leftSize, rightSize), Math.Min(leftSize, rightSize)) >= new FloatPair(Math.Max(minLeft, minRight), Math.Min(minLeft, minRight))) + { + break; + } + } + + if (new FloatPair(Math.Max(leftSize, rightSize), Math.Min(leftSize, rightSize)) < new FloatPair(Math.Max(minLeft, minRight), Math.Min(minLeft, minRight))) + { + minLeft = leftSize; + minRight = rightSize; + optimalSplit = i; + } + } + + { + /* Build split node. */ + IList leftObstacles = new List(minLeft); + + for (int n = 0; n < minLeft; ++n) + { + leftObstacles.Add(null); + } + + IList rightObstacles = new List(minRight); + + for (int n = 0; n < minRight; ++n) + { + rightObstacles.Add(null); + } + + int leftCounter = 0; + int rightCounter = 0; + int i = optimalSplit; + + Obstacle obstacleI1 = obstacles[i]; + Obstacle obstacleI2 = obstacleI1._next; + + for (int j = 0; j < obstacles.Count; ++j) + { + if (i == j) + { + continue; + } + + Obstacle obstacleJ1 = obstacles[j]; + Obstacle obstacleJ2 = obstacleJ1._next; + + float j1LeftOfI = RVOMath.LeftOf(obstacleI1._point, obstacleI2._point, obstacleJ1._point); + float j2LeftOfI = RVOMath.LeftOf(obstacleI1._point, obstacleI2._point, obstacleJ2._point); + + if (j1LeftOfI >= -RVOMath.RVO_EPSILON && j2LeftOfI >= -RVOMath.RVO_EPSILON) + { + leftObstacles[leftCounter++] = obstacles[j]; + } + else if (j1LeftOfI <= RVOMath.RVO_EPSILON && j2LeftOfI <= RVOMath.RVO_EPSILON) + { + rightObstacles[rightCounter++] = obstacles[j]; + } + else + { + /* Split obstacle j. */ + float t = RVOMath.Det(obstacleI2._point - obstacleI1._point, obstacleJ1._point - obstacleI1._point) / RVOMath.Det(obstacleI2._point - obstacleI1._point, obstacleJ1._point - obstacleJ2._point); + + Vector2 splitPoint = obstacleJ1._point + t * (obstacleJ2._point - obstacleJ1._point); + + Obstacle newObstacle = new(); + newObstacle._point = splitPoint; + newObstacle._previous = obstacleJ1; + newObstacle._next = obstacleJ2; + newObstacle._convex = true; + newObstacle._direction = obstacleJ1._direction; + + newObstacle._id = Simulator.Instance._obstacles.Count; + + Simulator.Instance._obstacles.Add(newObstacle); + + obstacleJ1._next = newObstacle; + obstacleJ2._previous = newObstacle; + + if (j1LeftOfI > 0.0f) + { + leftObstacles[leftCounter++] = obstacleJ1; + rightObstacles[rightCounter++] = newObstacle; + } + else + { + rightObstacles[rightCounter++] = obstacleJ1; + leftObstacles[leftCounter++] = newObstacle; + } + } + } + + node._obstacle = obstacleI1; + node._left = BuildObstacleTreeRecursive(leftObstacles); + node._right = BuildObstacleTreeRecursive(rightObstacles); + + return node; + } + } + + /// Recursive method for computing the agent neighbors of the + /// specified agent. + /// + /// The agent for which agent neighbors are to be + /// computed. + /// The squared range around the agent. + /// The current agent k-D tree node index. + private void QueryAgentTreeRecursive(Agent agent, ref float rangeSq, int node) + { + if (_agentTree[node]._end - _agentTree[node]._begin <= MAX_LEAF_SIZE) + { + for (int i = _agentTree[node]._begin; i < _agentTree[node]._end; ++i) + { + agent.InsertAgentNeighbor(_agents[i], ref rangeSq); + } + } + else + { + int leftNode = _agentTree[node]._left; + float leftDx = Math.Max(0.0f, _agentTree[leftNode]._minX - agent._position._x) + Math.Max(0.0f, agent._position._x - _agentTree[leftNode]._maxX); + float leftDy = Math.Max(0.0f, _agentTree[leftNode]._minY - agent._position._y) + Math.Max(0.0f, agent._position._y - _agentTree[leftNode]._maxY); + float distSqLeft = leftDx * leftDx + leftDy * leftDy; + + int rightNode = _agentTree[node]._right; + float rightDx = Math.Max(0.0f, _agentTree[rightNode]._minX - agent._position._x) + Math.Max(0.0f, agent._position._x - _agentTree[rightNode]._maxX); + float rightDy = Math.Max(0.0f, _agentTree[rightNode]._minY - agent._position._y) + Math.Max(0.0f, agent._position._y - _agentTree[rightNode]._maxY); + float distSqRight = rightDx * rightDx + rightDy * rightDy; + + if (distSqLeft < distSqRight) + { + if (distSqLeft < rangeSq) + { + QueryAgentTreeRecursive(agent, ref rangeSq, leftNode); + + if (distSqRight < rangeSq) + { + QueryAgentTreeRecursive(agent, ref rangeSq, rightNode); + } + } + } + else + { + if (distSqRight < rangeSq) + { + QueryAgentTreeRecursive(agent, ref rangeSq, rightNode); + + if (distSqLeft < rangeSq) + { + QueryAgentTreeRecursive(agent, ref rangeSq, leftNode); + } + } + } + + } + } + + /// Recursive method for computing the obstacle neighbors of the + /// specified agent. + /// + /// The agent for which obstacle neighbors are to be + /// computed. + /// The squared range around the agent. + /// The current obstacle k-D node. + private void QueryObstacleTreeRecursive(Agent agent, float rangeSq, ObstacleTreeNode node) + { + if (node is not null) + { + Obstacle obstacle1 = node._obstacle; + Obstacle obstacle2 = obstacle1._next; + + float agentLeftOfLine = RVOMath.LeftOf(obstacle1._point, obstacle2._point, agent._position); + + QueryObstacleTreeRecursive(agent, rangeSq, agentLeftOfLine >= 0.0f ? node._left : node._right); + + float distSqLine = agentLeftOfLine * agentLeftOfLine / RVOMath.AbsSq(obstacle2._point - obstacle1._point); + + if (distSqLine < rangeSq) + { + if (agentLeftOfLine < 0.0f) + { + /* + * Try obstacle at this node only if agent is on right side of + * obstacle (and can see obstacle). + */ + agent.InsertObstacleNeighbor(node._obstacle, rangeSq); + } + + /* Try other side of line. */ + QueryObstacleTreeRecursive(agent, rangeSq, agentLeftOfLine >= 0.0f ? node._right : node._left); + } + } + } + + /// Recursive method for querying the visibility between two + /// points within a specified radius. + /// + /// True if q1 and q2 are mutually visible within the radius; + /// false otherwise. + /// + /// The first point between which visibility is to be + /// tested. + /// The second point between which visibility is to be + /// tested. + /// The radius within which visibility is to be + /// tested. + /// The current obstacle k-D node. + private bool QueryVisibilityRecursive(Vector2 q1, Vector2 q2, float radius, ObstacleTreeNode node) + { + if (node is null) + { + return true; + } + + Obstacle obstacle1 = node._obstacle; + Obstacle obstacle2 = obstacle1._next; + + float q1LeftOfI = RVOMath.LeftOf(obstacle1._point, obstacle2._point, q1); + float q2LeftOfI = RVOMath.LeftOf(obstacle1._point, obstacle2._point, q2); + float invLengthI = 1.0f / RVOMath.AbsSq(obstacle2._point - obstacle1._point); + float radiusSq = radius * radius; + + if (q1LeftOfI >= 0.0f && q2LeftOfI >= 0.0f) + { + return QueryVisibilityRecursive(q1, q2, radius, node._left) && ((q1LeftOfI * q1LeftOfI * invLengthI >= radiusSq && q2LeftOfI * q2LeftOfI * invLengthI >= radiusSq) || QueryVisibilityRecursive(q1, q2, radius, node._right)); + } + + if (q1LeftOfI <= 0.0f && q2LeftOfI <= 0.0f) + { + return QueryVisibilityRecursive(q1, q2, radius, node._right) && ((q1LeftOfI * q1LeftOfI * invLengthI >= radiusSq && q2LeftOfI * q2LeftOfI * invLengthI >= radiusSq) || QueryVisibilityRecursive(q1, q2, radius, node._left)); + } + + if (q1LeftOfI >= 0.0f && q2LeftOfI <= 0.0f) + { + /* One can see through obstacle from left to right. */ + return QueryVisibilityRecursive(q1, q2, radius, node._left) && QueryVisibilityRecursive(q1, q2, radius, node._right); + } + + float point1LeftOfQ = RVOMath.LeftOf(q1, q2, obstacle1._point); + float point2LeftOfQ = RVOMath.LeftOf(q1, q2, obstacle2._point); + float invLengthQ = 1.0f / RVOMath.AbsSq(q2 - q1); + + return point1LeftOfQ * point2LeftOfQ >= 0.0f && point1LeftOfQ * point1LeftOfQ * invLengthQ > radiusSq && point2LeftOfQ * point2LeftOfQ * invLengthQ > radiusSq && QueryVisibilityRecursive(q1, q2, radius, node._left) && QueryVisibilityRecursive(q1, q2, radius, node._right); + } + } +} diff --git a/FishROV/Assets/ThirdParty/RVO2-CS/RVOCS/KdTree.cs.meta b/FishROV/Assets/ThirdParty/RVO2-CS/RVOCS/KdTree.cs.meta new file mode 100644 index 0000000..6d1be7f --- /dev/null +++ b/FishROV/Assets/ThirdParty/RVO2-CS/RVOCS/KdTree.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 41302aad673ebad45936305f3222217c +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/FishROV/Assets/ThirdParty/RVO2-CS/RVOCS/Line.cs b/FishROV/Assets/ThirdParty/RVO2-CS/RVOCS/Line.cs new file mode 100644 index 0000000..11bd74f --- /dev/null +++ b/FishROV/Assets/ThirdParty/RVO2-CS/RVOCS/Line.cs @@ -0,0 +1,42 @@ +/* + * Line.cs + * RVO2 Library C# + * + * SPDX-FileCopyrightText: 2008 University of North Carolina at Chapel Hill + * SPDX-License-Identifier: Apache-2.0 + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + * Please send all bug reports to . + * + * The authors may be contacted via: + * + * Jur van den Berg, Stephen J. Guy, Jamie Snape, Ming C. Lin, Dinesh Manocha + * Dept. of Computer Science + * 201 S. Columbia St. + * Frederick P. Brooks, Jr. Computer Science Bldg. + * Chapel Hill, N.C. 27599-3175 + * United States of America + * + * + */ + +namespace RVO +{ + /// Defines a directed line. + public struct Line + { + public Vector2 Direction; + public Vector2 Point; + } +} diff --git a/FishROV/Assets/ThirdParty/RVO2-CS/RVOCS/Line.cs.meta b/FishROV/Assets/ThirdParty/RVO2-CS/RVOCS/Line.cs.meta new file mode 100644 index 0000000..7884ab7 --- /dev/null +++ b/FishROV/Assets/ThirdParty/RVO2-CS/RVOCS/Line.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 8cef7bba85dbe4647891612de7e749a2 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/FishROV/Assets/ThirdParty/RVO2-CS/RVOCS/Obstacle.cs b/FishROV/Assets/ThirdParty/RVO2-CS/RVOCS/Obstacle.cs new file mode 100644 index 0000000..fb4301d --- /dev/null +++ b/FishROV/Assets/ThirdParty/RVO2-CS/RVOCS/Obstacle.cs @@ -0,0 +1,47 @@ +/* + * Obstacle.cs + * RVO2 Library C# + * + * SPDX-FileCopyrightText: 2008 University of North Carolina at Chapel Hill + * SPDX-License-Identifier: Apache-2.0 + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + * Please send all bug reports to . + * + * The authors may be contacted via: + * + * Jur van den Berg, Stephen J. Guy, Jamie Snape, Ming C. Lin, Dinesh Manocha + * Dept. of Computer Science + * 201 S. Columbia St. + * Frederick P. Brooks, Jr. Computer Science Bldg. + * Chapel Hill, N.C. 27599-3175 + * United States of America + * + * + */ + +namespace RVO +{ + /// Defines static obstacles in the simulation. + internal class Obstacle + { + + internal Obstacle _next; + internal Obstacle _previous; + internal Vector2 _direction; + internal Vector2 _point; + internal int _id; + internal bool _convex; + } +} diff --git a/FishROV/Assets/ThirdParty/RVO2-CS/RVOCS/Obstacle.cs.meta b/FishROV/Assets/ThirdParty/RVO2-CS/RVOCS/Obstacle.cs.meta new file mode 100644 index 0000000..2430cb1 --- /dev/null +++ b/FishROV/Assets/ThirdParty/RVO2-CS/RVOCS/Obstacle.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 5b59751871022b84e88519071917ffa4 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/FishROV/Assets/ThirdParty/RVO2-CS/RVOCS/RVOMath.cs b/FishROV/Assets/ThirdParty/RVO2-CS/RVOCS/RVOMath.cs new file mode 100644 index 0000000..45cf69e --- /dev/null +++ b/FishROV/Assets/ThirdParty/RVO2-CS/RVOCS/RVOMath.cs @@ -0,0 +1,155 @@ +/* + * RVOMath.cs + * RVO2 Library C# + * + * SPDX-FileCopyrightText: 2008 University of North Carolina at Chapel Hill + * SPDX-License-Identifier: Apache-2.0 + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + * Please send all bug reports to . + * + * The authors may be contacted via: + * + * Jur van den Berg, Stephen J. Guy, Jamie Snape, Ming C. Lin, Dinesh Manocha + * Dept. of Computer Science + * 201 S. Columbia St. + * Frederick P. Brooks, Jr. Computer Science Bldg. + * Chapel Hill, N.C. 27599-3175 + * United States of America + * + * + */ + +using System; + +namespace RVO +{ + /// Contains functions and constants used in multiple classes. + /// + public struct RVOMath + { + /// A sufficiently small positive number. + internal const float RVO_EPSILON = 0.00001f; + + /// Computes the length of a specified two-dimensional vector. + /// + /// + /// The two-dimensional vector whose length is to be + /// computed. + /// The length of the two-dimensional vector. + public static float Abs(Vector2 vector) + { + return MathF.Sqrt(AbsSq(vector)); + } + + /// Computes the squared length of a specified two-dimensional + /// vector. + /// + /// The squared length of the two-dimensional vector. + /// + /// The two-dimensional vector whose squared length + /// is to be computed. + public static float AbsSq(Vector2 vector) + { + return vector * vector; + } + + /// Computes the normalization of the specified two-dimensional + /// vector. + /// + /// The normalization of the two-dimensional vector. + /// + /// The two-dimensional vector whose normalization + /// is to be computed. + public static Vector2 Normalize(Vector2 vector) + { + float length = Abs(vector); + + if (length <= RVO_EPSILON) + { + return new Vector2(0.0f, 0.0f); + } + + return vector / length; + } + + /// Computes the determinant of a two-dimensional square matrix + /// with rows consisting of the specified two-dimensional vectors. + /// + /// + /// The determinant of the two-dimensional square matrix. + /// + /// + /// The top row of the two-dimensional square + /// matrix. + /// The bottom row of the two-dimensional square + /// matrix. + internal static float Det(Vector2 vector1, Vector2 vector2) + { + return vector1._x * vector2._y - vector1._y * vector2._x; + } + + /// Computes the squared distance from a line segment with the + /// specified endpoints to a specified point. + /// + /// The squared distance from the line segment to the point. + /// + /// + /// The first endpoint of the line segment. + /// The second endpoint of the line segment. + /// + /// The point to which the squared distance is to + /// be calculated. + internal static float DistSqPointLineSegment(Vector2 vector1, Vector2 vector2, Vector2 vector3) + { + float lengthSq = AbsSq(vector2 - vector1); + + if (lengthSq <= RVO_EPSILON * RVO_EPSILON) + { + /* Degenerate segment: both endpoints are the same point. */ + return AbsSq(vector3 - vector1); + } + + float r = ((vector3 - vector1) * (vector2 - vector1)) / lengthSq; + + if (r < 0.0f) + { + return AbsSq(vector3 - vector1); + } + + if (r > 1.0f) + { + return AbsSq(vector3 - vector2); + } + + return AbsSq(vector3 - (vector1 + r * (vector2 - vector1))); + } + + /// Computes the signed distance from a line connecting the + /// specified points to a specified point. + /// + /// Positive when the point c lies to the left of the line ab. + /// + /// + /// The first point on the line. + /// The second point on the line. + /// The point to which the signed distance is to be + /// calculated. + internal static float LeftOf(Vector2 a, Vector2 b, Vector2 c) + { + return Det(a - c, b - a); + } + + } +} diff --git a/FishROV/Assets/ThirdParty/RVO2-CS/RVOCS/RVOMath.cs.meta b/FishROV/Assets/ThirdParty/RVO2-CS/RVOCS/RVOMath.cs.meta new file mode 100644 index 0000000..5f62c64 --- /dev/null +++ b/FishROV/Assets/ThirdParty/RVO2-CS/RVOCS/RVOMath.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 30121b9365c39de4aad24770212c01c1 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/FishROV/Assets/ThirdParty/RVO2-CS/RVOCS/Simulator.cs b/FishROV/Assets/ThirdParty/RVO2-CS/RVOCS/Simulator.cs new file mode 100644 index 0000000..197e908 --- /dev/null +++ b/FishROV/Assets/ThirdParty/RVO2-CS/RVOCS/Simulator.cs @@ -0,0 +1,1368 @@ +/* + * Simulator.cs + * RVO2 Library C# + * + * SPDX-FileCopyrightText: 2008 University of North Carolina at Chapel Hill + * SPDX-License-Identifier: Apache-2.0 + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + * Please send all bug reports to . + * + * The authors may be contacted via: + * + * Jur van den Berg, Stephen J. Guy, Jamie Snape, Ming C. Lin, Dinesh Manocha + * Dept. of Computer Science + * 201 S. Columbia St. + * Frederick P. Brooks, Jr. Computer Science Bldg. + * Chapel Hill, N.C. 27599-3175 + * United States of America + * + * + */ + +using System; +using System.Collections.Generic; +using System.Threading.Tasks; + +namespace RVO +{ + /// Defines the simulation. + public class Simulator + { + internal IList _agents; + internal IList _obstacles; + internal KdTree _kdTree; + internal float _timeStep; + + private static readonly Simulator _instance = new(); + + private Agent _defaultAgent; + private int _numWorkers; + private float _globalTime; + private bool _obstaclesProcessed; + + public static Simulator Instance + { + get + { + return _instance; + } + } + + /// Adds a new agent with default properties to the simulation. + /// + /// + /// The number of the agent, or -1 when the agent defaults have + /// not been set. + /// + /// The two-dimensional starting position of this + /// agent. + public int AddAgent(Vector2 position) + { + if (_defaultAgent is null) + { + return -1; + } + + Agent agent = new(); + agent._id = _agents.Count; + agent._maxNeighbors = _defaultAgent._maxNeighbors; + agent._maxSpeed = _defaultAgent._maxSpeed; + agent._neighborDist = _defaultAgent._neighborDist; + agent._position = position; + agent._radius = _defaultAgent._radius; + agent._timeHorizon = _defaultAgent._timeHorizon; + agent._timeHorizonObst = _defaultAgent._timeHorizonObst; + agent._velocity = _defaultAgent._velocity; + _agents.Add(agent); + + return agent._id; + } + + /// Adds a new agent with default properties to the simulation. + /// + /// + /// The number of the agent, or -1 when the agent defaults have + /// not been set. + /// + /// The two-dimensional starting position of this + /// agent. + [Obsolete("Use AddAgent instead.", false)] + public int addAgent(Vector2 position) + { + return AddAgent(position); + } + + /// Adds a new agent to the simulation. + /// + /// The number of the agent. + /// + /// The two-dimensional starting position of this + /// agent. + /// The maximum distance (center point to + /// center point) to other agents this agent takes into account in the + /// navigation. The larger this number, the longer the running time of + /// the simulation. If the number is too low, the simulation will not be + /// safe. Must be non-negative. + /// The maximum number of other agents this + /// agent takes into account in the navigation. The larger this number, + /// the longer the running time of the simulation. If the number is too + /// low, the simulation will not be safe. + /// The minimal amount of time for which this + /// agent's velocities that are computed by the simulation are safe with + /// respect to other agents. The larger this number, the sooner this + /// agent will respond to the presence of other agents, but the less + /// freedom this agent has in choosing its velocities. Must be positive. + /// + /// The minimal amount of time for which + /// this agent's velocities that are computed by the simulation are safe + /// with respect to obstacles. The larger this number, the sooner this + /// agent will respond to the presence of obstacles, but the less freedom + /// this agent has in choosing its velocities. Must be positive. + /// The radius of this agent. Must be non-negative. + /// + /// The maximum speed of this agent. Must be + /// non-negative. + /// The initial two-dimensional linear velocity of + /// this agent. + public int AddAgent(Vector2 position, float neighborDist, int maxNeighbors, float timeHorizon, float timeHorizonObst, float radius, float maxSpeed, Vector2 velocity) + { + UnityArgumentOutOfRange.ThrowIfNegative(maxNeighbors, nameof(maxNeighbors)); + UnityArgumentOutOfRange.ThrowIfNegative(neighborDist, nameof(neighborDist)); + UnityArgumentOutOfRange.ThrowIfNegativeOrZero(timeHorizon, nameof(timeHorizon)); + UnityArgumentOutOfRange.ThrowIfNegativeOrZero(timeHorizonObst, nameof(timeHorizonObst)); + UnityArgumentOutOfRange.ThrowIfNegative(radius, nameof(radius)); + UnityArgumentOutOfRange.ThrowIfNegative(maxSpeed, nameof(maxSpeed)); + + Agent agent = new(); + agent._id = _agents.Count; + agent._maxNeighbors = maxNeighbors; + agent._maxSpeed = maxSpeed; + agent._neighborDist = neighborDist; + agent._position = position; + agent._radius = radius; + agent._timeHorizon = timeHorizon; + agent._timeHorizonObst = timeHorizonObst; + agent._velocity = velocity; + _agents.Add(agent); + + return agent._id; + } + + /// Adds a new agent to the simulation. + /// + /// The number of the agent. + /// + /// The two-dimensional starting position of this + /// agent. + /// The maximum distance (center point to + /// center point) to other agents this agent takes into account in the + /// navigation. The larger this number, the longer the running time of + /// the simulation. If the number is too low, the simulation will not be + /// safe. Must be non-negative. + /// The maximum number of other agents this + /// agent takes into account in the navigation. The larger this number, + /// the longer the running time of the simulation. If the number is too + /// low, the simulation will not be safe. + /// The minimal amount of time for which this + /// agent's velocities that are computed by the simulation are safe with + /// respect to other agents. The larger this number, the sooner this + /// agent will respond to the presence of other agents, but the less + /// freedom this agent has in choosing its velocities. Must be positive. + /// + /// The minimal amount of time for which + /// this agent's velocities that are computed by the simulation are safe + /// with respect to obstacles. The larger this number, the sooner this + /// agent will respond to the presence of obstacles, but the less freedom + /// this agent has in choosing its velocities. Must be positive. + /// The radius of this agent. Must be non-negative. + /// + /// The maximum speed of this agent. Must be + /// non-negative. + /// The initial two-dimensional linear velocity of + /// this agent. + [Obsolete("Use AddAgent instead.", false)] + public int addAgent(Vector2 position, float neighborDist, int maxNeighbors, float timeHorizon, float timeHorizonObst, float radius, float maxSpeed, Vector2 velocity) + { + return AddAgent(position, neighborDist, maxNeighbors, timeHorizon, timeHorizonObst, radius, maxSpeed, velocity); + } + + /// Adds a new obstacle to the simulation. + /// + /// The number of the first vertex of the obstacle, or -1 when + /// the number of vertices is less than two. + /// + /// List of the vertices of the polygonal obstacle + /// in counterclockwise order. + /// + /// To add a "negative" obstacle, e.g. a bounding polygon around + /// the environment, the vertices should be listed in clockwise order. + /// + public int AddObstacle(IList vertices) + { + if (vertices.Count < 2) + { + return -1; + } + + for (int i = 0; i < vertices.Count; ++i) + { + int next = i == vertices.Count - 1 ? 0 : i + 1; + + if (RVOMath.AbsSq(vertices[next] - vertices[i]) <= RVOMath.RVO_EPSILON * RVOMath.RVO_EPSILON) + { + return -1; + } + } + + int obstacleNo = _obstacles.Count; + + for (int i = 0; i < vertices.Count; ++i) + { + Obstacle obstacle = new(); + obstacle._point = vertices[i]; + + if (i != 0) + { + obstacle._previous = _obstacles[_obstacles.Count - 1]; + obstacle._previous._next = obstacle; + } + + if (i == vertices.Count - 1) + { + obstacle._next = _obstacles[obstacleNo]; + obstacle._next._previous = obstacle; + } + + obstacle._direction = RVOMath.Normalize(vertices[(i == vertices.Count - 1 ? 0 : i + 1)] - vertices[i]); + + if (vertices.Count == 2) + { + obstacle._convex = true; + } + else + { + obstacle._convex = (RVOMath.LeftOf(vertices[(i == 0 ? vertices.Count - 1 : i - 1)], vertices[i], vertices[(i == vertices.Count - 1 ? 0 : i + 1)]) >= 0.0f); + } + + obstacle._id = _obstacles.Count; + _obstacles.Add(obstacle); + } + + return obstacleNo; + } + + /// Adds a new obstacle to the simulation. + /// + /// The number of the first vertex of the obstacle, or -1 when + /// the number of vertices is less than two. + /// + /// List of the vertices of the polygonal obstacle + /// in counterclockwise order. + /// + /// To add a "negative" obstacle, e.g. a bounding polygon around + /// the environment, the vertices should be listed in clockwise order. + /// + [Obsolete("Use AddObstacle instead.", false)] + public int addObstacle(IList vertices) + { + return AddObstacle(vertices); + } + + /// Clears the simulation. + public void Clear() + { + _agents = new List(); + _defaultAgent = null; + _kdTree = new KdTree(); + _obstacles = new List(); + _globalTime = 0.0f; + _obstaclesProcessed = false; + _timeStep = 0.1f; + + NumWorkers = 0; + } + + /// Performs a simulation step and updates the two-dimensional + /// position and two-dimensional velocity of each agent. + /// + /// The global time after the simulation step. + public float DoStep() + { + ParallelOptions options = new() { MaxDegreeOfParallelism = _numWorkers }; + + _kdTree.BuildAgentTree(); + + Parallel.For(0, _agents.Count, options, i => + { + _agents[i].ComputeNeighbors(); + _agents[i].ComputeNewVelocity(); + }); + + Parallel.For(0, _agents.Count, options, i => _agents[i].Update()); + + _globalTime += _timeStep; + + return _globalTime; + } + + /// Performs a simulation step and updates the two-dimensional + /// position and two-dimensional velocity of each agent. + /// + /// The global time after the simulation step. + [Obsolete("Use DoStep instead.", false)] + public float doStep() + { + return DoStep(); + } + + /// Returns the specified agent neighbor of the specified agent. + /// + /// + /// The number of the neighboring agent. + /// + /// The number of the agent whose agent neighbor is + /// to be retrieved. + /// The number of the agent neighbor to be + /// retrieved. + public int GetAgentAgentNeighbor(int agentNo, int neighborNo) + { + UnityArgumentOutOfRange.ThrowIfNegative(agentNo, nameof(agentNo)); + UnityArgumentOutOfRange.ThrowIfGreaterThanOrEqual(agentNo, _agents.Count, nameof(agentNo)); + UnityArgumentOutOfRange.ThrowIfNegative(neighborNo, nameof(neighborNo)); + UnityArgumentOutOfRange.ThrowIfGreaterThanOrEqual(neighborNo, _agents[agentNo]._agentNeighbors.Count, nameof(neighborNo)); + + return _agents[agentNo]._agentNeighbors[neighborNo].Value._id; + } + + /// Returns the specified agent neighbor of the specified agent. + /// + /// + /// The number of the neighboring agent. + /// + /// The number of the agent whose agent neighbor is + /// to be retrieved. + /// The number of the agent neighbor to be + /// retrieved. + [Obsolete("Use GetAgentAgentNeighbor instead.", false)] + public int getAgentAgentNeighbor(int agentNo, int neighborNo) + { + return GetAgentAgentNeighbor(agentNo, neighborNo); + } + + /// Returns the maximum neighbor count of a specified agent. + /// + /// + /// The present maximum neighbor count of the agent. + /// + /// The number of the agent whose maximum neighbor + /// count is to be retrieved. + public int GetAgentMaxNeighbors(int agentNo) + { + UnityArgumentOutOfRange.ThrowIfNegative(agentNo, nameof(agentNo)); + UnityArgumentOutOfRange.ThrowIfGreaterThanOrEqual(agentNo, _agents.Count, nameof(agentNo)); + + return _agents[agentNo]._maxNeighbors; + } + + /// Returns the maximum neighbor count of a specified agent. + /// + /// + /// The present maximum neighbor count of the agent. + /// + /// The number of the agent whose maximum neighbor + /// count is to be retrieved. + [Obsolete("Use GetAgentMaxNeighbors instead.", false)] + public int getAgentMaxNeighbors(int agentNo) + { + return GetAgentMaxNeighbors(agentNo); + } + + /// Returns the maximum speed of a specified agent. + /// + /// The present maximum speed of the agent. + /// + /// The number of the agent whose maximum speed is + /// to be retrieved. + public float GetAgentMaxSpeed(int agentNo) + { + UnityArgumentOutOfRange.ThrowIfNegative(agentNo, nameof(agentNo)); + UnityArgumentOutOfRange.ThrowIfGreaterThanOrEqual(agentNo, _agents.Count, nameof(agentNo)); + + return _agents[agentNo]._maxSpeed; + } + + /// Returns the maximum speed of a specified agent. + /// + /// The present maximum speed of the agent. + /// + /// The number of the agent whose maximum speed is + /// to be retrieved. + [Obsolete("Use GetAgentMaxSpeed instead.", false)] + public float getAgentMaxSpeed(int agentNo) + { + return GetAgentMaxSpeed(agentNo); + } + + /// Returns the maximum neighbor distance of a specified agent. + /// + /// + /// The present maximum neighbor distance of the agent. + /// + /// + /// The number of the agent whose maximum neighbor + /// distance is to be retrieved. + public float GetAgentNeighborDist(int agentNo) + { + UnityArgumentOutOfRange.ThrowIfNegative(agentNo, nameof(agentNo)); + UnityArgumentOutOfRange.ThrowIfGreaterThanOrEqual(agentNo, _agents.Count, nameof(agentNo)); + + return _agents[agentNo]._neighborDist; + } + + /// Returns the maximum neighbor distance of a specified agent. + /// + /// + /// The present maximum neighbor distance of the agent. + /// + /// + /// The number of the agent whose maximum neighbor + /// distance is to be retrieved. + [Obsolete("Use GetAgentNeighborDist instead.", false)] + public float getAgentNeighborDist(int agentNo) + { + return GetAgentNeighborDist(agentNo); + } + + /// Returns the count of agent neighbors taken into account to + /// compute the current velocity for the specified agent. + /// + /// The count of agent neighbors taken into account to compute + /// the current velocity for the specified agent. + /// + /// The number of the agent whose count of agent + /// neighbors is to be retrieved. + public int GetAgentNumAgentNeighbors(int agentNo) + { + UnityArgumentOutOfRange.ThrowIfNegative(agentNo, nameof(agentNo)); + UnityArgumentOutOfRange.ThrowIfGreaterThanOrEqual(agentNo, _agents.Count, nameof(agentNo)); + + return _agents[agentNo]._agentNeighbors.Count; + } + + /// Returns the count of agent neighbors taken into account to + /// compute the current velocity for the specified agent. + /// + /// The count of agent neighbors taken into account to compute + /// the current velocity for the specified agent. + /// + /// The number of the agent whose count of agent + /// neighbors is to be retrieved. + [Obsolete("Use GetAgentNumAgentNeighbors instead.", false)] + public int getAgentNumAgentNeighbors(int agentNo) + { + return GetAgentNumAgentNeighbors(agentNo); + } + + /// Returns the count of obstacle neighbors taken into account + /// to compute the current velocity for the specified agent. + /// + /// The count of obstacle neighbors taken into account to + /// compute the current velocity for the specified agent. + /// + /// The number of the agent whose count of obstacle + /// neighbors is to be retrieved. + public int GetAgentNumObstacleNeighbors(int agentNo) + { + UnityArgumentOutOfRange.ThrowIfNegative(agentNo, nameof(agentNo)); + UnityArgumentOutOfRange.ThrowIfGreaterThanOrEqual(agentNo, _agents.Count, nameof(agentNo)); + + return _agents[agentNo]._obstacleNeighbors.Count; + } + + /// Returns the count of obstacle neighbors taken into account + /// to compute the current velocity for the specified agent. + /// + /// The count of obstacle neighbors taken into account to + /// compute the current velocity for the specified agent. + /// + /// The number of the agent whose count of obstacle + /// neighbors is to be retrieved. + [Obsolete("Use GetAgentNumObstacleNeighbors instead.", false)] + public int getAgentNumObstacleNeighbors(int agentNo) + { + return GetAgentNumObstacleNeighbors(agentNo); + } + + /// Returns the specified obstacle neighbor of the specified + /// agent. + /// + /// The number of the first vertex of the neighboring obstacle + /// edge. + /// + /// The number of the agent whose obstacle neighbor + /// is to be retrieved. + /// The number of the obstacle neighbor to be + /// retrieved. + public int GetAgentObstacleNeighbor(int agentNo, int neighborNo) + { + UnityArgumentOutOfRange.ThrowIfNegative(agentNo, nameof(agentNo)); + UnityArgumentOutOfRange.ThrowIfGreaterThanOrEqual(agentNo, _agents.Count, nameof(agentNo)); + UnityArgumentOutOfRange.ThrowIfNegative(neighborNo, nameof(neighborNo)); + UnityArgumentOutOfRange.ThrowIfGreaterThanOrEqual(neighborNo, _agents[agentNo]._obstacleNeighbors.Count, nameof(neighborNo)); + + return _agents[agentNo]._obstacleNeighbors[neighborNo].Value._id; + } + + /// Returns the specified obstacle neighbor of the specified + /// agent. + /// + /// The number of the first vertex of the neighboring obstacle + /// edge. + /// + /// The number of the agent whose obstacle neighbor + /// is to be retrieved. + /// The number of the obstacle neighbor to be + /// retrieved. + [Obsolete("Use GetAgentObstacleNeighbor instead.", false)] + public int getAgentObstacleNeighbor(int agentNo, int neighborNo) + { + return GetAgentObstacleNeighbor(agentNo, neighborNo); + } + + /// Returns the ORCA constraints of the specified agent. + /// + /// + /// A list of lines representing the ORCA constraints. + /// + /// The number of the agent whose ORCA constraints + /// are to be retrieved. + /// + /// The halfplane to the left of each line is the region of + /// permissible velocities with respect to that ORCA constraint. + /// + public IList GetAgentOrcaLines(int agentNo) + { + UnityArgumentOutOfRange.ThrowIfNegative(agentNo, nameof(agentNo)); + UnityArgumentOutOfRange.ThrowIfGreaterThanOrEqual(agentNo, _agents.Count, nameof(agentNo)); + + return new List(_agents[agentNo]._orcaLines); + } + + /// Returns the ORCA constraints of the specified agent. + /// + /// + /// A list of lines representing the ORCA constraints. + /// + /// The number of the agent whose ORCA constraints + /// are to be retrieved. + /// + /// The halfplane to the left of each line is the region of + /// permissible velocities with respect to that ORCA constraint. + /// + [Obsolete("Use GetAgentOrcaLines instead.", false)] + public IList getAgentOrcaLines(int agentNo) + { + return GetAgentOrcaLines(agentNo); + } + + /// Returns the two-dimensional position of a specified agent. + /// + /// + /// The present two-dimensional position of the (center of the) + /// agent. + /// + /// The number of the agent whose two-dimensional + /// position is to be retrieved. + public Vector2 GetAgentPosition(int agentNo) + { + UnityArgumentOutOfRange.ThrowIfNegative(agentNo, nameof(agentNo)); + UnityArgumentOutOfRange.ThrowIfGreaterThanOrEqual(agentNo, _agents.Count, nameof(agentNo)); + + return _agents[agentNo]._position; + } + + /// Returns the two-dimensional position of a specified agent. + /// + /// + /// The present two-dimensional position of the (center of the) + /// agent. + /// + /// The number of the agent whose two-dimensional + /// position is to be retrieved. + [Obsolete("Use GetAgentPosition instead.", false)] + public Vector2 getAgentPosition(int agentNo) + { + return GetAgentPosition(agentNo); + } + + /// Returns the two-dimensional preferred velocity of a + /// specified agent. + /// + /// The present two-dimensional preferred velocity of the agent. + /// + /// + /// The number of the agent whose two-dimensional + /// preferred velocity is to be retrieved. + public Vector2 GetAgentPrefVelocity(int agentNo) + { + UnityArgumentOutOfRange.ThrowIfNegative(agentNo, nameof(agentNo)); + UnityArgumentOutOfRange.ThrowIfGreaterThanOrEqual(agentNo, _agents.Count, nameof(agentNo)); + + return _agents[agentNo]._prefVelocity; + } + + /// Returns the two-dimensional preferred velocity of a + /// specified agent. + /// + /// The present two-dimensional preferred velocity of the agent. + /// + /// + /// The number of the agent whose two-dimensional + /// preferred velocity is to be retrieved. + [Obsolete("Use GetAgentPrefVelocity instead.", false)] + public Vector2 getAgentPrefVelocity(int agentNo) + { + return GetAgentPrefVelocity(agentNo); + } + + /// Returns the radius of a specified agent. + /// + /// The present radius of the agent. + /// + /// The number of the agent whose radius is to be + /// retrieved. + public float GetAgentRadius(int agentNo) + { + UnityArgumentOutOfRange.ThrowIfNegative(agentNo, nameof(agentNo)); + UnityArgumentOutOfRange.ThrowIfGreaterThanOrEqual(agentNo, _agents.Count, nameof(agentNo)); + + return _agents[agentNo]._radius; + } + + /// Returns the radius of a specified agent. + /// + /// The present radius of the agent. + /// + /// The number of the agent whose radius is to be + /// retrieved. + [Obsolete("Use GetAgentRadius instead.", false)] + public float getAgentRadius(int agentNo) + { + return GetAgentRadius(agentNo); + } + + /// Returns the time horizon of a specified agent. + /// + /// The present time horizon of the agent. + /// + /// The number of the agent whose time horizon is + /// to be retrieved. + public float GetAgentTimeHorizon(int agentNo) + { + UnityArgumentOutOfRange.ThrowIfNegative(agentNo, nameof(agentNo)); + UnityArgumentOutOfRange.ThrowIfGreaterThanOrEqual(agentNo, _agents.Count, nameof(agentNo)); + + return _agents[agentNo]._timeHorizon; + } + + /// Returns the time horizon of a specified agent. + /// + /// The present time horizon of the agent. + /// + /// The number of the agent whose time horizon is + /// to be retrieved. + [Obsolete("Use GetAgentTimeHorizon instead.", false)] + public float getAgentTimeHorizon(int agentNo) + { + return GetAgentTimeHorizon(agentNo); + } + + /// Returns the time horizon with respect to obstacles of a + /// specified agent. + /// + /// The present time horizon with respect to obstacles of the + /// agent. + /// + /// The number of the agent whose time horizon with + /// respect to obstacles is to be retrieved. + public float GetAgentTimeHorizonObst(int agentNo) + { + UnityArgumentOutOfRange.ThrowIfNegative(agentNo, nameof(agentNo)); + UnityArgumentOutOfRange.ThrowIfGreaterThanOrEqual(agentNo, _agents.Count, nameof(agentNo)); + + return _agents[agentNo]._timeHorizonObst; + } + + /// Returns the time horizon with respect to obstacles of a + /// specified agent. + /// + /// The present time horizon with respect to obstacles of the + /// agent. + /// + /// The number of the agent whose time horizon with + /// respect to obstacles is to be retrieved. + [Obsolete("Use GetAgentTimeHorizonObst instead.", false)] + public float getAgentTimeHorizonObst(int agentNo) + { + return GetAgentTimeHorizonObst(agentNo); + } + + /// Returns the two-dimensional linear velocity of a specified + /// agent. + /// + /// The present two-dimensional linear velocity of the agent. + /// + /// + /// The number of the agent whose two-dimensional + /// linear velocity is to be retrieved. + public Vector2 GetAgentVelocity(int agentNo) + { + UnityArgumentOutOfRange.ThrowIfNegative(agentNo, nameof(agentNo)); + UnityArgumentOutOfRange.ThrowIfGreaterThanOrEqual(agentNo, _agents.Count, nameof(agentNo)); + + return _agents[agentNo]._velocity; + } + + /// Returns the two-dimensional linear velocity of a specified + /// agent. + /// + /// The present two-dimensional linear velocity of the agent. + /// + /// + /// The number of the agent whose two-dimensional + /// linear velocity is to be retrieved. + [Obsolete("Use GetAgentVelocity instead.", false)] + public Vector2 getAgentVelocity(int agentNo) + { + return GetAgentVelocity(agentNo); + } + + /// Gets the global time of the simulation. + /// + /// The present global time of the simulation (zero initially). + /// + public float GlobalTime => _globalTime; + + /// Returns the global time of the simulation. + /// + /// The present global time of the simulation (zero initially). + /// + [Obsolete("Use GlobalTime instead.", false)] + public float getGlobalTime() + { + return GlobalTime; + } + + /// Gets the count of agents in the simulation. + /// + /// The count of agents in the simulation. + public int NumAgents => _agents.Count; + + /// Returns the count of agents in the simulation. + /// + /// The count of agents in the simulation. + [Obsolete("Use NumAgents instead.", false)] + public int getNumAgents() + { + return NumAgents; + } + + /// Gets the count of obstacle vertices in the simulation. + /// + /// + /// The count of obstacle vertices in the simulation. + public int NumObstacleVertices => _obstacles.Count; + + /// Returns the count of obstacle vertices in the simulation. + /// + /// + /// The count of obstacle vertices in the simulation. + [Obsolete("Use NumObstacleVertices instead.", false)] + public int getNumObstacleVertices() + { + return NumObstacleVertices; + } + + /// Gets or sets the maximum degree of parallelism for the + /// simulation step. + /// + /// The maximum number of concurrent tasks used during DoStep. A + /// value of zero or less imposes no limit. + public int NumWorkers + { + get => _numWorkers; + set => _numWorkers = value <= 0 ? -1 : value; + } + + /// Returns the two-dimensional position of a specified obstacle + /// vertex. + /// + /// The two-dimensional position of the specified obstacle + /// vertex. + /// + /// The number of the obstacle vertex to be + /// retrieved. + public Vector2 GetObstacleVertex(int vertexNo) + { + UnityArgumentOutOfRange.ThrowIfNegative(vertexNo, nameof(vertexNo)); + UnityArgumentOutOfRange.ThrowIfGreaterThanOrEqual(vertexNo, _obstacles.Count, nameof(vertexNo)); + + return _obstacles[vertexNo]._point; + } + + /// Returns the two-dimensional position of a specified obstacle + /// vertex. + /// + /// The two-dimensional position of the specified obstacle + /// vertex. + /// + /// The number of the obstacle vertex to be + /// retrieved. + [Obsolete("Use GetObstacleVertex instead.", false)] + public Vector2 getObstacleVertex(int vertexNo) + { + return GetObstacleVertex(vertexNo); + } + + /// Returns the number of the obstacle vertex succeeding the + /// specified obstacle vertex in its polygon. + /// + /// The number of the obstacle vertex succeeding the specified + /// obstacle vertex in its polygon. + /// + /// The number of the obstacle vertex whose + /// successor is to be retrieved. + public int GetNextObstacleVertexNo(int vertexNo) + { + UnityArgumentOutOfRange.ThrowIfNegative(vertexNo, nameof(vertexNo)); + UnityArgumentOutOfRange.ThrowIfGreaterThanOrEqual(vertexNo, _obstacles.Count, nameof(vertexNo)); + + return _obstacles[vertexNo]._next._id; + } + + /// Returns the number of the obstacle vertex succeeding the + /// specified obstacle vertex in its polygon. + /// + /// The number of the obstacle vertex succeeding the specified + /// obstacle vertex in its polygon. + /// + /// The number of the obstacle vertex whose + /// successor is to be retrieved. + [Obsolete("Use GetNextObstacleVertexNo instead.", false)] + public int getNextObstacleVertexNo(int vertexNo) + { + return GetNextObstacleVertexNo(vertexNo); + } + + /// Returns the number of the obstacle vertex preceding the + /// specified obstacle vertex in its polygon. + /// + /// The number of the obstacle vertex preceding the specified + /// obstacle vertex in its polygon. + /// + /// The number of the obstacle vertex whose + /// predecessor is to be retrieved. + public int GetPrevObstacleVertexNo(int vertexNo) + { + UnityArgumentOutOfRange.ThrowIfNegative(vertexNo, nameof(vertexNo)); + UnityArgumentOutOfRange.ThrowIfGreaterThanOrEqual(vertexNo, _obstacles.Count, nameof(vertexNo)); + + return _obstacles[vertexNo]._previous._id; + } + + /// Returns the number of the obstacle vertex preceding the + /// specified obstacle vertex in its polygon. + /// + /// The number of the obstacle vertex preceding the specified + /// obstacle vertex in its polygon. + /// + /// The number of the obstacle vertex whose + /// predecessor is to be retrieved. + [Obsolete("Use GetPrevObstacleVertexNo instead.", false)] + public int getPrevObstacleVertexNo(int vertexNo) + { + return GetPrevObstacleVertexNo(vertexNo); + } + + /// Gets or sets the time step of the simulation. + /// + /// The time step of the simulation. Must be positive. + public float TimeStep + { + get => _timeStep; + set + { + UnityArgumentOutOfRange.ThrowIfNegativeOrZero(value, nameof(value)); + + _timeStep = value; + } + } + + /// Returns the time step of the simulation. + /// + /// The present time step of the simulation. + [Obsolete("Use TimeStep instead.", false)] + public float getTimeStep() + { + return TimeStep; + } + + /// Processes the obstacles that have been added so that they + /// are accounted for in the simulation. + /// + /// Obstacles added to the simulation after this function has + /// been called are not accounted for in the simulation. + public void ProcessObstacles() + { + if (_obstaclesProcessed) + { + throw new InvalidOperationException("ProcessObstacles has already been called. Call Clear() to reset the simulation before processing obstacles again."); + } + + _kdTree.BuildObstacleTree(); + _obstaclesProcessed = true; + } + + /// Processes the obstacles that have been added so that they + /// are accounted for in the simulation. + /// + /// Obstacles added to the simulation after this function has + /// been called are not accounted for in the simulation. + [Obsolete("Use ProcessObstacles instead.", false)] + public void processObstacles() + { + ProcessObstacles(); + } + + /// Performs a visibility query between the two specified points + /// with respect to the obstacles. + /// + /// A boolean specifying whether the two points are mutually + /// visible. Returns true when the obstacles have not been processed. + /// + /// + /// The first point of the query. + /// The second point of the query. + /// The minimal distance between the line connecting + /// the two points and the obstacles in order for the points to be + /// mutually visible (optional). Must be non-negative. + public bool QueryVisibility(Vector2 point1, Vector2 point2, float radius) + { + UnityArgumentOutOfRange.ThrowIfNegative(radius, nameof(radius)); + + if (RVOMath.AbsSq(point2 - point1) <= RVOMath.RVO_EPSILON * RVOMath.RVO_EPSILON) + { + return true; + } + + return _kdTree.QueryVisibility(point1, point2, radius); + } + + /// Performs a visibility query between the two specified points + /// with respect to the obstacles. + /// + /// A boolean specifying whether the two points are mutually + /// visible. Returns true when the obstacles have not been processed. + /// + /// + /// The first point of the query. + /// The second point of the query. + /// The minimal distance between the line connecting + /// the two points and the obstacles in order for the points to be + /// mutually visible (optional). Must be non-negative. + [Obsolete("Use QueryVisibility instead.", false)] + public bool queryVisibility(Vector2 point1, Vector2 point2, float radius) + { + return QueryVisibility(point1, point2, radius); + } + + /// Sets the default properties for any new agent that is added. + /// + /// + /// The default maximum distance (center point + /// to center point) to other agents a new agent takes into account in + /// the navigation. The larger this number, the longer the running time of + /// the simulation. If the number is too low, the simulation will not be + /// safe. Must be non-negative. + /// The default maximum number of other agents + /// a new agent takes into account in the navigation. The larger this + /// number, the longer the running time of the simulation. If the number + /// is too low, the simulation will not be safe. + /// The default minimal amount of time for + /// which a new agent's velocities that are computed by the simulation + /// are safe with respect to other agents. The larger this number, the + /// sooner an agent will respond to the presence of other agents, but the + /// less freedom the agent has in choosing its velocities. Must be + /// positive. + /// The default minimal amount of time for + /// which a new agent's velocities that are computed by the simulation + /// are safe with respect to obstacles. The larger this number, the + /// sooner an agent will respond to the presence of obstacles, but the + /// less freedom the agent has in choosing its velocities. Must be + /// positive. + /// The default radius of a new agent. Must be + /// non-negative. + /// The default maximum speed of a new agent. Must + /// be non-negative. + /// The default initial two-dimensional linear + /// velocity of a new agent. + public void SetAgentDefaults(float neighborDist, int maxNeighbors, float timeHorizon, float timeHorizonObst, float radius, float maxSpeed, Vector2 velocity) + { + UnityArgumentOutOfRange.ThrowIfNegative(maxNeighbors, nameof(maxNeighbors)); + UnityArgumentOutOfRange.ThrowIfNegative(neighborDist, nameof(neighborDist)); + UnityArgumentOutOfRange.ThrowIfNegativeOrZero(timeHorizon, nameof(timeHorizon)); + UnityArgumentOutOfRange.ThrowIfNegativeOrZero(timeHorizonObst, nameof(timeHorizonObst)); + UnityArgumentOutOfRange.ThrowIfNegative(radius, nameof(radius)); + UnityArgumentOutOfRange.ThrowIfNegative(maxSpeed, nameof(maxSpeed)); + + if (_defaultAgent is null) + { + _defaultAgent = new Agent(); + } + + _defaultAgent._maxNeighbors = maxNeighbors; + _defaultAgent._maxSpeed = maxSpeed; + _defaultAgent._neighborDist = neighborDist; + _defaultAgent._radius = radius; + _defaultAgent._timeHorizon = timeHorizon; + _defaultAgent._timeHorizonObst = timeHorizonObst; + _defaultAgent._velocity = velocity; + } + + /// Sets the default properties for any new agent that is added. + /// + /// + /// The default maximum distance (center point + /// to center point) to other agents a new agent takes into account in + /// the navigation. The larger this number, the longer the running time of + /// the simulation. If the number is too low, the simulation will not be + /// safe. Must be non-negative. + /// The default maximum number of other agents + /// a new agent takes into account in the navigation. The larger this + /// number, the longer the running time of the simulation. If the number + /// is too low, the simulation will not be safe. + /// The default minimal amount of time for + /// which a new agent's velocities that are computed by the simulation + /// are safe with respect to other agents. The larger this number, the + /// sooner an agent will respond to the presence of other agents, but the + /// less freedom the agent has in choosing its velocities. Must be + /// positive. + /// The default minimal amount of time for + /// which a new agent's velocities that are computed by the simulation + /// are safe with respect to obstacles. The larger this number, the + /// sooner an agent will respond to the presence of obstacles, but the + /// less freedom the agent has in choosing its velocities. Must be + /// positive. + /// The default radius of a new agent. Must be + /// non-negative. + /// The default maximum speed of a new agent. Must + /// be non-negative. + /// The default initial two-dimensional linear + /// velocity of a new agent. + [Obsolete("Use SetAgentDefaults instead.", false)] + public void setAgentDefaults(float neighborDist, int maxNeighbors, float timeHorizon, float timeHorizonObst, float radius, float maxSpeed, Vector2 velocity) + { + SetAgentDefaults(neighborDist, maxNeighbors, timeHorizon, timeHorizonObst, radius, maxSpeed, velocity); + } + + /// Sets the maximum neighbor count of a specified agent. + /// + /// + /// The number of the agent whose maximum neighbor + /// count is to be modified. + /// The replacement maximum neighbor count. + /// Must be non-negative. + public void SetAgentMaxNeighbors(int agentNo, int maxNeighbors) + { + UnityArgumentOutOfRange.ThrowIfNegative(agentNo, nameof(agentNo)); + UnityArgumentOutOfRange.ThrowIfGreaterThanOrEqual(agentNo, _agents.Count, nameof(agentNo)); + UnityArgumentOutOfRange.ThrowIfNegative(maxNeighbors, nameof(maxNeighbors)); + + _agents[agentNo]._maxNeighbors = maxNeighbors; + } + + /// Sets the maximum neighbor count of a specified agent. + /// + /// + /// The number of the agent whose maximum neighbor + /// count is to be modified. + /// The replacement maximum neighbor count. + /// + [Obsolete("Use SetAgentMaxNeighbors instead.", false)] + public void setAgentMaxNeighbors(int agentNo, int maxNeighbors) + { + SetAgentMaxNeighbors(agentNo, maxNeighbors); + } + + /// Sets the maximum speed of a specified agent. + /// + /// The number of the agent whose maximum speed is + /// to be modified. + /// The replacement maximum speed. Must be + /// non-negative. + public void SetAgentMaxSpeed(int agentNo, float maxSpeed) + { + UnityArgumentOutOfRange.ThrowIfNegative(agentNo, nameof(agentNo)); + UnityArgumentOutOfRange.ThrowIfGreaterThanOrEqual(agentNo, _agents.Count, nameof(agentNo)); + UnityArgumentOutOfRange.ThrowIfNegative(maxSpeed, nameof(maxSpeed)); + + _agents[agentNo]._maxSpeed = maxSpeed; + } + + /// Sets the maximum speed of a specified agent. + /// + /// The number of the agent whose maximum speed is + /// to be modified. + /// The replacement maximum speed. Must be + /// non-negative. + [Obsolete("Use SetAgentMaxSpeed instead.", false)] + public void setAgentMaxSpeed(int agentNo, float maxSpeed) + { + SetAgentMaxSpeed(agentNo, maxSpeed); + } + + /// Sets the maximum neighbor distance of a specified agent. + /// + /// + /// The number of the agent whose maximum neighbor + /// distance is to be modified. + /// The replacement maximum neighbor distance. + /// Must be non-negative. + public void SetAgentNeighborDist(int agentNo, float neighborDist) + { + UnityArgumentOutOfRange.ThrowIfNegative(agentNo, nameof(agentNo)); + UnityArgumentOutOfRange.ThrowIfGreaterThanOrEqual(agentNo, _agents.Count, nameof(agentNo)); + UnityArgumentOutOfRange.ThrowIfNegative(neighborDist, nameof(neighborDist)); + + _agents[agentNo]._neighborDist = neighborDist; + } + + /// Sets the maximum neighbor distance of a specified agent. + /// + /// + /// The number of the agent whose maximum neighbor + /// distance is to be modified. + /// The replacement maximum neighbor distance. + /// Must be non-negative. + [Obsolete("Use SetAgentNeighborDist instead.", false)] + public void setAgentNeighborDist(int agentNo, float neighborDist) + { + SetAgentNeighborDist(agentNo, neighborDist); + } + + /// Sets the two-dimensional position of a specified agent. + /// + /// + /// The number of the agent whose two-dimensional + /// position is to be modified. + /// The replacement of the two-dimensional + /// position. + public void SetAgentPosition(int agentNo, Vector2 position) + { + UnityArgumentOutOfRange.ThrowIfNegative(agentNo, nameof(agentNo)); + UnityArgumentOutOfRange.ThrowIfGreaterThanOrEqual(agentNo, _agents.Count, nameof(agentNo)); + + _agents[agentNo]._position = position; + } + + /// Sets the two-dimensional position of a specified agent. + /// + /// + /// The number of the agent whose two-dimensional + /// position is to be modified. + /// The replacement of the two-dimensional + /// position. + [Obsolete("Use SetAgentPosition instead.", false)] + public void setAgentPosition(int agentNo, Vector2 position) + { + SetAgentPosition(agentNo, position); + } + + /// Sets the two-dimensional preferred velocity of a specified + /// agent. + /// + /// The number of the agent whose two-dimensional + /// preferred velocity is to be modified. + /// The replacement of the two-dimensional + /// preferred velocity. + public void SetAgentPrefVelocity(int agentNo, Vector2 prefVelocity) + { + UnityArgumentOutOfRange.ThrowIfNegative(agentNo, nameof(agentNo)); + UnityArgumentOutOfRange.ThrowIfGreaterThanOrEqual(agentNo, _agents.Count, nameof(agentNo)); + + _agents[agentNo]._prefVelocity = prefVelocity; + } + + /// Sets the two-dimensional preferred velocity of a specified + /// agent. + /// + /// The number of the agent whose two-dimensional + /// preferred velocity is to be modified. + /// The replacement of the two-dimensional + /// preferred velocity. + [Obsolete("Use SetAgentPrefVelocity instead.", false)] + public void setAgentPrefVelocity(int agentNo, Vector2 prefVelocity) + { + SetAgentPrefVelocity(agentNo, prefVelocity); + } + + /// Sets the radius of a specified agent. + /// + /// The number of the agent whose radius is to be + /// modified. + /// The replacement radius. Must be non-negative. + /// + public void SetAgentRadius(int agentNo, float radius) + { + UnityArgumentOutOfRange.ThrowIfNegative(agentNo, nameof(agentNo)); + UnityArgumentOutOfRange.ThrowIfGreaterThanOrEqual(agentNo, _agents.Count, nameof(agentNo)); + UnityArgumentOutOfRange.ThrowIfNegative(radius, nameof(radius)); + + _agents[agentNo]._radius = radius; + } + + /// Sets the radius of a specified agent. + /// + /// The number of the agent whose radius is to be + /// modified. + /// The replacement radius. Must be non-negative. + /// + [Obsolete("Use SetAgentRadius instead.", false)] + public void setAgentRadius(int agentNo, float radius) + { + SetAgentRadius(agentNo, radius); + } + + /// Sets the time horizon of a specified agent with respect to + /// other agents. + /// + /// The number of the agent whose time horizon is + /// to be modified. + /// The replacement time horizon with respect + /// to other agents. Must be positive. + public void SetAgentTimeHorizon(int agentNo, float timeHorizon) + { + UnityArgumentOutOfRange.ThrowIfNegative(agentNo, nameof(agentNo)); + UnityArgumentOutOfRange.ThrowIfGreaterThanOrEqual(agentNo, _agents.Count, nameof(agentNo)); + UnityArgumentOutOfRange.ThrowIfNegativeOrZero(timeHorizon, nameof(timeHorizon)); + + _agents[agentNo]._timeHorizon = timeHorizon; + } + + /// Sets the time horizon of a specified agent with respect to + /// other agents. + /// + /// The number of the agent whose time horizon is + /// to be modified. + /// The replacement time horizon with respect + /// to other agents. Must be positive. + [Obsolete("Use SetAgentTimeHorizon instead.", false)] + public void setAgentTimeHorizon(int agentNo, float timeHorizon) + { + SetAgentTimeHorizon(agentNo, timeHorizon); + } + + /// Sets the time horizon of a specified agent with respect to + /// obstacles. + /// + /// The number of the agent whose time horizon with + /// respect to obstacles is to be modified. + /// The replacement time horizon with + /// respect to obstacles. Must be positive. + public void SetAgentTimeHorizonObst(int agentNo, float timeHorizonObst) + { + UnityArgumentOutOfRange.ThrowIfNegative(agentNo, nameof(agentNo)); + UnityArgumentOutOfRange.ThrowIfGreaterThanOrEqual(agentNo, _agents.Count, nameof(agentNo)); + UnityArgumentOutOfRange.ThrowIfNegativeOrZero(timeHorizonObst, nameof(timeHorizonObst)); + + _agents[agentNo]._timeHorizonObst = timeHorizonObst; + } + + /// Sets the time horizon of a specified agent with respect to + /// obstacles. + /// + /// The number of the agent whose time horizon with + /// respect to obstacles is to be modified. + /// The replacement time horizon with + /// respect to obstacles. Must be positive. + [Obsolete("Use SetAgentTimeHorizonObst instead.", false)] + public void setAgentTimeHorizonObst(int agentNo, float timeHorizonObst) + { + SetAgentTimeHorizonObst(agentNo, timeHorizonObst); + } + + /// Sets the two-dimensional linear velocity of a specified + /// agent. + /// + /// The number of the agent whose two-dimensional + /// linear velocity is to be modified. + /// The replacement two-dimensional linear + /// velocity. + public void SetAgentVelocity(int agentNo, Vector2 velocity) + { + UnityArgumentOutOfRange.ThrowIfNegative(agentNo, nameof(agentNo)); + UnityArgumentOutOfRange.ThrowIfGreaterThanOrEqual(agentNo, _agents.Count, nameof(agentNo)); + + _agents[agentNo]._velocity = velocity; + } + + /// Sets the two-dimensional linear velocity of a specified + /// agent. + /// + /// The number of the agent whose two-dimensional + /// linear velocity is to be modified. + /// The replacement two-dimensional linear + /// velocity. + [Obsolete("Use SetAgentVelocity instead.", false)] + public void setAgentVelocity(int agentNo, Vector2 velocity) + { + SetAgentVelocity(agentNo, velocity); + } + + /// Sets the global time of the simulation. + /// + /// The global time of the simulation. + public void SetGlobalTime(float globalTime) + { + _globalTime = globalTime; + } + + /// Sets the global time of the simulation. + /// + /// The global time of the simulation. + [Obsolete("Use SetGlobalTime instead.", false)] + public void setGlobalTime(float globalTime) + { + SetGlobalTime(globalTime); + } + + /// Sets the time step of the simulation. + /// + /// The time step of the simulation. Must be + /// positive. + [Obsolete("Use TimeStep instead.", false)] + public void setTimeStep(float timeStep) + { + TimeStep = timeStep; + } + + /// Constructs and initializes a simulation. + private Simulator() + { + Clear(); + } + } +} + diff --git a/FishROV/Assets/ThirdParty/RVO2-CS/RVOCS/Simulator.cs.meta b/FishROV/Assets/ThirdParty/RVO2-CS/RVOCS/Simulator.cs.meta new file mode 100644 index 0000000..6da69ef --- /dev/null +++ b/FishROV/Assets/ThirdParty/RVO2-CS/RVOCS/Simulator.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 123507813c7674546a3b3050097363ef +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/FishROV/Assets/ThirdParty/RVO2-CS/RVOCS/UnityArgumentOutOfRange.cs b/FishROV/Assets/ThirdParty/RVO2-CS/RVOCS/UnityArgumentOutOfRange.cs new file mode 100644 index 0000000..8bf67f3 --- /dev/null +++ b/FishROV/Assets/ThirdParty/RVO2-CS/RVOCS/UnityArgumentOutOfRange.cs @@ -0,0 +1,47 @@ +/* + * UnityArgumentOutOfRange.cs + * Unity compatibility helpers for RVO2 Library C#. + * + * SPDX-FileCopyrightText: 2008 University of North Carolina at Chapel Hill + * SPDX-License-Identifier: Apache-2.0 + */ + +using System; + +namespace RVO +{ + internal static class UnityArgumentOutOfRange + { + public static void ThrowIfNegative(int value, string paramName) + { + if (value < 0) + { + throw new ArgumentOutOfRangeException(paramName); + } + } + + public static void ThrowIfNegative(float value, string paramName) + { + if (value < 0f) + { + throw new ArgumentOutOfRangeException(paramName); + } + } + + public static void ThrowIfNegativeOrZero(float value, string paramName) + { + if (value <= 0f) + { + throw new ArgumentOutOfRangeException(paramName); + } + } + + public static void ThrowIfGreaterThanOrEqual(int value, int other, string paramName) + { + if (value >= other) + { + throw new ArgumentOutOfRangeException(paramName); + } + } + } +} diff --git a/FishROV/Assets/ThirdParty/RVO2-CS/RVOCS/UnityArgumentOutOfRange.cs.meta b/FishROV/Assets/ThirdParty/RVO2-CS/RVOCS/UnityArgumentOutOfRange.cs.meta new file mode 100644 index 0000000..88feedf --- /dev/null +++ b/FishROV/Assets/ThirdParty/RVO2-CS/RVOCS/UnityArgumentOutOfRange.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 0e4ae8cc39feef744babebc44c47f507 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/FishROV/Assets/ThirdParty/RVO2-CS/RVOCS/Vector2.cs b/FishROV/Assets/ThirdParty/RVO2-CS/RVOCS/Vector2.cs new file mode 100644 index 0000000..d705e6b --- /dev/null +++ b/FishROV/Assets/ThirdParty/RVO2-CS/RVOCS/Vector2.cs @@ -0,0 +1,240 @@ +/* + * Vector2.cs + * RVO2 Library C# + * + * SPDX-FileCopyrightText: 2008 University of North Carolina at Chapel Hill + * SPDX-License-Identifier: Apache-2.0 + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + * Please send all bug reports to . + * + * The authors may be contacted via: + * + * Jur van den Berg, Stephen J. Guy, Jamie Snape, Ming C. Lin, Dinesh Manocha + * Dept. of Computer Science + * 201 S. Columbia St. + * Frederick P. Brooks, Jr. Computer Science Bldg. + * Chapel Hill, N.C. 27599-3175 + * United States of America + * + * + */ + +using System; +using System.Globalization; + +namespace RVO +{ + /// Defines a two-dimensional vector. + public readonly struct Vector2 : IEquatable + { + internal readonly float _x; + internal readonly float _y; + + /// Constructs and initializes a two-dimensional vector from the + /// specified xy-coordinates. + /// + /// The x-coordinate of the two-dimensional vector. + /// + /// The y-coordinate of the two-dimensional vector. + /// + public Vector2(float x, float y) + { + _x = x; + _y = y; + } + + /// Returns the string representation of this vector. + /// + /// The string representation of this vector. + public override string ToString() + { + return $"({_x.ToString(CultureInfo.InvariantCulture)},{_y.ToString(CultureInfo.InvariantCulture)})"; + } + + /// Returns true if this vector equals the specified vector. + /// + /// + /// True if this vector equals the specified vector. + /// + /// The vector to compare with this vector. + public bool Equals(Vector2 other) + { + return _x == other._x && _y == other._y; + } + + /// Returns true if this vector equals the specified object. + /// + /// + /// True if this vector equals the specified object. + /// + /// The object to compare with this vector. + public override bool Equals(object obj) + { + return obj is Vector2 other && Equals(other); + } + + /// Returns the hash code for this vector. + /// + /// The hash code for this vector. + public override int GetHashCode() + { + return HashCode.Combine(_x, _y); + } + + /// Returns true if the two vectors are equal. + /// + /// True if the two vectors are equal. + /// + /// The first vector. + /// The second vector. + public static bool operator ==(Vector2 left, Vector2 right) + { + return left.Equals(right); + } + + /// Returns true if the two vectors are not equal. + /// + /// True if the two vectors are not equal. + /// + /// The first vector. + /// The second vector. + public static bool operator !=(Vector2 left, Vector2 right) + { + return !left.Equals(right); + } + + /// Gets the x-coordinate of this two-dimensional vector. + /// + /// + /// The x-coordinate of the two-dimensional vector. + public float X => _x; + + /// Gets the y-coordinate of this two-dimensional vector. + /// + /// + /// The y-coordinate of the two-dimensional vector. + public float Y => _y; + + /// Returns the x-coordinate of this two-dimensional vector. + /// + /// + /// The x-coordinate of the two-dimensional vector. + [Obsolete("Use the X property instead.", false)] + public float x() + { + return _x; + } + + /// Returns the y-coordinate of this two-dimensional vector. + /// + /// + /// The y-coordinate of the two-dimensional vector. + [Obsolete("Use the Y property instead.", false)] + public float y() + { + return _y; + } + + /// Computes the dot product of the two specified + /// two-dimensional vectors. + /// + /// The dot product of the two specified two-dimensional + /// vectors. + /// + /// The first two-dimensional vector. + /// The second two-dimensional vector. + public static float operator *(Vector2 vector1, Vector2 vector2) + { + return vector1._x * vector2._x + vector1._y * vector2._y; + } + + /// Computes the scalar multiplication of the specified + /// two-dimensional vector with the specified scalar value. + /// + /// The scalar multiplication of the specified two-dimensional + /// vector with the specified scalar value. + /// + /// The scalar value. + /// The two-dimensional vector. + public static Vector2 operator *(float scalar, Vector2 vector) + { + return vector * scalar; + } + + /// Computes the scalar multiplication of the specified + /// two-dimensional vector with the specified scalar value. + /// + /// The scalar multiplication of the specified two-dimensional + /// vector with the specified scalar value. + /// + /// The two-dimensional vector. + /// The scalar value. + public static Vector2 operator *(Vector2 vector, float scalar) + { + return new Vector2(vector._x * scalar, vector._y * scalar); + } + + /// Computes the scalar division of the specified + /// two-dimensional vector with the specified scalar value. + /// + /// The scalar division of the specified two-dimensional vector + /// with the specified scalar value. + /// + /// The two-dimensional vector. + /// The scalar value. + public static Vector2 operator /(Vector2 vector, float scalar) + { + return new Vector2(vector._x / scalar, vector._y / scalar); + } + + /// Computes the vector sum of the two specified two-dimensional + /// vectors. + /// + /// The vector sum of the two specified two-dimensional vectors. + /// + /// + /// The first two-dimensional vector. + /// The second two-dimensional vector. + public static Vector2 operator +(Vector2 vector1, Vector2 vector2) + { + return new Vector2(vector1._x + vector2._x, vector1._y + vector2._y); + } + + /// Computes the vector difference of the two specified + /// two-dimensional vectors + /// + /// The vector difference of the two specified two-dimensional + /// vectors. + /// + /// The first two-dimensional vector. + /// The second two-dimensional vector. + public static Vector2 operator -(Vector2 vector1, Vector2 vector2) + { + return new Vector2(vector1._x - vector2._x, vector1._y - vector2._y); + } + + /// Computes the negation of the specified two-dimensional + /// vector. + /// + /// The negation of the specified two-dimensional vector. + /// + /// + /// The two-dimensional vector. + public static Vector2 operator -(Vector2 vector) + { + return new Vector2(-vector._x, -vector._y); + } + } +} diff --git a/FishROV/Assets/ThirdParty/RVO2-CS/RVOCS/Vector2.cs.meta b/FishROV/Assets/ThirdParty/RVO2-CS/RVOCS/Vector2.cs.meta new file mode 100644 index 0000000..e96e5cb --- /dev/null +++ b/FishROV/Assets/ThirdParty/RVO2-CS/RVOCS/Vector2.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: d8e0975e105dca34aa11badd99980f16 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/FishROV/Assets/ThirdParty/RVO2-CS/THIRD_PARTY_NOTICE.md b/FishROV/Assets/ThirdParty/RVO2-CS/THIRD_PARTY_NOTICE.md new file mode 100644 index 0000000..eff2aba --- /dev/null +++ b/FishROV/Assets/ThirdParty/RVO2-CS/THIRD_PARTY_NOTICE.md @@ -0,0 +1,11 @@ +# RVO2-CS + +本目录导入的是 `snape/RVO2-CS` 的核心 `RVOCS` 源码，用于本地避障 benchmark 的 RVO2 方案。 + +- 上游仓库：https://github.com/snape/RVO2-CS +- 官方 RVO2 页面：https://gamma.cs.unc.edu/RVO2/ +- 许可证：Apache License 2.0，见 `LICENSE` 与 `LICENSES/Apache-2.0.txt` + +Unity 兼容改动： + +- `Simulator.cs` 中的 .NET 新版参数检查 API 已替换为 `UnityArgumentOutOfRange`，以兼容 Unity 2022 的运行时。 diff --git a/FishROV/Assets/ThirdParty/RVO2-CS/THIRD_PARTY_NOTICE.md.meta b/FishROV/Assets/ThirdParty/RVO2-CS/THIRD_PARTY_NOTICE.md.meta new file mode 100644 index 0000000..18df5c0 --- /dev/null +++ b/FishROV/Assets/ThirdParty/RVO2-CS/THIRD_PARTY_NOTICE.md.meta @@ -0,0 +1,7 @@ +fileFormatVersion: 2 +guid: b985d3a31ffcb5a44b2e2442cb2c8fb9 +TextScriptImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/FishROV/Docs/local-avoidance-benchmark-notes.md b/FishROV/Docs/local-avoidance-benchmark-notes.md index 7453706..7d362b7 100644 --- a/FishROV/Docs/local-avoidance-benchmark-notes.md +++ b/FishROV/Docs/local-avoidance-benchmark-notes.md @@ -16,7 +16,7 @@ - 小鱼数量预设和滑条 - 暂停 / 继续 / 重置 - 平均 FPS、1% Low FPS、模拟耗时、GC 分配 -- 表现对象：运行时创建的 Cube 和 Capsule。 +- 表现对象：鱼群为运行时圆锥，圆锥尖头表示朝向；鲨鱼为 Capsule。 - 共享场景： - 自由巡游 - 对向穿流 @@ -56,21 +56,21 @@ A* RVO、RVO2、Unity NavMesh 都通过同一个调试面板选择，用同一 - 如果类型存在，adapter 会创建运行时 `RVOSimulator`，给 benchmark 对象添加 `RVOController`，把期望速度送入 A* RVO，再把反射得到的避障速度写回 `BenchmarkAgent.ApplyVelocity`。 - 共享场景、HUD、数量档、指标和相机逻辑不变。 -## RVO2 / RVO2-3D Adapter +## RVO2 Adapter -`Rvo2Adapter` 是反射 adapter。没有 RVO2 源码或包时，工程仍可编译。 +`Rvo2Adapter` 是反射 adapter。当前已在 `Assets/ThirdParty/RVO2-CS/` 导入 `snape/RVO2-CS` 的核心 `RVOCS` 源码，并保留 Apache-2.0 许可证说明。 -- 如果缺少 RVO2/RVO2-3D，选择 `RVO2` 时会创建同样的 benchmark 对象，并在 Game 视口状态中提示缺包，随后退回基线移动。 -- 要启用真实框架，需要把 C# 版本 RVO2/RVO2-3D 放到 `Assets/` 或 `Packages/`。 +- 选择 `RVO2` 时，adapter 会优先命中导入后的 `RVO.Simulator`。 +- 如果之后移除 RVO2 源码，工程仍可编译；Game 视口状态会提示缺包，并退回基线移动。 - adapter 会查找类似 `RVO.Simulator`、`RVO2.Simulator`、`RVO3D.Simulator`、`RVO.Simulator3D` 的 Simulator 类型。 - 必要方法包括 `addAgent` / `AddAgent`、`setAgentPrefVelocity` / `SetAgentPrefVelocity`、`getAgentVelocity` / `GetAgentVelocity`、`doStep` / `DoStep`。 +- 如果存在 `setAgentPosition` / `SetAgentPosition`，adapter 每次设置期望速度前会同步 Unity Transform 位置，避免 RVO 内部位置和 benchmark 边界夹取结果漂移。 - 2D 和 2.5D 使用 RVO 的 Vector2 风格类型，把 Unity `x/z` 映射到 RVO `x/y`；3D 模式在可用时绑定 Vector3 风格类型。 ## Unity NavMesh Adapter `UnityNavMeshAdapter` 使用内置 `UnityEngine.AI.NavMeshAgent` 做对照组。 -- benchmark 场景里的鱼和鲨鱼是运行时创建的对象，空场景默认不会有可用 NavMesh。 -- 没有运行时 NavMesh 数据时，adapter 会在 Game 视口状态中提示，并退回基线移动，不会添加无效 `NavMeshAgent`。 -- 如果要测试 Unity NavMesh 避障，需要在选择 `NavMesh` 前提供 NavMesh 数据；烘焙场景 NavMesh 即可。 -- 如果希望运行时生成 benchmark 地面 NavMesh，需要启用 `com.unity.ai.navigation` 并补 `NavMeshSurface` 构建流程。 +- adapter 会使用 `NavMeshBuilder.BuildNavMeshData` 为 benchmark 场景运行时生成一张平面 NavMesh，不需要手动烘焙场景。 +- 如果运行时 NavMesh 构建失败，Game 视口状态会提示，并退回基线移动，不会添加无效 `NavMeshAgent`。 +- 因为 NavMesh 是平面数据，3D 压测更适合作为 2D / 2.5D 对照；完整 3D 体积避障仍需要专门方案。