shandong/FishROV
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Import RVO2 CS source

Browse Source
This commit is contained in:
JSD\13999
2026-06-13 14:57:30 +08:00
parent 47142e749d
commit 6af887adee
30 changed files with 3904 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files

27
FishROV/Assets/Scripts/Benchmark/FrameworkAdapters/Rvo2Adapter.cs
View File

@@ -12,7 +12,7 @@ namespace FishROV.AvoidanceBenchmark
private AvoidanceBenchmarkConfig config; private AvoidanceBenchmarkConfig config;
private bool initialized; private bool initialized;
public string Name => "RVO2 / RVO2-3D 本地避障"; public string Name => "RVO2 本地避障";
public bool IsAvailable => bridge != null && bridge.IsAvailable; public bool IsAvailable => bridge != null && bridge.IsAvailable;
public string Status => bridge != null public string Status => bridge != null
? bridge.Status ? bridge.Status
@@ -52,6 +52,7 @@ namespace FishROV.AvoidanceBenchmark
return; return;
} }
bridge.SetAgentPosition(id, agent.transform.position);
bridge.SetPreferredVelocity(id, preferredVelocity); bridge.SetPreferredVelocity(id, preferredVelocity);
} }
@@ -104,6 +105,7 @@ namespace FishROV.AvoidanceBenchmark
private readonly string status; private readonly string status;
private readonly MethodInfo addAgent; private readonly MethodInfo addAgent;
private readonly MethodInfo setAgentPrefVelocity; private readonly MethodInfo setAgentPrefVelocity;
private readonly MethodInfo setAgentPosition;
private readonly MethodInfo doStep; private readonly MethodInfo doStep;
private readonly MethodInfo getAgentVelocity; private readonly MethodInfo getAgentVelocity;
private readonly MethodInfo setTimeStep; private readonly MethodInfo setTimeStep;
@@ -119,6 +121,7 @@ namespace FishROV.AvoidanceBenchmark
string status, string status,
MethodInfo addAgent, MethodInfo addAgent,
MethodInfo setAgentPrefVelocity, MethodInfo setAgentPrefVelocity,
MethodInfo setAgentPosition,
MethodInfo doStep, MethodInfo doStep,
MethodInfo getAgentVelocity, MethodInfo getAgentVelocity,
MethodInfo setTimeStep, MethodInfo setTimeStep,
@@ -133,6 +136,7 @@ namespace FishROV.AvoidanceBenchmark
this.status = status; this.status = status;
this.addAgent = addAgent; this.addAgent = addAgent;
this.setAgentPrefVelocity = setAgentPrefVelocity; this.setAgentPrefVelocity = setAgentPrefVelocity;
this.setAgentPosition = setAgentPosition;
this.doStep = doStep; this.doStep = doStep;
this.getAgentVelocity = getAgentVelocity; this.getAgentVelocity = getAgentVelocity;
this.setTimeStep = setTimeStep; this.setTimeStep = setTimeStep;
@@ -168,6 +172,7 @@ namespace FishROV.AvoidanceBenchmark
var addAgent = FindMethod(simulatorType, "addAgent", "AddAgent", new[] { agentVectorType }); var addAgent = FindMethod(simulatorType, "addAgent", "AddAgent", new[] { agentVectorType });
var setAgentPrefVelocity = FindMethod(simulatorType, "setAgentPrefVelocity", "SetAgentPrefVelocity", new[] { typeof(int), 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 getAgentVelocity = FindMethod(simulatorType, "getAgentVelocity", "GetAgentVelocity", new[] { typeof(int) });
var doStep = FindMethod(simulatorType, "doStep", "DoStep", Type.EmptyTypes); var doStep = FindMethod(simulatorType, "doStep", "DoStep", Type.EmptyTypes);
var setTimeStep = FindMethod(simulatorType, "setTimeStep", "SetTimeStep", new[] { typeof(float) }); var setTimeStep = FindMethod(simulatorType, "setTimeStep", "SetTimeStep", new[] { typeof(float) });
@@ -188,6 +193,7 @@ namespace FishROV.AvoidanceBenchmark
use3D ? "RVO2-3D 已通过反射接入。" : "RVO2 已通过反射接入。", use3D ? "RVO2-3D 已通过反射接入。" : "RVO2 已通过反射接入。",
addAgent, addAgent,
setAgentPrefVelocity, setAgentPrefVelocity,
setAgentPosition,
doStep, doStep,
getAgentVelocity, getAgentVelocity,
setTimeStep, 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) public void SetPreferredVelocity(int id, Vector3 preferredVelocity)
{ {
try try
@@ -435,7 +458,7 @@ namespace FishROV.AvoidanceBenchmark
private static Rvo2ReflectionBridge Unavailable(string status = MissingPackageStatus) 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) private static object CreateOrGetSimulator(Type type)

8
FishROV/Assets/ThirdParty.meta Normal file
View File

@@ -0,0 +1,8 @@
fileFormatVersion: 2
guid: 08562b8a001a1ee459ff33a1ad143d23
folderAsset: yes
DefaultImporter:
externalObjects: {}
userData:
assetBundleName:
assetBundleVariant:

8
FishROV/Assets/ThirdParty/RVO2-CS.meta vendored Normal file
View File

@@ -0,0 +1,8 @@
fileFormatVersion: 2
guid: 3d0866b55235ff948be97fbeb160eb12
folderAsset: yes
DefaultImporter:
externalObjects: {}
userData:
assetBundleName:
assetBundleVariant:

202
FishROV/Assets/ThirdParty/RVO2-CS/LICENSE vendored Normal file
View File

@@ -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.

7
FishROV/Assets/ThirdParty/RVO2-CS/LICENSE.meta vendored Normal file
View File

@@ -0,0 +1,7 @@
fileFormatVersion: 2
guid: 2e6d028a17551e240ae4528dcafd0158
DefaultImporter:
externalObjects: {}
userData:
assetBundleName:
assetBundleVariant:

8
FishROV/Assets/ThirdParty/RVO2-CS/LICENSES.meta vendored Normal file
View File

@@ -0,0 +1,8 @@
fileFormatVersion: 2
guid: cb86a7b56767e3b4e9c9b2614f8dfaf0
folderAsset: yes
DefaultImporter:
externalObjects: {}
userData:
assetBundleName:
assetBundleVariant:

190
FishROV/Assets/ThirdParty/RVO2-CS/LICENSES/Apache-2.0.txt vendored Normal file
View File

@@ -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.

7
FishROV/Assets/ThirdParty/RVO2-CS/LICENSES/Apache-2.0.txt.meta vendored Normal file
View File

@@ -0,0 +1,7 @@
fileFormatVersion: 2
guid: a6d8b6920c5e24c40ac8c3b79ef24675
TextScriptImporter:
externalObjects: {}
userData:
assetBundleName:
assetBundleVariant:

111
FishROV/Assets/ThirdParty/RVO2-CS/README.md vendored Normal file
View File

@@ -0,0 +1,111 @@
<!--
README.md
RVO2 Library C#
SPDX-FileCopyrightText: 2008 University of North Carolina at Chapel Hill
SPDX-License-Identifier: CC-BY-SA-4.0
Creative Commons Attribution-ShareAlike 4.0 International Public License
You are free to:
* Share -- copy and redistribute the material in any medium or format
* ShareAlike -- If you remix, transform, or build upon the material, you must
distribute your contributions under the same license as the original
* Adapt -- remix, transform, and build upon the material for any purpose, even
commercially.
The licensor cannot revoke these freedoms as long as you follow the license
terms.
Under the following terms:
* Attribution -- You must give appropriate credit, provide a link to the
license, and indicate if changes were made. You may do so in any reasonable
manner, but not in any way that suggests the licensor endorses you or your
use.
* No additional restrictions -- You may not apply legal terms or technological
measures that legally restrict others from doing anything the license
permits.
Notices:
* You do not have to comply with the license for elements of the material in
the public domain or where your use is permitted by an applicable exception
or limitation.
* No warranties are given. The license may not give you all of the permissions
necessary for your intended use. For example, other rights such as publicity,
privacy, or moral rights may limit how you use the material.
Please send all bug reports to <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
<https://gamma.cs.unc.edu/RVO2/>
-->
Optimal Reciprocal Collision Avoidance for C#
=============================================
<https://gamma.cs.unc.edu/RVO2/>
[![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)
<!-- REUSE-IgnoreStart -->
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
&nbsp;&nbsp;<https://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 [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
<!-- REUSE-IgnoreEnd -->

7
FishROV/Assets/ThirdParty/RVO2-CS/README.md.meta vendored Normal file
View File

@@ -0,0 +1,7 @@
fileFormatVersion: 2
guid: d4db5e3fc23bec64cb3ea454f330224a
TextScriptImporter:
externalObjects: {}
userData:
assetBundleName:
assetBundleVariant:

8
FishROV/Assets/ThirdParty/RVO2-CS/RVOCS.meta vendored Normal file
View File

@@ -0,0 +1,8 @@
fileFormatVersion: 2
guid: 6af893cb0842767489c05c232f3c840c
folderAsset: yes
DefaultImporter:
externalObjects: {}
userData:
assetBundleName:
assetBundleVariant:

719
FishROV/Assets/ThirdParty/RVO2-CS/RVOCS/Agent.cs vendored Normal file
View File

@@ -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 <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
*
* <http://gamma.cs.unc.edu/RVO2/>
*/
using System;
using System.Collections.Generic;
namespace RVO
{
/// <summary>Defines an agent in the simulation.</summary>
internal class Agent
{
internal IList<KeyValuePair<float, Agent>> _agentNeighbors = new List<KeyValuePair<float, Agent>>();
internal IList<KeyValuePair<float, Obstacle>> _obstacleNeighbors = new List<KeyValuePair<float, Obstacle>>();
internal IList<Line> _orcaLines = new List<Line>();
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;
/// <summary>Computes the neighbors of this agent.</summary>
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);
}
}
/// <summary>Computes the new velocity of this agent.</summary>
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);
}
}
/// <summary>Inserts an agent neighbor into the set of neighbors of this
/// agent.</summary>
///
/// <param name="agent">A pointer to the agent to be inserted.</param>
/// <param name="rangeSq">The squared range around this agent.</param>
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<float, Agent>(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<float, Agent>(distSq, agent);
if (_agentNeighbors.Count == _maxNeighbors)
{
rangeSq = _agentNeighbors[_agentNeighbors.Count - 1].Key;
}
}
}
}
/// <summary>Inserts a static obstacle neighbor into the set of neighbors
/// of this agent.</summary>
///
/// <param name="obstacle">The number of the static obstacle to be
/// inserted.</param>
/// <param name="rangeSq">The squared range around this agent.</param>
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<float, Obstacle>(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<float, Obstacle>(distSq, obstacle);
}
}
/// <summary>Updates the two-dimensional position and two-dimensional
/// velocity of this agent.</summary>
internal void Update()
{
_velocity = _newVelocity;
if (RVOMath.AbsSq(_velocity) > _maxSpeed * _maxSpeed)
{
_velocity = RVOMath.Normalize(_velocity) * _maxSpeed;
}
_position += _velocity * Simulator.Instance.TimeStep;
}
/// <summary>Solves a one-dimensional linear program on a specified line
/// subject to linear constraints defined by lines and a circular
/// constraint.</summary>
///
/// <returns>True if successful.</returns>
///
/// <param name="lines">Lines defining the linear constraints.</param>
/// <param name="lineNo">The specified line constraint.</param>
/// <param name="radius">The radius of the circular constraint.</param>
/// <param name="optVelocity">The optimization velocity.</param>
/// <param name="directionOpt">True if the direction should be optimized.
/// </param>
/// <param name="result">A reference to the result of the linear program.
/// </param>
private bool LinearProgram1(IList<Line> 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;
}
/// <summary>Solves a two-dimensional linear program subject to linear
/// constraints defined by lines and a circular constraint.</summary>
///
/// <returns>The number of the line it fails on, and the number of lines
/// if successful.</returns>
///
/// <param name="lines">Lines defining the linear constraints.</param>
/// <param name="radius">The radius of the circular constraint.</param>
/// <param name="optVelocity">The optimization velocity.</param>
/// <param name="directionOpt">True if the direction should be optimized.
/// </param>
/// <param name="result">A reference to the result of the linear program.
/// </param>
private int LinearProgram2(IList<Line> 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;
}
/// <summary>Solves a two-dimensional linear program subject to linear
/// constraints defined by lines and a circular constraint.</summary>
///
/// <param name="lines">Lines defining the linear constraints.</param>
/// <param name="numObstLines">Count of obstacle lines.</param>
/// <param name="beginLine">The line on which the 2-d linear program
/// failed.</param>
/// <param name="radius">The radius of the circular constraint.</param>
/// <param name="result">A reference to the result of the linear program.
/// </param>
private void LinearProgram3(IList<Line> 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<Line> projLines = new List<Line>();
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);
}
}
}
}
}

