classdef CHOMPBodySpheresEncoding
%CHOMPBodySpheresEncoding A body spheres encoding used by manipulatorCHOMP
%   For computing collision avoidance costs during
%   manipulatorCHOMP/optimize, this encoding is useful to access the
%   spheres of a rigid body on a rigid body tree.

%Copyright 2022-2024 The MathWorks, Inc.

%#codegen
    properties(SetAccess=immutable,GetAccess=private)
        %EncodingMatrix Encoding of body point and its sphere on rigid body tree
        %   CHOMP iterates over all the body points during optimization of
        %   a trajectory. Each body point lies on a rigid body on the
        %   robot, and has a corresponding spherical representation. Thus,
        %   the encoding captured in EncodingMatrix is a 5 element vector with
        %   the first 4 elements representing the sphere corresponding to
        %   the body point, and the last element indicating the rigid body
        %   index it is on.
        EncodingMatrix
    end

    methods
        function obj=CHOMPBodySpheresEncoding(encodingMatrix)
        %Constructor
            obj.EncodingMatrix=encodingMatrix;
        end

        function num=numpoints(obj)
        %numpoints Number of body points
            num=size(obj.EncodingMatrix,2);
        end

        function maxsphrad=computeMaxRBTSpheresRadius(obj)
        %computeMaxRBTSpheresRadius Return the radius of the largest sphere in the encoding
            maxsphrad=max(obj.EncodingMatrix(1,:));
        end

        function bidx=rigidBodyIndex(obj,u)
        %bidx Rigid body's index on the rigid body tree
        %   BIDX=rigidBodyIndex(obj,U) returns the rigid body index at
        %   the body point at index U of the EncodingMatrix.
            bidx=obj.EncodingMatrix(5,u);
        end

        function center=sphereCenter(obj,u)
        %sphereCenter Coordinates of the center of the sphere at body point
        %   CENTER=sphereCenter(obj,U) returns the coordinates of the
        %   center of the sphere at body point U of the EncodingMatrix.
            center=obj.EncodingMatrix(2:4,u);
        end

        function sph=sphere(obj,u)
        %sphere The sphere at a body point
        %   SPH=sphere(obj,U) returns the sphere at a body point U of
        %   the EncodingMatrix
            sph=obj.EncodingMatrix(1:4,u);
        end

        function encoding=encodingAtIndices(obj,indices)
        %encodingAtIndices body spheres encoding at rigid body indices
            encodingSlice=obj.EncodingMatrix(:,indices);
            encoding=robotics.manip.internal.CHOMPBodySpheresEncoding(encodingSlice);
        end

        function bidx=rigidBodyIndices(obj)
        %rigidBodyIndices Rigid body indices of all body points
        %   BIDX=rigidBodyIndices(obj) Returns all the rigid body
        %   indices of every body point.
            bidx=obj.EncodingMatrix(5,:);
        end

    end
end
