 	int indexOfClosestNode = int(s / l) * nNodes;
 	if (indexOfClosestNode == nNodes) indexOfClosestNode--;
 
-	real3 beamPointPos = ANCF_Point_Pos(ANCF_Nodes, ANCF_Slopes, indexOfClosestNode, s, l); //interpolation using ANCF beam, cubic hermit equation
-	real3 beamPointSlope = ANCF_Point_Slope(ANCF_Nodes, ANCF_Slopes, indexOfClosestNode, s, l); //interpolation using ANCF beam, cubic hermit equation
+	real3 beamPointPos = Calc_ANCF_Point_Pos(ANCF_Nodes, ANCF_Slopes, indexOfClosestNode, s, l); //interpolation using ANCF beam, cubic hermit equation
+	real3 beamPointSlope = Calc_ANCF_Point_Slope(ANCF_Nodes, ANCF_Slopes, indexOfClosestNode, s, l); //interpolation using ANCF beam, cubic hermit equation
 	real3 beamPointOmega;
 
-	real3 beamPointVel = ANCF_Point_Vel(ANCF_Nodes, ANCF_Slopes, ANCF_VelNodes, ANCF_VelSlopes, indexOfClosestNode, s, l); //interpolation using ANCF beam, cubic hermit equation
+	real3 beamPointVel = Calc_ANCF_Point_Vel(ANCF_Nodes, ANCF_Slopes, ANCF_VelNodes, ANCF_VelSlopes, indexOfClosestNode, s, l); //interpolation using ANCF beam, cubic hermit equation
 
 	real3 dist3 = flexSPH_MeshPos_LRF_D[index];
+	real3 beamPointSlopeInitial = flexSPH_MeshSlope_Initial_D[index];
+	//Important Important Important Important Important Important Important Important Important
+	//Important Important Important Important Important Important Important Important Important
+	//Important Important Important Important Important Important Important Important Important
+	// Assumed Calc_ANCF_Point_Slope returns the unit vector. theta calculation is based on this assumption. Also cross product
+	real_ theta = acos(dot(beamPointSlopeInitial, beamPointSlope));
+	real3 n3 = cross(beamPointSlopeInitial, beamPointSlope);
+	n3 /= length(n3);
+	real4 q = R4(cos(0.5 * theta),
+			n3.x * sin(0.5 * theta), n3.y * sin(0.5 * theta), n3.z * sin(0.5 * theta));
+	real3 A1, A2, A3;
+	RotationMatirixFromQuaternion_kernelD(A1, A2, A3, q);
+	posRadD[absMarkerIndex] = beamPointPos + R3(dot(A1, dist3), dot(A2, dist3), dot(A3, dist3));
+	real3 absOmega = 	Calc_ANCF_Point_Omega(ANCF_Nodes, ANCF_Slopes, ANCF_VelNodes, ANCF_VelSlopes, indexOfClosestNode, s, l); //interpolation using ANCF beam, cubic hermit equation
+	velMasD[absMarkerIndex] = beamPointVel + cross(absOmega, dist3);
+
 }
 //--------------------------------------------------------------------------------------------------------------------------------
 void MapSPH_ToGrid(

	int indexOfClosestNode = int(s / l) * nNodes;
	if (indexOfClosestNode == nNodes) indexOfClosestNode--;

	real3 beamPointPos = ANCF_Point_Pos(ANCF_Nodes, ANCF_Slopes, indexOfClosestNode, s, l); //interpolation using ANCF beam, cubic hermit equation

	real3 dist3 = posRadD[absMarkerIndex] - beamPointPos;
	flexSPH_MeshPos_LRF_D[index] = dist3;
}

//--------------------------------------------------------------------------------------------------------------------------------

	rigidIdentifierD.clear();
	rigidSPH_MeshPos_LRF_D.clear();
	flexSPH_MeshPos_LRF_D.clear();
	flexSPH_MeshSlope_Initial_D.clear();
	qD1.clear();
	AD1.clear();
	AD2.clear();