11
FishROV/Assets/ThirdParty/RVO2-CS/RVOCS/Agent.cs.meta vendored Normal file
View File

@@ -0,0 +1,11 @@
fileFormatVersion: 2
guid: 8acc64d87f8b9c14a966b32cd9aab419
MonoImporter:
externalObjects: {}
serializedVersion: 2
defaultReferences: []
executionOrder: 0
icon: {instanceID: 0}
userData:
assetBundleName:
assetBundleVariant:

590
FishROV/Assets/ThirdParty/RVO2-CS/RVOCS/KdTree.cs vendored Normal file
View File

@@ -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 <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
*
* <http://gamma.cs.unc.edu/RVO2/>
*/
using System;
using System.Collections.Generic;
namespace RVO
{
/// <summary>Defines k-D trees for agents and static obstacles in the
/// simulation.</summary>
internal class KdTree
{
/// <summary>Defines a node of an agent k-D tree.</summary>
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;
}
/// <summary>Defines a pair of scalar values.</summary>
private struct FloatPair
{
private readonly float _a;
private readonly float _b;
/// <summary>Constructs and initializes a pair of scalar
/// values.</summary>
///
/// <param name="a">The first scalar value.</param>
/// <param name="b">The second scalar value.</param>
internal FloatPair(float a, float b)
{
_a = a;
_b = b;
}
/// <summary>Returns true if the first pair of scalar values is less
/// than the second pair of scalar values.</summary>
///
/// <returns>True if the first pair of scalar values is less than the
/// second pair of scalar values.</returns>
///
/// <param name="pair1">The first pair of scalar values.</param>
/// <param name="pair2">The second pair of scalar values.</param>
public static bool operator <(FloatPair pair1, FloatPair pair2)
{
return pair1._a < pair2._a || pair1._a == pair2._a && pair1._b < pair2._b;
}
/// <summary>Returns true if the first pair of scalar values is less
/// than or equal to the second pair of scalar values.</summary>
///
/// <returns>True if the first pair of scalar values is less than or
/// equal to the second pair of scalar values.</returns>
///
/// <param name="pair1">The first pair of scalar values.</param>
/// <param name="pair2">The second pair of scalar values.</param>
public static bool operator <=(FloatPair pair1, FloatPair pair2)
{
return (pair1._a == pair2._a && pair1._b == pair2._b) || pair1 < pair2;
}
/// <summary>Returns true if the first pair of scalar values is
/// greater than the second pair of scalar values.</summary>
///
/// <returns>True if the first pair of scalar values is greater than
/// the second pair of scalar values.</returns>
///
/// <param name="pair1">The first pair of scalar values.</param>
/// <param name="pair2">The second pair of scalar values.</param>
public static bool operator >(FloatPair pair1, FloatPair pair2)
{
return !(pair1 <= pair2);
}
/// <summary>Returns true if the first pair of scalar values is
/// greater than or equal to the second pair of scalar values.
/// </summary>
///
/// <returns>True if the first pair of scalar values is greater than
/// or equal to the second pair of scalar values.</returns>
///
/// <param name="pair1">The first pair of scalar values.</param>
/// <param name="pair2">The second pair of scalar values.</param>
public static bool operator >=(FloatPair pair1, FloatPair pair2)
{
return !(pair1 < pair2);
}
}
/// <summary>Defines a node of an obstacle k-D tree.</summary>
private class ObstacleTreeNode
{
internal Obstacle _obstacle;
internal ObstacleTreeNode _left;
internal ObstacleTreeNode _right;
};
/// <summary>The maximum size of an agent k-D tree leaf.</summary>
/* 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;
/// <summary>Builds an agent k-D tree.</summary>
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);
}
}
/// <summary>Builds an obstacle k-D tree.</summary>
internal void BuildObstacleTree()
{
_obstacleTree = new ObstacleTreeNode();
IList<Obstacle> obstacles = new List<Obstacle>(Simulator.Instance._obstacles.Count);
for (int i = 0; i < Simulator.Instance._obstacles.Count; ++i)
{
obstacles.Add(Simulator.Instance._obstacles[i]);
}
_obstacleTree = BuildObstacleTreeRecursive(obstacles);
}
/// <summary>Computes the agent neighbors of the specified agent.
/// </summary>
///
/// <param name="agent">The agent for which agent neighbors are to be
/// computed.</param>
/// <param name="rangeSq">The squared range around the agent.</param>
internal void ComputeAgentNeighbors(Agent agent, ref float rangeSq)
{
QueryAgentTreeRecursive(agent, ref rangeSq, 0);
}
/// <summary>Computes the obstacle neighbors of the specified agent.
/// </summary>
///
/// <param name="agent">The agent for which obstacle neighbors are to be
/// computed.</param>
/// <param name="rangeSq">The squared range around the agent.</param>
internal void ComputeObstacleNeighbors(Agent agent, float rangeSq)
{
QueryObstacleTreeRecursive(agent, rangeSq, _obstacleTree);
}
/// <summary>Queries the visibility between two points within a specified
/// radius.</summary>
///
/// <returns>True if q1 and q2 are mutually visible within the radius;
/// false otherwise.</returns>
///
/// <param name="q1">The first point between which visibility is to be
/// tested.</param>
/// <param name="q2">The second point between which visibility is to be
/// tested.</param>
/// <param name="radius">The radius within which visibility is to be
/// tested.</param>
internal bool QueryVisibility(Vector2 q1, Vector2 q2, float radius)
{
return QueryVisibilityRecursive(q1, q2, radius, _obstacleTree);
}
/// <summary>Recursive method for building an agent k-D tree.</summary>
///
/// <param name="begin">The beginning agent k-D tree node node index.
/// </param>
/// <param name="end">The ending agent k-D tree node index.</param>
/// <param name="node">The current agent k-D tree node index.</param>
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);
}
}
/// <summary>Recursive method for building an obstacle k-D tree.
/// </summary>
///
/// <returns>An obstacle k-D tree node.</returns>
///
/// <param name="obstacles">A list of obstacles.</param>
private ObstacleTreeNode BuildObstacleTreeRecursive(IList<Obstacle> 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<Obstacle> leftObstacles = new List<Obstacle>(minLeft);
for (int n = 0; n < minLeft; ++n)
{
leftObstacles.Add(null);
}
IList<Obstacle> rightObstacles = new List<Obstacle>(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;
}
}
/// <summary>Recursive method for computing the agent neighbors of the
/// specified agent.</summary>
///
/// <param name="agent">The agent for which agent neighbors are to be
/// computed.</param>
/// <param name="rangeSq">The squared range around the agent.</param>
/// <param name="node">The current agent k-D tree node index.</param>
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);
}
}
}
}
}
/// <summary>Recursive method for computing the obstacle neighbors of the
/// specified agent.</summary>
///
/// <param name="agent">The agent for which obstacle neighbors are to be
/// computed.</param>
/// <param name="rangeSq">The squared range around the agent.</param>
/// <param name="node">The current obstacle k-D node.</param>
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);
}
}
}
/// <summary>Recursive method for querying the visibility between two
/// points within a specified radius.</summary>
///
/// <returns>True if q1 and q2 are mutually visible within the radius;
/// false otherwise.</returns>
///
/// <param name="q1">The first point between which visibility is to be
/// tested.</param>
/// <param name="q2">The second point between which visibility is to be
/// tested.</param>
/// <param name="radius">The radius within which visibility is to be
/// tested.</param>
/// <param name="node">The current obstacle k-D node.</param>
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);
}
}
}

11
FishROV/Assets/ThirdParty/RVO2-CS/RVOCS/KdTree.cs.meta vendored Normal file
View File

@@ -0,0 +1,11 @@
fileFormatVersion: 2
guid: 41302aad673ebad45936305f3222217c
MonoImporter:
externalObjects: {}
serializedVersion: 2
defaultReferences: []
executionOrder: 0
icon: {instanceID: 0}
userData:
assetBundleName:
assetBundleVariant:

42
FishROV/Assets/ThirdParty/RVO2-CS/RVOCS/Line.cs vendored Normal file
View File

@@ -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 <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
*
* <http://gamma.cs.unc.edu/RVO2/>
*/
namespace RVO
{
/// <summary>Defines a directed line.</summary>
public struct Line
{
public Vector2 Direction;
public Vector2 Point;
}
}

11
FishROV/Assets/ThirdParty/RVO2-CS/RVOCS/Line.cs.meta vendored Normal file
View File

@@ -0,0 +1,11 @@
fileFormatVersion: 2
guid: 8cef7bba85dbe4647891612de7e749a2
MonoImporter:
externalObjects: {}
serializedVersion: 2
defaultReferences: []
executionOrder: 0
icon: {instanceID: 0}
userData:
assetBundleName:
assetBundleVariant:

47
FishROV/Assets/ThirdParty/RVO2-CS/RVOCS/Obstacle.cs vendored Normal file
View File

@@ -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 <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
*
* <http://gamma.cs.unc.edu/RVO2/>
*/
namespace RVO
{
/// <summary>Defines static obstacles in the simulation.</summary>
internal class Obstacle
{
internal Obstacle _next;
internal Obstacle _previous;
internal Vector2 _direction;
internal Vector2 _point;
internal int _id;
internal bool _convex;
}
}

11
FishROV/Assets/ThirdParty/RVO2-CS/RVOCS/Obstacle.cs.meta vendored Normal file
View File

@@ -0,0 +1,11 @@
fileFormatVersion: 2
guid: 5b59751871022b84e88519071917ffa4
MonoImporter:
externalObjects: {}
serializedVersion: 2
defaultReferences: []
executionOrder: 0
icon: {instanceID: 0}
userData:
assetBundleName:
assetBundleVariant:

155
FishROV/Assets/ThirdParty/RVO2-CS/RVOCS/RVOMath.cs vendored Normal file
View File

@@ -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 <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
*
* <http://gamma.cs.unc.edu/RVO2/>
*/
using System;
namespace RVO
{
/// <summary>Contains functions and constants used in multiple classes.
/// </summary>
public struct RVOMath
{
/// <summary>A sufficiently small positive number.</summary>
internal const float RVO_EPSILON = 0.00001f;
/// <summary>Computes the length of a specified two-dimensional vector.
/// </summary>
///
/// <param name="vector">The two-dimensional vector whose length is to be
/// computed.</param>
/// <returns>The length of the two-dimensional vector.</returns>
public static float Abs(Vector2 vector)
{
return MathF.Sqrt(AbsSq(vector));
}
/// <summary>Computes the squared length of a specified two-dimensional
/// vector.</summary>
///
/// <returns>The squared length of the two-dimensional vector.</returns>
///
/// <param name="vector">The two-dimensional vector whose squared length
/// is to be computed.</param>
public static float AbsSq(Vector2 vector)
{
return vector * vector;
}
/// <summary>Computes the normalization of the specified two-dimensional
/// vector.</summary>
///
/// <returns>The normalization of the two-dimensional vector.</returns>
///
/// <param name="vector">The two-dimensional vector whose normalization
/// is to be computed.</param>
public static Vector2 Normalize(Vector2 vector)
{
float length = Abs(vector);
if (length <= RVO_EPSILON)
{
return new Vector2(0.0f, 0.0f);
}
return vector / length;
}
/// <summary>Computes the determinant of a two-dimensional square matrix
/// with rows consisting of the specified two-dimensional vectors.
/// </summary>
///
/// <returns>The determinant of the two-dimensional square matrix.
/// </returns>
///
/// <param name="vector1">The top row of the two-dimensional square
/// matrix.</param>
/// <param name="vector2">The bottom row of the two-dimensional square
/// matrix.</param>
internal static float Det(Vector2 vector1, Vector2 vector2)
{
return vector1._x * vector2._y - vector1._y * vector2._x;
}
/// <summary>Computes the squared distance from a line segment with the
/// specified endpoints to a specified point.</summary>
///
/// <returns>The squared distance from the line segment to the point.
/// </returns>
///
/// <param name="vector1">The first endpoint of the line segment.</param>
/// <param name="vector2">The second endpoint of the line segment.
/// </param>
/// <param name="vector3">The point to which the squared distance is to
/// be calculated.</param>
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)));
}
/// <summary>Computes the signed distance from a line connecting the
/// specified points to a specified point.</summary>
///
/// <returns>Positive when the point c lies to the left of the line ab.
/// </returns>
///
/// <param name="a">The first point on the line.</param>
/// <param name="b">The second point on the line.</param>
/// <param name="c">The point to which the signed distance is to be
/// calculated.</param>
internal static float LeftOf(Vector2 a, Vector2 b, Vector2 c)
{
return Det(a - c, b - a);
}
}
}

