Laser Scanning Simulation Using a Simple Physics Engine in MATLAB

This article provides a simulation environment for modeling laser beams hitting object surfaces and the ground. Users can configure the laser range, resolution, object position, size, and rotation.

Recently, we needed to analyze occlusions caused by objects in a laser scanner's background. Unable to find a suitable environment, we built one in MATLAB.

The principle is straightforward, but there are many details to handle. What was expected to take a day ended up taking three days of debugging.

Adhering to a "if it works, don't fix it" philosophy, the code hasn't been thoroughly tested and has minor bugs. For instance, objects closer to the scanner must be loaded first, and the laser source should not be inside an object.

Another post shows a program for calculating scanned coverage area: Simple Physics Engine — Estimating Laser Scan Coverage Area with MATLAB

Simulation Preview

Implementation Flow

Generate Laser Beams: Create beams with specified angular range and resolution at a given position and orientation.
Create Cuboids: Based on input position and size, generate cuboids in space, compute their 8 vertices, and derive gemoetric information for each of the 6 faces.
Calculate Intersections: For each laser beam, find intersections with all planes of all cuboids. Note: A single beam may intersect two planes of a cuboid; keep only the closest point to the source. Also, collinear but opposite-direction beams may be generated; keep only those pointing toward the cuboid by checking the angle between the beam direction and the plane normal.
Mark beams that hit an object; they are excluded from subsequent iterations.
Background Intersections: Beams that did not hit any object are used to compute intersections with the background.
Plotting: Visualize results. (Coverage area calculation is covered in another post.)

MATLAB Implementation

Main Program: `Environment_Main.m`

clc; clear;
% Basic parameters
% Scanner settings
lidar.Location = [0 0 1200];  % Scanner origin
lidar.Rotation = [0 0 0];     % RPY rotation angles
horizontalStep = 1; verticalStep = 1;  % Step angles for elevation and azimuth
% Room dimensions
roomX = 8000; roomY = 6000; roomZ = 4000;
% Plot settings
axis equal;
axis([-roomX/2, roomX/2, -roomY/2, roomY/2, 0, roomZ]);
hold on; grid on;
xlabel('x'); ylabel('y'); zlabel('z');

% Generate laser beams
lidar.laserCount = 0;
laserPoints = zeros((360/verticalStep)*(360/horizontalStep), 3);
for elev = -90:horizontalStep:68   % Elevation angle
    for azim = -180:verticalStep:180
        lidar.laserCount = lidar.laserCount + 1;
        c = 0;
        RA = baseRotateMatrix(elev, azim, c);
        laser_point = RA * [100 0 0]';
        laserPoints(lidar.laserCount, :) = laser_point';
    end
end

