 
 #include <cuda_runtime_api.h>
 #include <device_launch_parameters.h>
+#include <math.h>
 
-__global__ void shifting(PointXYZI *d_points, double *d_shift) {
+constexpr int THREADS_PER_BLOCK = 512;
 
-    int idx = threadIdx.x;
-    d_points[idx].x += d_shift[0];
-    d_points[idx].y += d_shift[1];
-    d_points[idx].z += d_shift[2];
+__global__ void shifting(PointXYZI *d_points, double *d_shift, size_t size) {
+
+    long idx = blockIdx.x * blockDim.x + threadIdx.x;
+
+    if (idx < size) {
+        d_points[idx].x += d_shift[0];
+        d_points[idx].y += d_shift[1];
+        d_points[idx].z += d_shift[2];
+    }
 }
 
-__global__ void rotate(PointXYZI *d_points, Quaternion *d_quaternion) {
+__global__ void rotate(PointXYZI *d_points, Quaternion *d_quaternion, size_t size) {
 
-    int idx = threadIdx.x;
+    long idx = blockIdx.x * blockDim.x + threadIdx.x;
 
     Quaternion q(d_points[idx]);
 

    cudaMemcpy(d_points, h_points, h_cloud.points.size() * sizeof(PointXYZI), cudaMemcpyHostToDevice);
    cudaMemcpy(d_shift, h_shift, shift.size() * sizeof(double), cudaMemcpyHostToDevice);

    int N = h_cloud.points.size();

    shifting<<<1,N>>>(d_points, d_shift);

    cudaDeviceSynchronize();


    cudaMemcpy(d_points, h_points, h_cloud.points.size() * sizeof(PointXYZI), cudaMemcpyHostToDevice);
    cudaMemcpy(d_quaternion, &h_quaternion, sizeof(Quaternion), cudaMemcpyHostToDevice);

    long N = h_cloud.points.size();
    int NUM_BLOCKS = ceil(N / THREADS_PER_BLOCK);

    rotate<<<NUM_BLOCKS,THREADS_PER_BLOCK>>>(d_points, d_quaternion, N);

    cudaDeviceSynchronize();

