main/src/georegression/geometry/GeometryMath_F64.java
public static <T extends GeoTuple3D_F64> T addMult( T p0, DMatrixRMaj M, T p1, T result ) {
		if( M.numRows != 3 || M.numCols != 3 )
			throw new IllegalArgumentException( "Input matrix must be 3 by 3, not " + M.numRows + " " + M.numCols );

		if( result == null ) {
			result = (T) p0.createNewInstance();
		}

		double x = p1.x;
		double y = p1.y;
		double z = p1.z;

		result.x = p0.x + (double) ( M.data[0] * x + M.data[1] * y + M.data[2] * z );
		result.y = p0.y + (double) ( M.data[3] * x + M.data[4] * y + M.data[5] * z );
		result.z = p0.z + (double) ( M.data[6] * x + M.data[7] * y + M.data[8] * z );

		return result;
	}
public static <T extends GeoTuple3D_F64> T addMult( T p0, DMatrixRMaj M, T p1, T result ) {
		if( M.numRows != 3 || M.numCols != 3 )
			throw new IllegalArgumentException( "Input matrix must be 3 by 3, not " + M.numRows + " " + M.numCols );

		if( result == null ) {
			result = (T) p0.createNewInstance();
		}

		double x = p1.x;
		double y = p1.y;
		double z = p1.z;

		result.x = p0.x + (double) ( M.data[0] * x + M.data[1] * y + M.data[2] * z );
		result.y = p0.y + (double) ( M.data[3] * x + M.data[4] * y + M.data[5] * z );
		result.z = p0.z + (double) ( M.data[6] * x + M.data[7] * y + M.data[8] * z );

		return result;
	}
<p>ret = p0 + M*p1</p>

Safe to pass in the same instance of a point more than once.