 		float3 * jInvD2,
 		float3 * omegaLRF_D) {
 	uint rigidSphereA = blockIdx.x * blockDim.x + threadIdx.x;
-	if (rigidSphereA > numRigidBodiesD) {
+	if (rigidSphereA >= numRigidBodiesD) {
 		return;
 	}
 

// in wikipedia, last quat comp is the angle, in my version, first one is the angle.
__global__ void RotationMatirixFromQuaternion_kernel(float3 * AD1, float3 * AD2, float3 * AD3, float4 * qD) {
	uint rigidSphereA = blockIdx.x * blockDim.x + threadIdx.x;
	if (rigidSphereA > numRigidBodiesD) {
		return;
	}
	float4 q = qD[rigidSphereA];

// A is rotation matrix, A = [AD1; AD2; AD3]
__global__ void UpdateRigidBodyQuaternion_kernel(float4 * qD, float3 * omegaLRF_D) {
	uint rigidSphereA = blockIdx.x * blockDim.x + threadIdx.x;
	if (rigidSphereA >= numRigidBodiesD) {
		return;
	}
	float3 omega = omegaLRF_D[rigidSphereA];