11
FishROV/Assets/ThirdParty/RVO2-CS/RVOCS/RVOMath.cs.meta vendored Normal file
View File

@@ -0,0 +1,11 @@
fileFormatVersion: 2
guid: 30121b9365c39de4aad24770212c01c1
MonoImporter:
externalObjects: {}
serializedVersion: 2
defaultReferences: []
executionOrder: 0
icon: {instanceID: 0}
userData:
assetBundleName:
assetBundleVariant:

1368
FishROV/Assets/ThirdParty/RVO2-CS/RVOCS/Simulator.cs vendored Normal file
View File

File diff suppressed because it is too large Load Diff

11
FishROV/Assets/ThirdParty/RVO2-CS/RVOCS/Simulator.cs.meta vendored Normal file
View File

@@ -0,0 +1,11 @@
fileFormatVersion: 2
guid: 123507813c7674546a3b3050097363ef
MonoImporter:
externalObjects: {}
serializedVersion: 2
defaultReferences: []
executionOrder: 0
icon: {instanceID: 0}
userData:
assetBundleName:
assetBundleVariant:

47
FishROV/Assets/ThirdParty/RVO2-CS/RVOCS/UnityArgumentOutOfRange.cs vendored Normal file
View File

@@ -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);
}
}
}
}

11
FishROV/Assets/ThirdParty/RVO2-CS/RVOCS/UnityArgumentOutOfRange.cs.meta vendored Normal file
View File

