occupancyMap3DCollisionOptions
Collision-checking options between 3-D occupancy map and collision geometries
Since R2022b
Description
The occupancyMap3DCollisionOptions
object contains options for checking for
collisions using the checkMapCollision
function, between occupied cells
of an occupancyMap3D
object
and collision geometry objects.
Creation
Description
returns a collision-checking options object,OPTS
= occupancyMap3DCollisionOptionsOPTS
.
specifies properties using one or more name-value arguments. For example,
OPTS
= occupancyMap3DCollisionOptions(Name=Value
)occupancyMap3DCollisionOptions(SearchDepth=8)
sets the
SearchDepth
property of the
occupancyMap3DCollisionOptions
object to a depth of
8
.
Properties
CheckBroadPhase
— Check collisions between AABBs of voxels and geometries
true
or 1
(default) | false
or 0
Check collisions between AABBs of voxels and geometries, specified as a logical 1
(true
) or 0
(false
).
If the CheckNarrowPhase
property is true
, the
narrow phase checks only the voxels that failed the broad phase check.
Example:
occupancyMap3DCollisionOptions(CheckBroadPhase=false)
Data Types: logical
CheckNarrowPhase
— Check collisions between voxels and raw input geometries
true
or 1
(default) | false
or 1
Check collisions between voxels and raw input geometries, specified as a logical 1
(true
) or 0
(false
).
If the CheckBroadPhase
property is true
, the
narrow phase checks only the voxels that were in collision during the broad phase
check.
Example:
occupancyMap3DCollisionOptions(CheckNarrowPhase=false)
Data Types: logical
Exhaustive
— Exhaustive search mode
false
or 0
(default) | true
or 1
Exhaustive search mode, specified as a logical 0
(false
) or 1
(true
). When
Exhaustive is specified as false, the collision-checking function stops collision
checking on the first valid collision in either the broad phase or narrow phase. When
specified as true
, the collision-checking function continues
collision checking until all voxels are checked.
Example: occupancyMap3DCollisionOptions(Exhaustive=true)
Data Types: logical
ReturnDistance
— Return closest point and distance
false
or 0
(default) | true
or 1
Return the closest point and distance, specified as a logical 0
(false
) or 1
(true
). When
specified as true
, the collision-checking function returns the
minimum distance between collision geometries and the nearest voxels in the occupancy
grid.
Example: occupancyMap3DCollisionOptions(ReturnDistance=true)
Data Types: logical
ReturnVoxels
— Return location and size of voxels in collision
false
or 0
(default) | true
or 1
Return the location and size of the voxels in collision, specified as a logical
0
(false
) or 1
(true
).
Example: occupancyMap3DCollisionOptions(ReturnVoxels=true)
Data Types: logical
SearchDepth
— Maximum search depth to check
16
(default) | integer in range [0
, 16
]
Maximum search depth to check in the octree, specified as an integer in the range
[0
, 16
].
If a voxel at one search depth encompasses any voxel that is occupied at a greater search depth, then the entire volume of the encompassing voxel is considered occupied. For more information, see Visualize 3-D Occupancy Maps with Varying Search Depths.
Example: occupancyMap3DCollisionOptions(SearchDepth=8)
Data Types: uint8
Examples
Check Collision Between 3-D Map and Collision Geometries
Create a 3-D occupancy map.
map = occupancyMap3D;
Specify 25 random coordinates in the occupancy map as occupied.
rng(0) pt = (rand(25,3)-.5)*20; setOccupancy(map,pt,1);
Create a collision sphere and a collision cylinder object.
sphere = collisionSphere(1); cylinder = collisionCylinder(3,6); sphere.Pose = trvec2tform([6.1 -4 -7.5]);
Visualize the occupancy map and collision geometry in the same figure.
exampleHelperPlotCylinderAndSphere(map,cylinder,sphere)
Perform only the broad-phase collision check for both the sphere and cylinder by setting the CheckNarrowPhase
property of an occupancyMap3DCollisionOptions
object to false
. Return voxel information and the distance to the nearest occupied voxels.
bpOpts = occupancyMap3DCollisionOptions(CheckNarrowPhase=false,ReturnDistance=true,ReturnVoxels=true); [bpIsCollidingCylinder,bpResultsCylinder] = checkMapCollision(map,cylinder,bpOpts);
Check the voxel distances for the collision geometries. Note that, because the cylinder is in collision with voxels, the distance values are NaN
. Because the sphere is not in collision with any voxels, its distance results are non-NaN
values.
bpDistCylinder = bpResultsCylinder.DistanceInfo.Distance
bpDistCylinder = NaN
bpWitnessptsCylinder = bpResultsCylinder.DistanceInfo.WitnessPoints
bpWitnessptsCylinder = 3×2
NaN NaN
NaN NaN
NaN NaN
Because the cylinder is in collision with the voxels, the distance results contain NaN
values. Since the sphere is not in collision with the voxels, the distance results consist of non-NaN
values.
[bpIsCollidingSphere,bpResultsSphere] = checkMapCollision(map,sphere,bpOpts); bpDistSphere = bpResultsSphere.DistanceInfo.Distance
bpDistSphere = 2.3259
bpWitnessptsSphere = bpResultsSphere.DistanceInfo.WitnessPoints
bpWitnessptsSphere = 3×2
3.0000 5.1000
-6.0000 -5.0000
-7.5000 -7.5000
Plot a line between the sphere and the closest voxel to it using its witness points.
figure exampleHelperPlotCylinderAndSphere(map,cylinder,sphere) hold on plot3(bpWitnessptsSphere(1,:),bpWitnessptsSphere(2,:),bpWitnessptsSphere(3,:),LineWidth=2,Color='r') hold off
Now perform a narrow-phase check, by using an occupancyMap3DCollisionOptions
object with the CheckNarrowPhase
property set to true
.
npOpts = occupancyMap3DCollisionOptions(CheckNarrowPhase=true,ReturnDistance=true,ReturnVoxels=true); [npIsCollidingSphere,bpResultsSphere] = checkMapCollision(map,sphere,npOpts);
Return the voxel distance and witness point coordinates for the sphere. The distance and witness points are slightly more accurate this time, because the narrow phase uses the distance between the primitive and the voxel, whereas the broad phase before uses the distance between the axis-aligned bounding box (AABB) of the collision object and the voxel.
npDist = bpResultsSphere.DistanceInfo.Distance
npDist = 2.6892
npWitnesspts = bpResultsSphere.DistanceInfo.WitnessPoints
npWitnesspts = 3×2
3.0000 5.2596
-6.0000 -4.5419
-7.5000 -7.5000
Visualize the occupancy map again and plot line showing the shortest distance between the voxel and sphere. The line between the witness points visually appears accurate after performing the narrow-phase check.
exampleHelperPlotCylinderAndSphere(map,cylinder,sphere) hold on plot3(npWitnesspts(1,:),npWitnesspts(2,:),npWitnesspts(3,:),LineWidth=2,Color='r') hold off
Extended Capabilities
C/C++ Code Generation
Generate C and C++ code using MATLAB® Coder™.
Version History
Introduced in R2022b
See Also
MATLAB Command
You clicked a link that corresponds to this MATLAB command:
Run the command by entering it in the MATLAB Command Window. Web browsers do not support MATLAB commands.
Select a Web Site
Choose a web site to get translated content where available and see local events and offers. Based on your location, we recommend that you select: .
You can also select a web site from the following list:
How to Get Best Site Performance
Select the China site (in Chinese or English) for best site performance. Other MathWorks country sites are not optimized for visits from your location.
Americas
- América Latina (Español)
- Canada (English)
- United States (English)
Europe
- Belgium (English)
- Denmark (English)
- Deutschland (Deutsch)
- España (Español)
- Finland (English)
- France (Français)
- Ireland (English)
- Italia (Italiano)
- Luxembourg (English)
- Netherlands (English)
- Norway (English)
- Österreich (Deutsch)
- Portugal (English)
- Sweden (English)
- Switzerland
- United Kingdom (English)