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

	qD[rigidSphereA] = q;
}
//--------------------------------------------------------------------------------------------------------------------------------
//updates the rigid body Rotation
// A is rotation matrix, A = [AD1; AD2; AD3], first comp of q is rotation, last 3 components are axis of rot
// in wikipedia, last quat comp is the angle, in my version, first one is the angle.
__global__ void RotationMatirixFromQuaternion_kernel(real3 * AD1, real3 * AD2, real3 * AD3, real4 * qD) {
	uint rigidSphereA = blockIdx.x * blockDim.x + threadIdx.x;
	if (rigidSphereA >= numRigidBodiesD) {

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

	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 = 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(