lidar.Directions = laserPoints(1:lidar.laserCount, :);  % Direction vectors
lidar.Directions = (baseRotateMatrix(lidar.Rotation(1), lidar.Rotation(2), lidar.Rotation(3)) * lidar.Directions')';
lidar.Flag = ones(lidar.laserCount, 1);  % Ray flags: 1 = not yet hit object
lidar.Lasers = lidar.Directions + lidar.Location;  % Ray lines
% scatter3(lidar.Lasers(:,1), lidar.Lasers(:,2), lidar.Lasers(:,3), '.');

% Add objects here
% Cuboid-------------------------------------------------------------------
cube.scale = [500, 500, 2800];
cube.Location = [1000, 0, cube.scale(3)/2];
cube.Rotation = [0, 0, 0];
cube_output = Environment_Driver_Box_Create(cube);
Environment_Driver_Box_Screen(cube_output);
lidar.Flag = Environment_Driver_Cube_Line(lidar, cube_output);

% Cuboid-------------------------------------------------------------------
cube.scale = [500, 500, 500];
cube.Location = [-1200, 0, cube.scale(3)/2];
cube.Rotation = [0, 0, 45];
cube_output = Environment_Driver_Box_Create(cube);
Environment_Driver_Box_Screen(cube_output);
lidar.Flag = Environment_Driver_Cube_Line(lidar, cube_output);

% Additional cuboids... (similar blocks omitted for brevity)

% Room walls intersection--------------------------------------------------
cube.scale = [roomX, roomY, roomZ];
cube.Location = [0, 0, roomZ/2];
cube.Rotation = [0, 0, 0];
cube_output = Environment_Driver_Box_Create(cube);
lidar.Flag = Environment_Driver_Room_Line(lidar, cube_output);

hold on;
fill3([-roomX/2, roomX/2, roomX/2, -roomX/2], [roomY/2, roomY/2, -roomY/2, -roomY/2], [0,0,0,0], [1 1 1]);
fill3([-roomX/2, roomX/2, roomX/2, -roomX/2], [roomY/2, roomY/2, roomY/2, roomY/2], [roomZ, roomZ, 0,0], [1 1 1]);
fill3([roomX/2, roomX/2, roomX/2, roomX/2], [-roomY/2, roomY/2, roomY/2, -roomY/2], [roomZ, roomZ, 0,0], [1 1 1]);

scatter3(lidar.Location(1), lidar.Location(2), lidar.Location(3), 200, '.');

Function: `Environment_Driver_Box_Create.m`

function output = Environment_Driver_Box_Create(input)
    % Compute 8 vertices of cuboid
    % Points A through H (see diagram in original)
    Point = zeros(8,3);
    Point(1,:) = [input.scale(1)/2, -input.scale(2)/2, input.scale(3)/2];   % A
    Point(2,:) = [input.scale(1)/2,  input.scale(2)/2, input.scale(3)/2];   % B
    Point(3,:) = [input.scale(1)/2,  input.scale(2)/2, -input.scale(3)/2];  % C
    Point(4,:) = [input.scale(1)/2, -input.scale(2)/2, -input.scale(3)/2];  % D
    Point(5,:) = [-input.scale(1)/2, -input.scale(2)/2, input.scale(3)/2];  % E
    Point(6,:) = [-input.scale(1)/2,  input.scale(2)/2, input.scale(3)/2];  % F
    Point(7,:) = [-input.scale(1)/2,  input.scale(2)/2, -input.scale(3)/2]; % G
    Point(8,:) = [-input.scale(1)/2, -input.scale(2)/2, -input.scale(3)/2]; % H

    % RPY rotation matrix
    a = input.Rotation(2); b = input.Rotation(3); c = input.Rotation(1);
    RA = baseRotateMatrix(a,b,c);
    Point = (RA * Point')' + input.Location;

    output.point = Point';
    % Compute plane equations for each face (order matters for normals)
    output.plane(1,:) = planeEquation(Point([1 2 3 4],:));  % ABCD
    output.plane(2,:) = planeEquation(Point([6 2 1 5],:));  % FBAE
    output.plane(3,:) = planeEquation(Point([3 2 6 7],:));  % CBFG
    output.plane(4,:) = planeEquation(Point([8 4 3 7],:));  % HDCG
    output.plane(5,:) = planeEquation(Point([8 7 6 5],:));  % HGFE
    output.plane(6,:) = planeEquation(Point([1 4 8 5],:));  % ADHE

    % Store line segments for boundary checks
    output.line(1,:) = reshape(Point([1 2 3 4],:)', 1, 12);
    output.line(2,:) = reshape(Point([6 2 1 5],:)', 1, 12);
    output.line(3,:) = reshape(Point([3 2 6 7],:)', 1, 12);
    output.line(4,:) = reshape(Point([8 4 3 7],:)', 1, 12);
    output.line(5,:) = reshape(Point([8 7 6 5],:)', 1, 12);
    output.line(6,:) = reshape(Point([1 4 8 5],:)', 1, 12);

    % Center points of faces
    output.centerPoint(1,:) = mean(Point([1 2 3 4],:),1);
    output.centerPoint(2,:) = mean(Point([6 2 1 5],:),1);
    output.centerPoint(3,:) = mean(Point([3 2 6 7],:),1);
    output.centerPoint(4,:) = mean(Point([8 4 3 7],:),1);
    output.centerPoint(5,:) = mean(Point([8 7 6 5],:),1);
    output.centerPoint(6,:) = mean(Point([1 4 8 5],:),1);
end

function plane = planeEquation(pts)
    % pts: 4x3 matrix of face corners
    p1 = pts(2,:) - pts(1,:);
    p2 = pts(2,:) - pts(3,:);
    normal = cross(p1, p2);
    normal = normal / norm(normal);
    d = -dot(pts(1,:), normal);
    plane = [normal, d];
end

Function: `Environment_Driver_Box_Screen.m`

function Environment_Driver_Box_Screen(input)
    point = input.point;
    fill3(point(1,[1 2 3 4]), point(2,[1 2 3 4]), point(3,[1 2 3 4]), 1);
    fill3(point(1,[1 4 8 5]), point(2,[1 4 8 5]), point(3,[1 4 8 5]), 1);
    fill3(point(1,[8 7 6 5]), point(2,[8 7 6 5]), point(3,[8 7 6 5]), 1);
    fill3(point(1,[6 7 3 2]), point(2,[6 7 3 2]), point(3,[6 7 3 2]), 1);
    fill3(point(1,[1 2 6 5]), point(2,[1 2 6 5]), point(3,[1 2 6 5]), 1);
    fill3(point(1,[3 4 8 7]), point(2,[3 4 8 7]), point(3,[3 4 8 7]), 1);
end

Function: `Environment_Driver_Cube_Line.m`

function output = Environment_Driver_Cube_Line(lidar, cube)
    % Compute intersections of rays with a cuboid
    intersectionPoints = zeros(5000, 3);
    pointCount = 0;
    hold on;
    
    for i = 1:lidar.laserCount
        if lidar.Flag(i) == 0
            continue;
        end
        minDist = 1e6;
        bestPoint = [0 0 0];
        found = 0;
        
        for j = 1:6
            % Skip if ray is pointing away from the plane
            centerVec = cube.centerPoint(j,:) - lidar.Location;
            angle = acos(dot(centerVec, lidar.Directions(i,:)) / (norm(centerVec)*norm(lidar.Directions(i,:))));
            if angle >= pi/2
                continue;
            end
            % Compute intersection with plane
            denom = abs(dot(cube.plane(j,1:3), lidar.Directions(i,:)));
            numer = abs(dot(cube.plane(j,1:3), lidar.Location) + cube.plane(j,4));
            if denom == 0
                continue;
            end
            t = numer / denom;
            point = lidar.Location + lidar.Directions(i,:) * t;
            % Check if point is inside face boundaries
            % Extract face edges
            edges = cube.line(j,:);
            p1 = edges(1:3); p2 = edges(4:6); p3 = edges(7:9); p4 = edges(10:12);
            L1 = norm(p1 - p2);
            L2 = norm(p2 - p3);
            d1 = norm(cross(point-p1, point-p2)) / norm(p2-p1);
            d2 = norm(cross(point-p2, point-p3)) / norm(p3-p2);
            d3 = norm(cross(point-p3, point-p4)) / norm(p4-p3);
            d4 = norm(cross(point-p4, point-p1)) / norm(p1-p4);
            if d1 > L2 || d2 > L1 || d3 > L2 || d4 > L1
                continue;
            end
            % Check if point lies on plane (tolerance 5 units)
            if abs(dot(cube.plane(j,1:3), point) + cube.plane(j,4)) > 5
                continue;
            end
            % Keep closest point
            dist = norm(point - lidar.Location);
            if dist < minDist
                minDist = dist;
                bestPoint = point;
                found = 1;
            end
        end
        if found
            pointCount = pointCount + 1;
            intersectionPoints(pointCount,:) = bestPoint;
            lidar.Flag(i) = 0;
        end
    end
    scatter3(intersectionPoints(:,1), intersectionPoints(:,2), intersectionPoints(:,3), 'r', '.');
    output = lidar.Flag;
end

Function: `Environment_Driver_Room_Line.m`

function output = Environment_Driver_Room_Line(lidar, cube)
    % Compute intersections with room walls (only three walls: right, front, ground)
    intersectionPoints = zeros(5000, 3);
    pointCount = 0;
    hold on;
    
    for i = 1:lidar.laserCount
        if lidar.Flag(i) == 0
            continue;
        end
        minDist = 1e6;
        bestPoint = [0 0 0];
        found = 0;
        
        for j = 1:6
            % Only consider walls 1, 3, 4 (right, front, ground) — adjust indices as needed
            if j == 2 || j == 5 || j == 6
                continue;
            end
            % Same logic as Environment_Driver_Cube_Line
            centerVec = cube.centerPoint(j,:) - lidar.Location;
            angle = acos(dot(centerVec, lidar.Directions(i,:)) / (norm(centerVec)*norm(lidar.Directions(i,:))));
            if angle >= pi/2
                continue;
            end
            denom = abs(dot(cube.plane(j,1:3), lidar.Directions(i,:)));
            numer = abs(dot(cube.plane(j,1:3), lidar.Location) + cube.plane(j,4));
            if denom == 0
                continue;
            end
            t = numer / denom;
            point = lidar.Location + lidar.Directions(i,:) * t;
            edges = cube.line(j,:);
            p1 = edges(1:3); p2 = edges(4:6); p3 = edges(7:9); p4 = edges(10:12);
            L1 = norm(p1 - p2);
            L2 = norm(p2 - p3);
            d1 = norm(cross(point-p1, point-p2)) / norm(p2-p1);
            d2 = norm(cross(point-p2, point-p3)) / norm(p3-p2);
            d3 = norm(cross(point-p3, point-p4)) / norm(p4-p3);
            d4 = norm(cross(point-p4, point-p1)) / norm(p1-p4);
            if d1 > L2 || d2 > L1 || d3 > L2 || d4 > L1
                continue;
            end
            if abs(dot(cube.plane(j,1:3), point) + cube.plane(j,4)) > 5
                continue;
            end
            dist = norm(point - lidar.Location);
            if dist < minDist
                minDist = dist;
                bestPoint = point;
                found = 1;
            end
        end
        if found
            pointCount = pointCount + 1;
            intersectionPoints(pointCount,:) = bestPoint;
            lidar.Flag(i) = 0;
        end
    end
    scatter3(intersectionPoints(:,1), intersectionPoints(:,2), intersectionPoints(:,3), 'r', '.');
    output = lidar.Flag;
end

Function: `baseRotateMatrix.m`

function RA = baseRotateMatrix(a, b, c)
    % RPY rotation matrix: a=roll, b=pitch, c=yaw (angles in degrees)
    RA = zeros(3,3);
    RA(1,1) = cosd(a)*cosd(b);
    RA(2,1) = cosd(a)*sind(b);
    RA(3,1) = -sind(a);
    RA(1,2) = sind(c)*sind(a)*cosd(b) - cosd(c)*sind(b);
    RA(2,2) = sind(c)*sind(a)*sind(b) + cosd(c)*cosd(b);
    RA(3,2) = sind(c)*cosd(a);
    RA(1,3) = cosd(c)*sind(a)*cosd(b) + sind(c)*sind(b);
    RA(2,3) = cosd(c)*sind(a)*sind(b) - sind(c)*cosd(b);
    RA(3,3) = cosd(c)*cosd(a);
end

Reference Formulas

Intersection of line and plane (CSDN article)

If you found this helpful, a like would be appreciated!

Tags: laser scanning simulation MATLAB physics engine

Posted on Thu, 07 May 2026 20:47:33 +0000 by Benmcfc

Freaks City

Laser Scanning Simulation Using a Simple Physics Engine in MATLAB

Implementation Flow

MATLAB Implementation

Main Program: `Environment_Main.m`

Function: `Environment_Driver_Box_Create.m`

Function: `Environment_Driver_Box_Screen.m`

Function: `Environment_Driver_Cube_Line.m`

Function: `Environment_Driver_Room_Line.m`

Function: `baseRotateMatrix.m`

Reference Formulas

Hot Tags

Freaks City

Laser Scanning Simulation Using a Simple Physics Engine in MATLAB

Implementation Flow

MATLAB Implementation

Main Program: Environment_Main.m

Function: Environment_Driver_Box_Create.m

Function: Environment_Driver_Box_Screen.m

Function: Environment_Driver_Cube_Line.m

Function: Environment_Driver_Room_Line.m

Function: baseRotateMatrix.m

Reference Formulas

Hot Tags

Main Program: `Environment_Main.m`

Function: `Environment_Driver_Box_Create.m`

Function: `Environment_Driver_Box_Screen.m`

Function: `Environment_Driver_Cube_Line.m`

Function: `Environment_Driver_Room_Line.m`

Function: `baseRotateMatrix.m`