@@ -0,0 +1,11 @@
fileFormatVersion: 2
guid: 0e4ae8cc39feef744babebc44c47f507
MonoImporter:
externalObjects: {}
serializedVersion: 2
defaultReferences: []
executionOrder: 0
icon: {instanceID: 0}
userData:
assetBundleName:
assetBundleVariant:

240
FishROV/Assets/ThirdParty/RVO2-CS/RVOCS/Vector2.cs vendored Normal file
View File

@@ -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 <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
*
* <http://gamma.cs.unc.edu/RVO2/>
*/
using System;
using System.Globalization;
namespace RVO
{
/// <summary>Defines a two-dimensional vector.</summary>
public readonly struct Vector2 : IEquatable<Vector2>
{
internal readonly float _x;
internal readonly float _y;
/// <summary>Constructs and initializes a two-dimensional vector from the
/// specified xy-coordinates.</summary>
///
/// <param name="x">The x-coordinate of the two-dimensional vector.
/// </param>
/// <param name="y">The y-coordinate of the two-dimensional vector.
/// </param>
public Vector2(float x, float y)
{
_x = x;
_y = y;
}
/// <summary>Returns the string representation of this vector.</summary>
///
/// <returns>The string representation of this vector.</returns>
public override string ToString()
{
return $"({_x.ToString(CultureInfo.InvariantCulture)},{_y.ToString(CultureInfo.InvariantCulture)})";
}
/// <summary>Returns true if this vector equals the specified vector.
/// </summary>
///
/// <returns>True if this vector equals the specified vector.</returns>
///
/// <param name="other">The vector to compare with this vector.</param>
public bool Equals(Vector2 other)
{
return _x == other._x && _y == other._y;
}
/// <summary>Returns true if this vector equals the specified object.
/// </summary>
///
/// <returns>True if this vector equals the specified object.</returns>
///
/// <param name="obj">The object to compare with this vector.</param>
public override bool Equals(object obj)
{
return obj is Vector2 other && Equals(other);
}
/// <summary>Returns the hash code for this vector.</summary>
///
/// <returns>The hash code for this vector.</returns>
public override int GetHashCode()
{
return HashCode.Combine(_x, _y);
}
/// <summary>Returns true if the two vectors are equal.</summary>
///
/// <returns>True if the two vectors are equal.</returns>
///
/// <param name="left">The first vector.</param>
/// <param name="right">The second vector.</param>
public static bool operator ==(Vector2 left, Vector2 right)
{
return left.Equals(right);
}
/// <summary>Returns true if the two vectors are not equal.</summary>
///
/// <returns>True if the two vectors are not equal.</returns>
///
/// <param name="left">The first vector.</param>
/// <param name="right">The second vector.</param>
public static bool operator !=(Vector2 left, Vector2 right)
{
return !left.Equals(right);
}
/// <summary>Gets the x-coordinate of this two-dimensional vector.
/// </summary>
///
/// <value>The x-coordinate of the two-dimensional vector.</value>
public float X => _x;
/// <summary>Gets the y-coordinate of this two-dimensional vector.
/// </summary>
///
/// <value>The y-coordinate of the two-dimensional vector.</value>
public float Y => _y;
/// <summary>Returns the x-coordinate of this two-dimensional vector.
/// </summary>
///
/// <returns>The x-coordinate of the two-dimensional vector.</returns>
[Obsolete("Use the X property instead.", false)]
public float x()
{
return _x;
}
/// <summary>Returns the y-coordinate of this two-dimensional vector.
/// </summary>
///
/// <returns>The y-coordinate of the two-dimensional vector.</returns>
[Obsolete("Use the Y property instead.", false)]
public float y()
{
return _y;
}
/// <summary>Computes the dot product of the two specified
/// two-dimensional vectors.</summary>
///
/// <returns>The dot product of the two specified two-dimensional
/// vectors.</returns>
///
/// <param name="vector1">The first two-dimensional vector.</param>
/// <param name="vector2">The second two-dimensional vector.</param>
public static float operator *(Vector2 vector1, Vector2 vector2)
{
return vector1._x * vector2._x + vector1._y * vector2._y;
}
/// <summary>Computes the scalar multiplication of the specified
/// two-dimensional vector with the specified scalar value.</summary>
///
/// <returns>The scalar multiplication of the specified two-dimensional
/// vector with the specified scalar value.</returns>
///
/// <param name="scalar">The scalar value.</param>
/// <param name="vector">The two-dimensional vector.</param>
public static Vector2 operator *(float scalar, Vector2 vector)
{
return vector * scalar;
}
/// <summary>Computes the scalar multiplication of the specified
/// two-dimensional vector with the specified scalar value.</summary>
///
/// <returns>The scalar multiplication of the specified two-dimensional
/// vector with the specified scalar value.</returns>
///
/// <param name="vector">The two-dimensional vector.</param>
/// <param name="scalar">The scalar value.</param>
public static Vector2 operator *(Vector2 vector, float scalar)
{
return new Vector2(vector._x * scalar, vector._y * scalar);
}
/// <summary>Computes the scalar division of the specified
/// two-dimensional vector with the specified scalar value.</summary>
///
/// <returns>The scalar division of the specified two-dimensional vector
/// with the specified scalar value.</returns>
///
/// <param name="vector">The two-dimensional vector.</param>
/// <param name="scalar">The scalar value.</param>
public static Vector2 operator /(Vector2 vector, float scalar)
{
return new Vector2(vector._x / scalar, vector._y / scalar);
}
/// <summary>Computes the vector sum of the two specified two-dimensional
/// vectors.</summary>
///
/// <returns>The vector sum of the two specified two-dimensional vectors.
/// </returns>
///
/// <param name="vector1">The first two-dimensional vector.</param>
/// <param name="vector2">The second two-dimensional vector.</param>
public static Vector2 operator +(Vector2 vector1, Vector2 vector2)
{
return new Vector2(vector1._x + vector2._x, vector1._y + vector2._y);
}
/// <summary>Computes the vector difference of the two specified
/// two-dimensional vectors</summary>
///
/// <returns>The vector difference of the two specified two-dimensional
/// vectors.</returns>
///
/// <param name="vector1">The first two-dimensional vector.</param>
/// <param name="vector2">The second two-dimensional vector.</param>
public static Vector2 operator -(Vector2 vector1, Vector2 vector2)
{
return new Vector2(vector1._x - vector2._x, vector1._y - vector2._y);
}
/// <summary>Computes the negation of the specified two-dimensional
/// vector.</summary>
///
/// <returns>The negation of the specified two-dimensional vector.
/// </returns>
///
/// <param name="vector">The two-dimensional vector.</param>
public static Vector2 operator -(Vector2 vector)
{
return new Vector2(-vector._x, -vector._y);
}
}
}

