Reassign with index (C++ file)

/* indexAssign_inplace.cpp */
#include "mex.h"
#include "matrix.h"
extern "C" int mxUnshareArray(mxArray *array_ptr, int level);

void modifyArrayByIndex(mxArray *inarr, const mxArray *inarr2, const mxArray *idxarr){
    size_t rowNum = mxGetM(inarr);
    size_t colNum = mxGetN(inarr);
    if (mxGetM(inarr2)!=rowNum || mxGetN(inarr2)!=colNum)
        mexErrMsgTxt("The dimensions of first two inputs must be equal.");

    double* pArr_A = (double*) mxGetPr(inarr);
    double* pArr_B = (double*) mxGetPr(inarr2);
    double* pVec_idx = (double*) mxGetPr(idxarr);
    size_t idxLength = mxGetNumberOfElements(idxarr);

    for (int i = 1; i < (int) idxLength; i++)
    {
        for (int j = 0; j < i; j++)
        {
            int index = ((int) pVec_idx[j] - 1) * rowNum + (int) pVec_idx[i] - 1;
            pArr_A[index] = pArr_B[index];
        }
    }
}

void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
    // input: prhs = [A, B, idx]
    if (nrhs != 3)
        mexErrMsgTxt("Input is not enough.");
    plhs[0] = (mxArray*) prhs[0];
    mxUnshareArray(plhs[0], true);
    modifyArrayByIndex(plhs[0], prhs[1], prhs[2]);
}

/* indexAssign.cpp */
#include "mex.h"
#include "matrix.h"

void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
    // input: prhs = [A, B, idx]
    if (nrhs != 3)
        mexErrMsgTxt("Input is not enough.");

    size_t rowNum = mxGetM(prhs[0]);
    size_t colNum = mxGetN(prhs[0]);
    if (mxGetM(prhs[1])!=rowNum || mxGetN(prhs[1])!=colNum)
        mexErrMsgTxt("The dimensions of first two inputs must be equal.");


    double* pArr_A = (double*) mxGetPr(prhs[0]);
    double* pArr_B = (double*) mxGetPr(prhs[1]);
    double* pVec_idx = (double*) mxGetPr(prhs[2]);
    size_t idxLength = mxGetNumberOfElements(prhs[2]);

    plhs[0] = mxDuplicateArray(prhs[0]);
    double* pOutputArr = (double*) mxGetPr(plhs[0]);
    for (int i = 1; i < (int) idxLength; i++)
    {
        for (int j = 0; j < i; j++)
        {
            int index = ((int) pVec_idx[j] - 1) * rowNum + (int) pVec_idx[i] - 1;
            pOutputArr[index] = pArr_B[index];
        }
    }
}