11
FishROV/Assets/ThirdParty/RVO2-CS/RVOCS/Vector2.cs.meta vendored Normal file
View File

@@ -0,0 +1,11 @@
fileFormatVersion: 2
guid: d8e0975e105dca34aa11badd99980f16
MonoImporter:
externalObjects: {}
serializedVersion: 2
defaultReferences: []
executionOrder: 0
icon: {instanceID: 0}
userData:
assetBundleName:
assetBundleVariant:

11
FishROV/Assets/ThirdParty/RVO2-CS/THIRD_PARTY_NOTICE.md vendored Normal file
View File

@@ -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 的运行时。

7
FishROV/Assets/ThirdParty/RVO2-CS/THIRD_PARTY_NOTICE.md.meta vendored Normal file
View File

@@ -0,0 +1,7 @@
fileFormatVersion: 2
guid: b985d3a31ffcb5a44b2e2442cb2c8fb9
TextScriptImporter:
externalObjects: {}
userData:
assetBundleName:
assetBundleVariant:

18
FishROV/Docs/local-avoidance-benchmark-notes.md
View File

@@ -16,7 +16,7 @@
- 小鱼数量预设和滑条 - 小鱼数量预设和滑条
- 暂停 / 继续 / 重置 - 暂停 / 继续 / 重置
- 平均 FPS、1% Low FPS、模拟耗时、GC 分配 - 平均 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`。 - 如果类型存在，adapter 会创建运行时 `RVOSimulator`，给 benchmark 对象添加 `RVOController`，把期望速度送入 A* RVO，再把反射得到的避障速度写回 `BenchmarkAgent.ApplyVelocity`。
- 共享场景、HUD、数量档、指标和相机逻辑不变。 - 共享场景、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 视口状态中提示缺包，随后退回基线移动。 - 选择 `RVO2` 时，adapter 会优先命中导入后的 `RVO.Simulator`。
- 要启用真实框架，需要把 C# 版本 RVO2/RVO2-3D 放到 `Assets/` 或 `Packages/`。 - 如果之后移除 RVO2 源码，工程仍可编译；Game 视口状态会提示缺包，并退回基线移动。
- adapter 会查找类似 `RVO.Simulator`、`RVO2.Simulator`、`RVO3D.Simulator`、`RVO.Simulator3D` 的 Simulator 类型。 - adapter 会查找类似 `RVO.Simulator`、`RVO2.Simulator`、`RVO3D.Simulator`、`RVO.Simulator3D` 的 Simulator 类型。
- 必要方法包括 `addAgent` / `AddAgent`、`setAgentPrefVelocity` / `SetAgentPrefVelocity`、`getAgentVelocity` / `GetAgentVelocity`、`doStep` / `DoStep`。 - 必要方法包括 `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 风格类型。 - 2D 和 2.5D 使用 RVO 的 Vector2 风格类型，把 Unity `x/z` 映射到 RVO `x/y`；3D 模式在可用时绑定 Vector3 风格类型。
## Unity NavMesh Adapter ## Unity NavMesh Adapter
`UnityNavMeshAdapter` 使用内置 `UnityEngine.AI.NavMeshAgent` 做对照组。 `UnityNavMeshAdapter` 使用内置 `UnityEngine.AI.NavMeshAgent` 做对照组。
- benchmark 场景里的鱼和鲨鱼是运行时创建的对象，空场景默认不会有可用 NavMesh。 - adapter 会使用 `NavMeshBuilder.BuildNavMeshData` 为 benchmark 场景运行时生成一张平面 NavMesh，不需要手动烘焙场景。
- 没有运行时 NavMesh 数据时，adapter 会在 Game 视口状态中提示，并退回基线移动，不会添加无效 `NavMeshAgent`。 - 如果运行时 NavMesh 构建失败，Game 视口状态会提示，并退回基线移动，不会添加无效 `NavMeshAgent`。
- 如果要测试 Unity NavMesh 避障，需要在选择 `NavMesh` 前提供 NavMesh 数据；烘焙场景 NavMesh 即可。 - 因为 NavMesh 是平面数据，3D 压测更适合作为 2D / 2.5D 对照；完整 3D 体积避障仍需要专门方案。
- 如果希望运行时生成 benchmark 地面 NavMesh，需要启用 `com.unity.ai.navigation` 并补 `NavMeshSurface` 构建流程。