// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions
// are met:
//  * Redistributions of source code must retain the above copyright
//    notice, this list of conditions and the following disclaimer.
//  * Redistributions in binary form must reproduce the above copyright
//    notice, this list of conditions and the following disclaimer in the
//    documentation and/or other materials provided with the distribution.
//  * Neither the name of NVIDIA CORPORATION nor the names of its
//    contributors may be used to endorse or promote products derived
//    from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS ''AS IS'' AND ANY
// EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
// PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT OWNER OR
// CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
// EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
// PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY
// OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
//
// Copyright (c) 2008-2025 NVIDIA Corporation. All rights reserved.
// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  

#ifndef __SOLVER_BLOCK_TGS_CUH__
#define __SOLVER_BLOCK_TGS_CUH__

#include "common/PxPhysXCommonConfig.h"
#include <cuda.h>
#include <sm_35_intrinsics.h>
#include "PxgSolverBody.h"
//#include "PxgSolverConstraint1D.h"
#include "PxgSolverConstraintBlock1D.h"
#include "PxgSolverConstraintDesc.h"
#include "PxgConstraint.h"
#include "PxgConstraintBlock.h"
#include "PxgIslandContext.h"
#include "PxgSolverContext.h"
#include "cutil_math.h"
#include "PxgSolverCoreDesc.h"
#include "DyThresholdTable.h"
#include "PxgFrictionPatch.h"
#include "foundation/PxUtilities.h"
#include "PxgConstraintWriteBack.h"
#include "PxgSolverFlags.h"
#include "stdio.h"
#include "assert.h"
#include "PxgIntrinsics.h"
#include "solverResidual.cuh"

#include "DyCpuGpu1dConstraint.h"

#include "solverBlockCommon.cuh"
#include "constraintPrepShared.cuh"

using namespace physx;

static __forceinline__ __device__ float4 shfl(const PxU32 syncMask, const float4 f, const PxU32 index)
{
	float4 ret;
	ret.x = __shfl_sync(syncMask, f.x, index);
	ret.y = __shfl_sync(syncMask, f.y, index);
	ret.z = __shfl_sync(syncMask, f.z, index);
	ret.w = __shfl_sync(syncMask, f.w, index);

	return ret;
}

//Loads a set of TxIData inertia tensors quickly using shuffles
static __device__ void loadTxInertia(const PxU32 syncMask, const PxgSolverTxIData* datas, const PxU32 bodyIndex, const PxU32 nbToLoad, const PxU32 threadIndexInWarp, PxgSolverTxIData& out)
{
	if (1)
	{
		{
			float4 data0, data1, data2, data3;

			//There are 4x float4 in a PxgSolverTxIData
			const PxU32 elementIndex = threadIndexInWarp / 4;

			const PxU32 index0 = __shfl_sync(syncMask, bodyIndex, elementIndex);
			const PxU32 index1 = __shfl_sync(syncMask, bodyIndex, elementIndex + 8);
			const PxU32 index2 = __shfl_sync(syncMask, bodyIndex, elementIndex + 16);
			const PxU32 index3 = __shfl_sync(syncMask, bodyIndex, elementIndex + 24);
			if (elementIndex < nbToLoad)
			{
				data0 = reinterpret_cast<const float4*>(&datas[index0])[threadIndexInWarp & 3];
				if ((elementIndex + 8) < nbToLoad)
				{
					data1 = reinterpret_cast<const float4*>(&datas[index1])[threadIndexInWarp & 3];
					if ((elementIndex + 16) < nbToLoad)
					{
						data2 = reinterpret_cast<const float4*>(&datas[index2])[threadIndexInWarp & 3];
						if ((elementIndex + 24) < nbToLoad)
						{
							data3 = reinterpret_cast<const float4*>(&datas[index3])[threadIndexInWarp & 3];
						}
					}
				}
			}

			//OK. We have our 4 vectors...now we have to shuffle them
			//Thread 0 has the first float4 for T0, T8, T16, T24
			//Thread 1 has the 2nd float4 for T0, T8, T16, T24 ...
			//Thread 4 has the first float4 T1, T9, T17, T25...

			const PxU32 idxAnd3 = threadIndexInWarp & 3;

			float4 swapVal0 = idxAnd3 == 0 ? data0 : idxAnd3 == 1 ? data1 : idxAnd3 == 2 ? data2 : data3;
			float4 swapVal1 = idxAnd3 == 0 ? data3 : idxAnd3 == 1 ? data0 : idxAnd3 == 2 ? data1 : data2;
			float4 swapVal2 = idxAnd3 == 0 ? data2 : idxAnd3 == 1 ? data3 : idxAnd3 == 2 ? data0 : data1;
			float4 swapVal3 = idxAnd3 == 0 ? data1 : idxAnd3 == 1 ? data2 : idxAnd3 == 2 ? data3 : data0;

			const PxU32 threadReadBase = ((threadIndexInWarp * 4) & 31);

			const PxU32 offset = threadIndexInWarp / 8;

			float4 val0 = shfl(syncMask, swapVal0, threadReadBase + offset);
			float4 val1 = shfl(syncMask, swapVal1, threadReadBase + ((offset + 1) & 3));
			float4 val2 = shfl(syncMask, swapVal2, threadReadBase + ((offset + 2) & 3));
			float4 val3 = shfl(syncMask, swapVal3, threadReadBase + ((offset + 3) & 3));

			float4 shuffled0 = offset == 0 ? val0 : offset == 1 ? val3 : offset == 2 ? val2 : val1;
			float4 shuffled1 = offset == 0 ? val1 : offset == 1 ? val0 : offset == 2 ? val3 : val2;
			float4 shuffled2 = offset == 0 ? val2 : offset == 1 ? val1 : offset == 2 ? val0 : val3;
			float4 shuffled3 = offset == 0 ? val3 : offset == 1 ? val2 : offset == 2 ? val1 : val0;

			//Now copy into the structure...
			out.deltaBody2World.q.x = shuffled0.x; out.deltaBody2World.q.y = shuffled0.y; out.deltaBody2World.q.z = shuffled0.z; out.deltaBody2World.q.w = shuffled0.w;
			out.deltaBody2World.p.x = shuffled1.x; out.deltaBody2World.p.y = shuffled1.y; out.deltaBody2World.p.z = shuffled1.z;
			out.sqrtInvInertia.column0.x = shuffled1.w; out.sqrtInvInertia.column0.y = shuffled2.x; out.sqrtInvInertia.column0.z = shuffled2.y;
			out.sqrtInvInertia.column1.x = shuffled2.z; out.sqrtInvInertia.column1.y = shuffled2.w; out.sqrtInvInertia.column1.z = shuffled3.x;
			out.sqrtInvInertia.column2.x = shuffled3.y; out.sqrtInvInertia.column2.y = shuffled3.z; out.sqrtInvInertia.column2.z = shuffled3.w;
			
		}
	}
	else
	{
		if (threadIndexInWarp < nbToLoad)
			out = datas[bodyIndex];
	}
}

// Using the same logic as the previous implementation, but mass-splitting is additionally performed per sub-timestep.
// Required data is packed and stored differently from the previous implementation to split mass at each sub-timestep; 
// i.e., mass-related terms are computed at every sub-timestep.
// Refer to "Mass Splitting for Jitter-Free Parallel Rigid Body Simulation" for the general mass-splitting idea.

static __device__ void solveContactBlockTGS(const PxgBlockConstraintBatch& batch, PxVec3& b0LinVel, PxVec3& b0AngVel, PxVec3& b1LinVel, PxVec3& b1AngVel,
	const PxVec3& b0LinDelta, const PxVec3& b0AngDelta, const PxVec3& b1LinDelta, const PxVec3& b1AngDelta,
	const PxU32 threadIndex, const PxgTGSBlockSolverContactHeader* PX_RESTRICT contactHeaders,
	PxgTGSBlockSolverFrictionHeader* PX_RESTRICT frictionHeaders, PxgTGSBlockSolverContactPoint* PX_RESTRICT contactPoints,
	PxgTGSBlockSolverContactFriction* PX_RESTRICT frictionPoints,
	const PxReal elapsedTime, const PxReal minPen, PxgErrorAccumulator* error, 
	PxReal ref0 = 1.f, PxReal ref1 = 1.f)
{  
	using namespace physx;

	PxVec3 linVel0(b0LinVel.x, b0LinVel.y, b0LinVel.z);
	PxVec3 linVel1(b1LinVel.x, b1LinVel.y, b1LinVel.z);
	PxVec3 angVel0(b0AngVel.x, b0AngVel.y, b0AngVel.z);
	PxVec3 angVel1(b1AngVel.x, b1AngVel.y, b1AngVel.z);

	float accumulatedNormalImpulse = 0.f;

	{
		const PxgTGSBlockSolverContactHeader* PX_RESTRICT contactHeader = &contactHeaders[batch.mConstraintBatchIndex];
		PxgTGSBlockSolverFrictionHeader* PX_RESTRICT frictionHeader = &frictionHeaders[batch.mConstraintBatchIndex];
		PxgTGSBlockSolverContactFriction* PX_RESTRICT frictions = &frictionPoints[batch.startFrictionIndex];

		const uint numNormalConstr = Pxldcs(contactHeader->numNormalConstr[threadIndex]);
		const uint numFrictionConstrTotal = Pxldcs(frictionHeader->numFrictionConstr[threadIndex]);
		const uint numFrictionConstr = numFrictionConstrTotal & (~1);

		float accumDeltaF = 0.f;

		if (numNormalConstr)
		{
			const PxReal maxPenBias = Pxldcs(contactHeader->maxPenBias[threadIndex]);

			PxgTGSBlockSolverContactPoint* PX_RESTRICT contacts = &contactPoints[batch.startConstraintIndex];
			const float4 invMass0_1_angDom0_1 = Pxldcs(contactHeader->invMass0_1_angDom0_1[threadIndex]);

			const float invMassA = ref0 * invMass0_1_angDom0_1.x;
			const float invMassB = ref1 * invMass0_1_angDom0_1.y;

			const float angDom0 = ref0 * invMass0_1_angDom0_1.z;
			const float angDom1 = ref1 * invMass0_1_angDom0_1.w;

			const float sumInvMass = invMassA + invMassB;

			const float4 normal_staticFriction = Pxldcg(contactHeader->normal_staticFriction[threadIndex]);

			const PxVec3 normal = PxVec3(normal_staticFriction.x, normal_staticFriction.y, normal_staticFriction.z);

			const float restitutionXdt = contactHeader->restitutionXdt[threadIndex];
			const float p8 = contactHeader->p8[threadIndex]; // using previous p8 value in the prep step.
			const PxU8 flags = contactHeader->flags[threadIndex];

			const PxVec3 delLinVel0 = normal * invMassA;
			const PxVec3 delLinVel1 = normal * invMassB;

			//Bring forward a read event
			const float staticFrictionCof = normal_staticFriction.w;

			const PxVec3 relMotion = b0LinDelta - b1LinDelta;

			const float deltaV = normal.dot(relMotion);

			float relVel1 = (linVel0 - linVel1).dot(normal);

			float4 next_raXn_extraCoeff = Pxldcs(contacts->raXn_extraCoeff[threadIndex]);
			float4 next_rbXn_targetVelW = Pxldcs(contacts->rbXn_targetVelW[threadIndex]);
			float next_appliedForce = Pxldcs(contacts->appliedForce[threadIndex]);
			float next_error = Pxldcs(contacts->separation[threadIndex]);
			float next_maxImpulse = Pxldcs(contacts->maxImpulse[threadIndex]);
			float next_biasCoefficient = Pxldcs(contacts->biasCoefficient[threadIndex]);

			float next_resp0 = ref0 * Pxldcs(contacts->resp0[threadIndex]);
			float next_resp1 = ref1 * Pxldcs(contacts->resp1[threadIndex]);

			float next_unitResponse = next_resp0 + next_resp1;
			float next_recipResponse = (next_unitResponse > 0.f) ? (1.f / next_unitResponse) : 0.f;
			float next_velMultiplier = next_recipResponse;

			if (restitutionXdt < 0.f)
			{
				computeCompliantContactCoefficientsTGS(flags, restitutionXdt, next_unitResponse,
					next_recipResponse, next_raXn_extraCoeff.w, next_velMultiplier,
					next_biasCoefficient);
			}

			{
				for (uint i = 0; i < numNormalConstr; i++)
				{
					PxgTGSBlockSolverContactPoint& c = contacts[i];

					const float4 raXn_contactCoeff = next_raXn_extraCoeff;
					const float velMultiplier = next_velMultiplier;
					const float4 rbXn_targetVelW = next_rbXn_targetVelW;
					const float appliedForce = next_appliedForce;
					const float separation = next_error;
					const float maxImpulse = next_maxImpulse;
					const float biasCoefficient = next_biasCoefficient;
					const float recipResponse = next_recipResponse;

					if ((i + 1) < numNormalConstr)
					{
						const PxgTGSBlockSolverContactPoint& nextC = contacts[i + 1];

						next_raXn_extraCoeff = Pxldcs(nextC.raXn_extraCoeff[threadIndex]);
						next_rbXn_targetVelW = Pxldcs(nextC.rbXn_targetVelW[threadIndex]);
						next_appliedForce = Pxldcs(nextC.appliedForce[threadIndex]);
						next_error = Pxldcs(nextC.separation[threadIndex]);
						next_maxImpulse = Pxldcs(nextC.maxImpulse[threadIndex]);
						next_biasCoefficient = Pxldcs(nextC.biasCoefficient[threadIndex]);

						next_resp0 = ref0 * Pxldcs(nextC.resp0[threadIndex]);
						next_resp1 = ref1 * Pxldcs(nextC.resp1[threadIndex]);

						next_unitResponse = next_resp0 + next_resp1;
						next_recipResponse = (next_unitResponse > 0.f) ? (1.f / next_unitResponse) : 0.f;

						next_velMultiplier = next_recipResponse;

						if (restitutionXdt < 0.f)
						{
							computeCompliantContactCoefficientsTGS(flags, restitutionXdt, next_unitResponse,
								next_recipResponse, next_raXn_extraCoeff.w, next_velMultiplier,
								next_biasCoefficient);
						}
					}
					else if (numFrictionConstr)
					{
						next_raXn_extraCoeff = Pxldcs(frictions[0].raXn_error[threadIndex]);
						next_rbXn_targetVelW = Pxldcs(frictions[0].rbXn_targetVelW[threadIndex]);
						next_error = next_raXn_extraCoeff.w;
						next_appliedForce = Pxldcs(frictions[0].appliedForce[threadIndex]);

						next_resp0 = ref0 * Pxldcs(frictions[0].resp0[threadIndex]);
						next_resp1 = ref1 * Pxldcs(frictions[0].resp1[threadIndex]);

						const float next_resp = next_resp0 + next_resp1;
						next_velMultiplier = (next_resp > 0.f) ? (p8 / next_resp) : 0.f;
					}

					const PxVec3 raXn = PxVec3(raXn_contactCoeff.x, raXn_contactCoeff.y, raXn_contactCoeff.z);
					const PxVec3 rbXn = PxVec3(rbXn_targetVelW.x, rbXn_targetVelW.y, rbXn_targetVelW.z);
					const float targetVel = rbXn_targetVelW.w;

					//Compute the normal velocity of the constraint.
					const PxReal v0 = angVel0.dot(raXn);
					const PxReal v1 = angVel1.dot(rbXn);
					const float normalVel = relVel1 + (v0 - v1);

					const float sep = PxMax(minPen, separation + deltaV + b0AngDelta.dot(raXn) - b1AngDelta.dot(rbXn));

					//How much the target vel should have changed the position of this constraint...
					const PxReal tVelErr = targetVel * elapsedTime;

					const PxReal biasedErr = recipResponse * fminf(-maxPenBias, biasCoefficient * (sep - tVelErr));

					//KS - clamp the maximum force
					const float tempDeltaF = biasedErr - (normalVel - targetVel) * velMultiplier;
					const float _deltaF = fmaxf(tempDeltaF, -appliedForce);//FMax(FNegScaleSub(normalVel, velMultiplier, biasedErr), FNeg(appliedForce));
					const float _newForce = appliedForce + _deltaF;
					const float newForce = fminf(_newForce, maxImpulse);//FMin(_newForce, maxImpulse);
					const float deltaF = newForce - appliedForce;

					accumDeltaF += deltaF;

					relVel1 += sumInvMass * deltaF;

					angVel0 += raXn * (deltaF * angDom0);
					angVel1 -= rbXn * (deltaF * angDom1);

					if(error)
						error->accumulateErrorLocal(deltaF, velMultiplier);

					Pxstcs(&c.appliedForce[threadIndex], newForce);

					accumulatedNormalImpulse = accumulatedNormalImpulse + newForce;
				}
			}

			linVel0 += delLinVel0 * accumDeltaF;
			linVel1 -= delLinVel1 * accumDeltaF;

			if (numFrictionConstr)
			{
				const float biasCoefficient = Pxldcg(frictionHeader->biasCoefficient[threadIndex]);

				const float dynamicFrictionCof = Pxldcg(frictionHeader->dynamicFriction[threadIndex]);
				const float maxFrictionImpulse = staticFrictionCof * accumulatedNormalImpulse;
				const float maxDynFrictionImpulse = dynamicFrictionCof * accumulatedNormalImpulse;

				PxU32 broken = 0;

				const float4 frictionNormal0 = Pxldcg(frictionHeader->frictionNormals[0][threadIndex]);
				const float4 frictionNormal1 = Pxldcg(frictionHeader->frictionNormals[1][threadIndex]);
				const PxVec3 normal0 = PxVec3(frictionNormal0.x, frictionNormal0.y, frictionNormal0.z);
				const PxVec3 normal1 = PxVec3(frictionNormal1.x, frictionNormal1.y, frictionNormal1.z);

				const PxVec3 delLinVel00 = normal0 * invMassA;
				const PxVec3 delLinVel10 = normal0 * invMassB;
				const PxVec3 delLinVel01 = normal1 * invMassA;
				const PxVec3 delLinVel11 = normal1 * invMassB;

				float relDelta0 = relMotion.dot(normal0);
				float relDelta1 = relMotion.dot(normal1);

				for (uint i = 0; i < numFrictionConstr; i += 2)
				{
					PxgTGSBlockSolverContactFriction& f0 = frictions[i];
					PxgTGSBlockSolverContactFriction& f1 = frictions[i + 1];

					const float4 raXn_extraCoeff1 = f1.raXn_error[threadIndex];
					const float4 rbXn_targetVelW1 = f1.rbXn_targetVelW[threadIndex];
					const float initialError1 = raXn_extraCoeff1.w;
					const float appliedForce1 = f1.appliedForce[threadIndex];
					const float targetVel1 = rbXn_targetVelW1.w;

					const float4 raXn_extraCoeff0 = next_raXn_extraCoeff;
					const float4 rbXn_targetVelW0 = next_rbXn_targetVelW;
					const float initialError0 = next_error;
					const float appliedForce0 = next_appliedForce;
					const float targetVel0 = rbXn_targetVelW0.w;

					const float f1_resp0 = ref0 * f1.resp0[threadIndex];
					const float f1_resp1 = ref1 * f1.resp1[threadIndex];
					const float f1_resp = f1_resp0 + f1_resp1;
					const float velMultiplier1 = (f1_resp > 0.f) ? (p8 / f1_resp) : 0.f;
					const float velMultiplier0 = next_velMultiplier;

					if ((i + 2) < numFrictionConstrTotal)
					{
						next_raXn_extraCoeff = Pxldcs(frictions[i + 2].raXn_error[threadIndex]);
						next_rbXn_targetVelW = Pxldcs(frictions[i + 2].rbXn_targetVelW[threadIndex]);
						next_error = next_raXn_extraCoeff.w;
						next_appliedForce = Pxldcs(frictions[i + 2].appliedForce[threadIndex]);

						next_resp0 = ref0 * Pxldcs(frictions[i + 2].resp0[threadIndex]);
						next_resp1 = ref1 * Pxldcs(frictions[i + 2].resp1[threadIndex]);

						const float next_resp = next_resp0 + next_resp1;
						next_velMultiplier = (next_resp > 0.f) ? (p8 / next_resp) : 0.f;
					}

					const PxVec3 raXn0 = PxVec3(raXn_extraCoeff0.x, raXn_extraCoeff0.y, raXn_extraCoeff0.z);
					const PxVec3 rbXn0 = PxVec3(rbXn_targetVelW0.x, rbXn_targetVelW0.y, rbXn_targetVelW0.z);
					const PxReal v00 = angVel0.dot(raXn0) + linVel0.dot(normal0);
					const PxReal v10 = angVel1.dot(rbXn0) + linVel1.dot(normal0);
					const float normalVel0 = v00 - v10;

					const float error0 = initialError0 - targetVel0 * elapsedTime +
						raXn0.dot(b0AngDelta) - rbXn0.dot(b1AngDelta) + relDelta0;

					const float bias0 = error0 * biasCoefficient;
					const float tmp10 = appliedForce0 - (bias0 - targetVel0) * velMultiplier0;
					const float totalImpulse0 = tmp10 - normalVel0 * velMultiplier0;

					const PxVec3 raXn1 = PxVec3(raXn_extraCoeff1.x, raXn_extraCoeff1.y, raXn_extraCoeff1.z);
					const PxVec3 rbXn1 = PxVec3(rbXn_targetVelW1.x, rbXn_targetVelW1.y, rbXn_targetVelW1.z);

					const PxReal v01 = angVel0.dot(raXn1) + linVel0.dot(normal1);
					const PxReal v11 = angVel1.dot(rbXn1) + linVel1.dot(normal1);
					const float normalVel1 = v01 - v11;

					const float error1 = initialError1 - targetVel1 * elapsedTime +
						raXn1.dot(b0AngDelta) - rbXn1.dot(b1AngDelta) + relDelta1;

					const float bias1 = error1 * biasCoefficient;
					const float tmp11 = appliedForce1 - (bias1 - targetVel1) * velMultiplier1;
					const float totalImpulse1 = tmp11 - normalVel1 * velMultiplier1;

					const float totalImpulse = PxSqrt(totalImpulse0 * totalImpulse0 + totalImpulse1 * totalImpulse1);

					const bool clamp = totalImpulse > maxFrictionImpulse;

					const float ratio = clamp ? fminf(maxDynFrictionImpulse, totalImpulse) / totalImpulse : 1.f;

					const float newAppliedForce0 = totalImpulse0 * ratio;
					const float newAppliedForce1 = totalImpulse1 * ratio;

					float deltaF0 = newAppliedForce0 - appliedForce0;
					float deltaF1 = newAppliedForce1 - appliedForce1;

					if (error)
						error->accumulateErrorLocal(deltaF0, deltaF1, velMultiplier0, velMultiplier1);

					linVel0 += delLinVel00 * deltaF0;
					linVel1 -= delLinVel10 * deltaF0;
					angVel0 += raXn0 * (deltaF0 * angDom0);
					angVel1 -= rbXn0 * (deltaF0 * angDom1);

					linVel0 += delLinVel01 * deltaF1;
					linVel1 -= delLinVel11 * deltaF1;
					angVel0 += raXn1 * (deltaF1 * angDom0);
					angVel1 -= rbXn1 * (deltaF1 * angDom1);

					Pxstcs(&f0.appliedForce[threadIndex], newAppliedForce0);
					Pxstcs(&f1.appliedForce[threadIndex], newAppliedForce1);
					broken = broken | clamp;
				}

				if (numFrictionConstr < numFrictionConstrTotal)
				{
					const PxReal frictionScale = frictionHeader->torsionalFrictionScale[threadIndex];
			
					//We have a torsional friction anchor, solve this...
					PxgTGSBlockSolverContactFriction& f0 = frictions[numFrictionConstr];
					const float4 raXn_extraCoeff0 = next_raXn_extraCoeff;
					const PxVec3 raXn0 = PxVec3(raXn_extraCoeff0.x, raXn_extraCoeff0.y, raXn_extraCoeff0.z);
					const float velMultiplier0 = next_velMultiplier;
					const float4 rbXn_targetVelW0 = next_rbXn_targetVelW;
					const float appliedForce0 = next_appliedForce;
					const float targetVel0 = rbXn_targetVelW0.w;

					const PxVec3 rbXn0 = PxVec3(rbXn_targetVelW0.x, rbXn_targetVelW0.y, rbXn_targetVelW0.z);

					const PxReal v00 = angVel0.dot(raXn0);
					const PxReal v10 = angVel1.dot(rbXn0);
					const float normalVel0 = v00 - v10;

					const float tmp10 = appliedForce0 - (-targetVel0) * velMultiplier0;
					const float totalImpulse = tmp10 - normalVel0 * velMultiplier0;

					const bool clamp = PxAbs(totalImpulse) > (maxFrictionImpulse * frictionScale);

					const PxReal totalClamped = PxClamp(totalImpulse, -maxDynFrictionImpulse * frictionScale, maxDynFrictionImpulse * frictionScale);

					const PxReal newAppliedForce = clamp ? totalClamped : totalImpulse;

					float deltaF = newAppliedForce - appliedForce0;
					if (error)
						error->accumulateErrorLocal(deltaF, velMultiplier0);

					angVel0 += raXn0 * (deltaF * angDom0);
					angVel1 -= rbXn0 * (deltaF * angDom1);

					Pxstcs(&f0.appliedForce[threadIndex], newAppliedForce);
					broken = broken | clamp;
				}

				Pxstcs(&frictionHeader->broken[threadIndex], broken);
			}
		}
	}

	// Write back
	b0LinVel = PxVec3(linVel0.x, linVel0.y, linVel0.z);
	b0AngVel = PxVec3(angVel0.x, angVel0.y, angVel0.z);
	b1LinVel = PxVec3(linVel1.x, linVel1.y, linVel1.z);
	b1AngVel = PxVec3(angVel1.x, angVel1.y, angVel1.z);
}

// A light version of the function "solveContactBlockTGS" to quickly check if there is any active contact.
// TODO: Make this even lighter.

static __device__ bool checkActiveContactBlockTGS(const PxgBlockConstraintBatch& batch, const PxVec3& linVel0, const PxVec3& angVel0,
	const PxVec3& linVel1, const PxVec3& angVel1,	const PxVec3& b0LinDelta, const PxVec3& b0AngDelta, const PxVec3& b1LinDelta,
	const PxVec3& b1AngDelta, const PxU32 threadIndex, const PxgTGSBlockSolverContactHeader* PX_RESTRICT contactHeaders,
	PxgTGSBlockSolverContactPoint* PX_RESTRICT contactPoints, const PxReal elapsedTime, const PxReal minPen)
{
	using namespace physx;

	{
		const PxgTGSBlockSolverContactHeader* PX_RESTRICT contactHeader = &contactHeaders[batch.mConstraintBatchIndex];
		const uint numNormalConstr = Pxldcs(contactHeader->numNormalConstr[threadIndex]);

		if (numNormalConstr)
		{
			const PxReal maxPenBias = Pxldcs(contactHeader->maxPenBias[threadIndex]);

			PxgTGSBlockSolverContactPoint* PX_RESTRICT contacts = &contactPoints[batch.startConstraintIndex];
			const float4 invMass0_1_angDom0_1 = Pxldcs(contactHeader->invMass0_1_angDom0_1[threadIndex]);

			const float invMassA = invMass0_1_angDom0_1.x;
			const float invMassB = invMass0_1_angDom0_1.y;

			const float4 normal_staticFriction = Pxldcg(contactHeader->normal_staticFriction[threadIndex]);
			const PxVec3 normal = PxVec3(normal_staticFriction.x, normal_staticFriction.y, normal_staticFriction.z);

			const float restitutionXdt = contactHeader->restitutionXdt[threadIndex];
			const PxU8 flags = contactHeader->flags[threadIndex];
			const PxVec3 delLinVel0 = normal * invMassA;
			const PxVec3 delLinVel1 = normal * invMassB;

			//Bring forward a read event
			const PxVec3 relMotion = b0LinDelta - b1LinDelta;
			const float deltaV = normal.dot(relMotion);
			float relVel1 = (linVel0 - linVel1).dot(normal);

			float4 next_raXn_extraCoeff = Pxldcs(contacts->raXn_extraCoeff[threadIndex]);
			float4 next_rbXn_targetVelW = Pxldcs(contacts->rbXn_targetVelW[threadIndex]);
			float next_appliedForce = Pxldcs(contacts->appliedForce[threadIndex]);
			float next_error = Pxldcs(contacts->separation[threadIndex]);
			float next_maxImpulse = Pxldcs(contacts->maxImpulse[threadIndex]);
			float next_biasCoefficient = Pxldcs(contacts->biasCoefficient[threadIndex]);

			float next_resp0 = Pxldcs(contacts->resp0[threadIndex]);
			float next_resp1 = Pxldcs(contacts->resp1[threadIndex]);

			float next_unitResponse = next_resp0 + next_resp1;
			float next_recipResponse = (next_unitResponse > 0.f) ? (1.f / next_unitResponse) : 0.f;
			float next_velMultiplier = next_recipResponse;

			if (restitutionXdt < 0.f)
			{
				computeCompliantContactCoefficientsTGS(flags, restitutionXdt, next_unitResponse,
					next_recipResponse, next_raXn_extraCoeff.w, next_velMultiplier,
					next_biasCoefficient);
			}

			{
				for (uint i = 0; i < numNormalConstr; i++)
				{
					const float4 raXn_contactCoeff = next_raXn_extraCoeff;
					const float velMultiplier = next_velMultiplier;
					const float4 rbXn_targetVelW = next_rbXn_targetVelW;
					const float appliedForce = next_appliedForce;
					const float separation = next_error;
					const float maxImpulse = next_maxImpulse;
					const float biasCoefficient = next_biasCoefficient;
					const float recipResponse = next_recipResponse;

					if ((i + 1) < numNormalConstr)
					{
						const PxgTGSBlockSolverContactPoint& nextC = contacts[i + 1];
						next_raXn_extraCoeff = Pxldcs(nextC.raXn_extraCoeff[threadIndex]);
						next_rbXn_targetVelW = Pxldcs(nextC.rbXn_targetVelW[threadIndex]);
						next_appliedForce = Pxldcs(nextC.appliedForce[threadIndex]);
						next_error = Pxldcs(nextC.separation[threadIndex]);
						next_maxImpulse = Pxldcs(nextC.maxImpulse[threadIndex]);
						next_biasCoefficient = Pxldcs(nextC.biasCoefficient[threadIndex]);

						next_resp0 = Pxldcs(nextC.resp0[threadIndex]);
						next_resp1 = Pxldcs(nextC.resp1[threadIndex]);

						next_unitResponse = next_resp0 + next_resp1;
						next_recipResponse = (next_unitResponse > 0.f) ? (1.f / next_unitResponse) : 0.f;

						next_velMultiplier = next_recipResponse;

						if (restitutionXdt < 0.f)
						{
							computeCompliantContactCoefficientsTGS(flags, restitutionXdt, next_unitResponse,
								next_recipResponse, next_raXn_extraCoeff.w, next_velMultiplier,
								next_biasCoefficient);
						}
					}

					const PxVec3 raXn = PxVec3(raXn_contactCoeff.x, raXn_contactCoeff.y, raXn_contactCoeff.z);
					const PxVec3 rbXn = PxVec3(rbXn_targetVelW.x, rbXn_targetVelW.y, rbXn_targetVelW.z);
					const float targetVel = rbXn_targetVelW.w;

					//Compute the normal velocity of the constraint.
					const PxReal v0 = angVel0.dot(raXn);
					const PxReal v1 = angVel1.dot(rbXn);
					const float normalVel = relVel1 + (v0 - v1);

					const float sep = PxMax(minPen, separation + deltaV + b0AngDelta.dot(raXn) - b1AngDelta.dot(rbXn));

					//How much the target vel should have changed the position of this constraint...
					const PxReal tVelErr = targetVel * elapsedTime;
					const PxReal biasedErr = recipResponse * fminf(-maxPenBias, biasCoefficient * (sep - tVelErr));

					//KS - clamp the maximum force
					const float tempDeltaF = biasedErr - (normalVel - targetVel) * velMultiplier;
					const float _deltaF = fmaxf(tempDeltaF, -appliedForce);//FMax(FNegScaleSub(normalVel, velMultiplier, biasedErr), FNeg(appliedForce));
					const float _newForce = appliedForce + _deltaF;
					const float newForce = fminf(_newForce, maxImpulse);//FMin(_newForce, maxImpulse);
					const float deltaF = newForce - appliedForce;
					
					// active contact
					if (PxAbs(deltaF) > 1.0e-8f)
					{
						return true;
					}
				}
			}
		}
	}

	return false;
}

static __device__ void concludeContactBlockTGS(const PxgBlockConstraintBatch& batch, const PxU32 threadIndex, PxgTGSBlockSolverContactHeader* contactHeaders, PxgTGSBlockSolverFrictionHeader* frictionHeaders,
	PxgTGSBlockSolverContactPoint* contactPoints, PxgTGSBlockSolverContactFriction* frictions)
{

#if 0
	using namespace physx;

	{
		const PxgTGSBlockSolverContactHeader* contactHeader = &contactHeaders[batch.mConstraintBatchIndex];
		PxgTGSBlockSolverFrictionHeader* frictionHeader = &frictionHeaders[batch.mConstraintBatchIndex];

		const uint32_t numNormalConstr = contactHeader->numNormalConstr[threadIndex];
		//const uint32_t numFrictionConstr = frictionHeader->numFrictionConstr[threadIndex];

		PxgTGSBlockSolverContactPoint* contacts = &contactPoints[batch.startConstraintIndex];
		for(uint32_t i=0;i<numNormalConstr;i++)
		{
			//contactPoints[i].setScaledBias(fmaxf(contactPoints[i].getScaledBias(), 0.f));
			contacts[i].biasCoefficeint[threadIndex] = 0.f;
		}

		frictionHeader->biasCoefficient[threadIndex] = 0.f;

		//PxgTGSBlockSolverContactFriction* frictionConstr = &frictions[batch.startFrictionIndex];
		//for(uint32_t i=0;i<numFrictionConstr;i++)
		//{
		//	//frictionConstr[i].setBias(0.f);
		//	frictionConstr[i].bias[threadIndex] = 0.f;
		//}
	}
#endif
}



static __device__ void writeBackContactBlockTGS(const PxgBlockConstraintBatch& batch, const PxU32 threadIndex,
											 const PxgSolverBodyData* bodies, Dy::ThresholdStreamElement* thresholdStream,
											 PxI32* sharedThresholdStreamIndex, PxgTGSBlockSolverContactHeader* contactHeaders, PxgTGSBlockSolverFrictionHeader* frictionHeaders,
											PxgTGSBlockSolverContactPoint* contactPoints, PxgTGSBlockSolverContactFriction* frictions,
											PxF32* forcewritebackBuffer, PxgBlockFrictionPatch& frictionPatchBlock,
											PxgFrictionPatchGPU* frictionPatches)
{
	const PxU32 bodyAIndex = batch.bodyAIndex[threadIndex];
	const PxU32 bodyBIndex = batch.bodyBIndex[threadIndex];

	const PxgSolverBodyData& bd0 = bodies[bodyAIndex];
	const PxgSolverBodyData& bd1 = bodies[bodyBIndex];
	bool forceThreshold = false;

	float normalForce = 0.f;

	{
		const PxgTGSBlockSolverContactHeader* PX_RESTRICT contactHeader = &contactHeaders[batch.mConstraintBatchIndex];
		const PxgTGSBlockSolverFrictionHeader* PX_RESTRICT frictionHeader = &frictionHeaders[batch.mConstraintBatchIndex];
	
		PxU32 forceWritebackOffset = contactHeader->forceWritebackOffset[threadIndex];

		forceThreshold = contactHeader->flags[threadIndex] & PxgSolverContactFlags::eHAS_FORCE_THRESHOLDS;

		const PxU32	numFrictionConstr = frictionHeader->numFrictionConstr[threadIndex];

		const PxU32 numNormalConstr = contactHeader->numNormalConstr[threadIndex];
		if(forceWritebackOffset!=0xFFFFFFFF)
		{
			PxReal* vForceWriteback = &forcewritebackBuffer[forceWritebackOffset];
			PxgTGSBlockSolverContactPoint* c = &contactPoints[batch.startConstraintIndex];
			for(PxU32 i=0; i<numNormalConstr; i++)
			{
				const PxReal appliedForce = c[i].appliedForce[threadIndex];//FStore(c->getAppliedForce());
				*vForceWriteback++ = appliedForce;
				normalForce += appliedForce;
			}
		}

		writeBackContactBlockFriction(threadIndex, numFrictionConstr, frictionHeader,
			frictionPatchBlock, frictions + batch.startFrictionIndex, frictionPatches);

		if(numFrictionConstr && frictionHeader->broken[threadIndex])
		{
			frictionPatchBlock.broken[threadIndex] = 1;
		}
	}

	float reportThreshold0 = bd0.reportThreshold;
	float reportThreshold1 = bd1.reportThreshold;

	if((forceThreshold && normalForce !=0 && (reportThreshold0 < PX_MAX_REAL  || reportThreshold1 < PX_MAX_REAL)))
	{
		//ToDo : support PxgThresholdStreamElement
		Dy::ThresholdStreamElement elt;
		elt.normalForce = normalForce;
		elt.threshold = PxMin<float>(reportThreshold0, reportThreshold1);
		
		elt.nodeIndexA = bd0.islandNodeIndex;
		elt.nodeIndexB = bd1.islandNodeIndex;
		elt.shapeInteraction = batch.shapeInteraction[threadIndex];
		PxOrder(elt.nodeIndexA, elt.nodeIndexB);
		assert(elt.nodeIndexA < elt.nodeIndexB);

		PxI32 index = atomicAdd(sharedThresholdStreamIndex, 1);

		//KS - force a 16-byte coalesced write
		//((float4*)thresholdStream)[index] = *((float4*)&elt);
		thresholdStream[index] = elt;
	}
}

// Using the same logic as the previous implementation, but mass-splitting is additionally performed per sub-timestep.
// Required data is packed and stored differently from the previous implementation to split mass at each sub-timestep; 
// i.e., mass-related terms are computed at every sub-timestep.
// Refer to "Mass Splitting for Jitter-Free Parallel Rigid Body Simulation" for the general mass-splitting idea.


static __device__ void solve1DBlockTGS(const PxgBlockConstraintBatch& batch, PxVec3& b0LinVel, PxVec3& b0AngVel, PxVec3& b1LinVel, PxVec3& b1AngVel,
	const PxVec3& linDelta0, const PxVec3& angDelta0, const PxVec3& linDelta1, const PxVec3& angDelta1, const PxU32 threadIndex,
	const PxgTGSBlockSolverConstraint1DHeader* PX_RESTRICT headers, PxgTGSBlockSolverConstraint1DCon* PX_RESTRICT rowsCon,
	const PxgSolverTxIData& iData0, const PxgSolverTxIData& iData1, const PxReal elapsedTime, bool residualReportingEnabled,
	PxReal ref0 = 1.f, PxReal ref1 = 1.f)
{
	using namespace physx;

	//
	// please refer to solve1DStep() in DyTGSContactPrep.cpp for a description of the parameters and some of the logic
	//

	const PxgTGSBlockSolverConstraint1DHeader* PX_RESTRICT  header = &headers[batch.mConstraintBatchIndex];
	PxgTGSBlockSolverConstraint1DCon* PX_RESTRICT baseCon = &rowsCon[batch.startConstraintIndex];

	PxVec3 linVel0(b0LinVel.x, b0LinVel.y, b0LinVel.z);
	PxVec3 linVel1(b1LinVel.x, b1LinVel.y, b1LinVel.z);
	PxVec3 angVel0(b0AngVel.x, b0AngVel.y, b0AngVel.z);
	PxVec3 angVel1(b1AngVel.x, b1AngVel.y, b1AngVel.z);

	const float4 raInvMass0 = header->rAWorld_invMass0D0[threadIndex];
	const float4 rbInvMass1 = header->rBWorld_invMass1D1[threadIndex];

	float invMass0 = ref0 * raInvMass0.w; //FLoad(header->invMass0D0);
	float invMass1 = ref1 * rbInvMass1.w; //FLoad(header->invMass1D1);

	const PxVec3 raPrev(raInvMass0.x, raInvMass0.y, raInvMass0.z);
	const PxVec3 rbPrev(rbInvMass1.x, rbInvMass1.y, rbInvMass1.z);

	const PxVec3 ra = iData0.deltaBody2World.q.rotate(raPrev);
	const PxVec3 rb = iData1.deltaBody2World.q.rotate(rbPrev);

	const PxVec3 raMotion = (ra + linDelta0) - raPrev;
	const PxVec3 rbMotion = (rb + linDelta1) - rbPrev;

	float invInertiaScale0 = ref0 * header->invInertiaScale0[threadIndex];
	float invInertiaScale1 = ref1 * header->invInertiaScale1[threadIndex];


	const uchar4 rowCounts_breakable_orthoAxisCount = header->rowCounts_breakable_orthoAxisCount[threadIndex];

	const PxU32 rowCount = rowCounts_breakable_orthoAxisCount.x;

	const float4 ang0Ortho0_recipResponseW = header->angOrthoAxis0_recipResponseW[0][threadIndex];
	const float4 ang0Ortho1_recipResponseW = header->angOrthoAxis0_recipResponseW[1][threadIndex];
	const float4 ang0Ortho2_recipResponseW = header->angOrthoAxis0_recipResponseW[2][threadIndex];

	const float4 ang1OrthoAxis0_ErrorW = header->angOrthoAxis1_ErrorW[0][threadIndex];
	const float4 ang1OrthoAxis1_ErrorW = header->angOrthoAxis1_ErrorW[1][threadIndex];
	const float4 ang1OrthoAxis2_ErrorW = header->angOrthoAxis1_ErrorW[2][threadIndex];

	const float recipResponse0 = ang0Ortho0_recipResponseW.w;
	const float recipResponse1 = ang0Ortho1_recipResponseW.w;
	const float recipResponse2 = ang0Ortho2_recipResponseW.w;

	const PxVec3 ang0Ortho0(ang0Ortho0_recipResponseW.x, ang0Ortho0_recipResponseW.y, ang0Ortho0_recipResponseW.z);
	const PxVec3 ang0Ortho1(ang0Ortho1_recipResponseW.x, ang0Ortho1_recipResponseW.y, ang0Ortho1_recipResponseW.z);
	const PxVec3 ang0Ortho2(ang0Ortho2_recipResponseW.x, ang0Ortho2_recipResponseW.y, ang0Ortho2_recipResponseW.z);

	const PxVec3 ang1Ortho0(ang1OrthoAxis0_ErrorW.x, ang1OrthoAxis0_ErrorW.y, ang1OrthoAxis0_ErrorW.z);
	const PxVec3 ang1Ortho1(ang1OrthoAxis1_ErrorW.x, ang1OrthoAxis1_ErrorW.y, ang1OrthoAxis1_ErrorW.z);
	const PxVec3 ang1Ortho2(ang1OrthoAxis2_ErrorW.x, ang1OrthoAxis2_ErrorW.y, ang1OrthoAxis2_ErrorW.z);

	PxReal error0 = ang1OrthoAxis0_ErrorW.w + (ang0Ortho0.dot(angDelta0) - ang1Ortho0.dot(angDelta1));
	PxReal error1 = ang1OrthoAxis1_ErrorW.w + (ang0Ortho1.dot(angDelta0) - ang1Ortho1.dot(angDelta1));
	PxReal error2 = ang1OrthoAxis2_ErrorW.w + (ang0Ortho2.dot(angDelta0) - ang1Ortho2.dot(angDelta1));

	for (PxU32 i = 0; i < rowCount; ++i)
	{
		PxgTGSBlockSolverConstraint1DCon& ccon = baseCon[i];

		const float4 _clinVel0_coeff0W = ccon.lin0XYZ_initBiasOrCoeff0[threadIndex];
		const float4 _clinVel1_coeff1W = ccon.lin1XYZ_biasScaleOrCoeff1[threadIndex];
		const float4 _cangVel0_coeff2W = ccon.ang0XYZ_velMultiplierOrCoeff2[threadIndex];
		const float4 _cangVel1_coeff3W = ccon.ang1XYZ_velTargetOrCoeff3[threadIndex];

		const PxVec3 clinVel0(_clinVel0_coeff0W.x, _clinVel0_coeff0W.y, _clinVel0_coeff0W.z);
		const PxVec3 clinVel1(_clinVel1_coeff1W.x, _clinVel1_coeff1W.y, _clinVel1_coeff1W.z);
		const PxVec3 cangVel0_(_cangVel0_coeff2W.x, _cangVel0_coeff2W.y, _cangVel0_coeff2W.z);
		const PxVec3 cangVel1_(_cangVel1_coeff3W.x, _cangVel1_coeff3W.y, _cangVel1_coeff3W.z);

		PxReal initBias = _clinVel0_coeff0W.w;
		const PxReal biasScale = _clinVel1_coeff1W.w;
		const PxReal velMultiplier = _cangVel0_coeff2W.w;
		const PxReal targetVel = _cangVel1_coeff3W.w;

		const PxVec3 cangVel0 = cangVel0_ + ra.cross(clinVel0);
		const PxVec3 cangVel1 = cangVel1_ + rb.cross(clinVel1);

		const PxU32 flags = ccon.flags[threadIndex];

		const PxReal maxBias = ccon.maxBias[threadIndex];

		const PxReal minBias = Dy::computeMinBiasTGS(flags, maxBias);

		PxVec3 raXnI = iData0.sqrtInvInertia * cangVel0;
		PxVec3 rbXnI = iData1.sqrtInvInertia * cangVel1;

		//KS - TODO - orthogonalization here...
		if (flags & DY_SC_FLAG_ORTHO_TARGET)
		{
			const PxReal proj0 = (raXnI.dot(ang0Ortho0) +
				rbXnI.dot(ang1Ortho0)) * recipResponse0;

			const PxReal proj1 = (raXnI.dot(ang0Ortho1) +
				rbXnI.dot(ang1Ortho1)) * recipResponse1;

			const PxReal proj2 = (raXnI.dot(ang0Ortho2) +
				rbXnI.dot(ang1Ortho2)) * recipResponse2;

			const PxVec3 delta0 = ang0Ortho0 * proj0 + ang0Ortho1 * proj1 + ang0Ortho2 * proj2;
			const PxVec3 delta1 = ang1Ortho0 * proj0 + ang1Ortho1 * proj1 + ang1Ortho2 * proj2;

			raXnI = raXnI - delta0;
			rbXnI = rbXnI - delta1;

			const PxReal orthoBasisError = biasScale * (error0 * proj0 + error1 * proj1 + error2 * proj2);
			initBias = initBias - orthoBasisError;
		}

		// If biasScale is zero, initBias must also be zero.
		// biasScale is enforced to zero before the velocity itertaion (e.g., in conclude1DBlockTGS), but not initBias.
		// Thus, explicitly resetting it to zero here for velocity iteration.
		initBias = (biasScale == 0.f) ? 0.f : initBias;

		const bool isSpringConstraint = (flags & DY_SC_FLAG_SPRING);

		const PxReal errorDelta = Dy::computeResolvedGeometricErrorTGS(raMotion, rbMotion, clinVel0, clinVel1,
			angDelta0, angDelta1, raXnI, rbXnI,
			ccon.angularErrorScale[threadIndex],
			isSpringConstraint, targetVel, elapsedTime);

		const PxReal resp0 = invMass0 * clinVel0.dot(clinVel0) + raXnI.dot(raXnI) * invInertiaScale0;
		const PxReal resp1 = invMass1 * clinVel1.dot(clinVel1) + rbXnI.dot(rbXnI) * invInertiaScale1;

		//KS - may be a - required here...
		const PxReal response = resp0 + resp1;

		const PxReal recipResponse = response > 0.f ? 1.f / response : 0.f;

		const PxReal vMul = isSpringConstraint ? velMultiplier : recipResponse * velMultiplier;

		const PxReal unclampedBias = initBias + errorDelta * biasScale;

		const PxReal bias = PxClamp(unclampedBias, minBias, maxBias);

		const PxReal constant = isSpringConstraint ? bias + targetVel : recipResponse * (bias + targetVel);

		const float appliedForce = ccon.appliedForce[threadIndex];//FLoad(c.appliedForce);
																  //const FloatV targetVel = FLoad(c.targetVelocity);

		const float maxImpulse = ccon.maxImpulse[threadIndex];//FLoad(c.maxImpulse);
		const float minImpulse = ccon.minImpulse[threadIndex];//FLoad(c.minImpulse);

														//const PxVec3 v0 = linVel0.multiply(clinVel0) + angVel0.multiply(cangVel0);//V3MulAdd(linVel0, clinVel0, V3Mul(angVel0, cangVel0));
														//const PxVec3 v1 = linVel1.multiply(clinVel1) + angVel1.multiply(cangVel1);//V3MulAdd(linVel1, clinVel1, V3Mul(angVel1, cangVel1));

		const float v0 = linVel0.dot(clinVel0) + angVel0.dot(raXnI);//V3MulAdd(linVel0, clinVel0, V3Mul(angVel0, cangVel0));
		const float v1 = linVel1.dot(clinVel1) + angVel1.dot(rbXnI);//V3MulAdd(linVel1, clinVel1, V3Mul(angVel1, cangVel1));

		const float normalVel = v0 - v1;

		const float unclampedForce = appliedForce + (vMul * normalVel + constant);//FMulAdd(iMul, appliedForce, FMulAdd(vMul, normalVel, constant));
		const float clampedForce = fminf(maxImpulse, fmaxf(minImpulse, unclampedForce));//FMin(maxImpulse, (FMax(minImpulse, unclampedForce)));
		const float deltaF = clampedForce - appliedForce;//FSub(clampedForce, appliedForce);

		ccon.appliedForce[threadIndex] = clampedForce; // FStore(clampedForce);
		if(residualReportingEnabled)
			ccon.residual[threadIndex] = PxgErrorAccumulator::calculateResidual(deltaF, vMul);

		linVel0 = linVel0 + clinVel0 * (deltaF * invMass0);//V3ScaleAdd(clinVel0, FMul(deltaF, invMass0), linVel0);			
		linVel1 = linVel1 - clinVel1 * (deltaF * invMass1);//V3NegScaleSub(clinVel1, FMul(deltaF, invMass1), linVel1);
		angVel0 = angVel0 + raXnI * deltaF * invInertiaScale0;//V3ScaleAdd(cangVel0, deltaF, angVel0);
		angVel1 = angVel1 - rbXnI * deltaF * invInertiaScale1;//V3NegScaleSub(cangVel1, deltaF, angVel1);

	}


	b0LinVel = PxVec3(linVel0.x, linVel0.y, linVel0.z);
	b0AngVel = PxVec3(angVel0.x, angVel0.y, angVel0.z);
	b1LinVel = PxVec3(linVel1.x, linVel1.y, linVel1.z);
	b1AngVel = PxVec3(angVel1.x, angVel1.y, angVel1.z);
}

// Using the same logic as the previous implementation, but mass-splitting is additionally performed per sub-timestep.
// Required data is packed and stored differently from the previous implementation to split mass at each sub-timestep; 
// i.e., mass-related terms are computed at every sub-timestep.

static __device__ PX_FORCE_INLINE void solveExt1DBlockTGS(const PxgBlockConstraintBatch& batch,
	Cm::UnAlignedSpatialVector& vel0,
	Cm::UnAlignedSpatialVector& vel1,
	const Cm::UnAlignedSpatialVector& motion0,
	const Cm::UnAlignedSpatialVector& motion1,
	const PxU32 threadIndex,
	const PxgTGSBlockSolverConstraint1DHeader* PX_RESTRICT headers,
	PxgTGSBlockSolverConstraint1DCon* PX_RESTRICT rowsCon,
	PxgArticulationBlockResponse* PX_RESTRICT artiResponse,
	const PxQuat& deltaQ0, const PxQuat& deltaQ1,
	const PxReal elapsedTime,
	Cm::UnAlignedSpatialVector& impluse0,
	Cm::UnAlignedSpatialVector& impluse1,
	bool residualReportingEnabled, 
	PxReal ref0 = 1.f, PxReal ref1 = 1.f)
{
	using namespace physx;

	const PxgTGSBlockSolverConstraint1DHeader* PX_RESTRICT  header = &headers[batch.mConstraintBatchIndex];
	PxgTGSBlockSolverConstraint1DCon* PX_RESTRICT baseCon = &rowsCon[batch.startConstraintIndex];
	const PxgArticulationBlockResponse* PX_RESTRICT responses = &artiResponse[batch.mArticulationResponseIndex];

	PxVec3 linVel0 = vel0.bottom;
	PxVec3 linVel1 = vel1.bottom;
	PxVec3 angVel0 = vel0.top;
	PxVec3 angVel1 = vel1.top;

	const float4 raInvMass0 = header->rAWorld_invMass0D0[threadIndex];
	const float4 rbInvMass1 = header->rBWorld_invMass1D1[threadIndex];

	float invMass0 = ref0 * raInvMass0.w; //FLoad(header->invMass0D0);
	float invMass1 = ref1 * rbInvMass1.w; //FLoad(header->invMass1D1);

	float invInertiaScale0 = ref0 * header->invInertiaScale0[threadIndex];
	float invInertiaScale1 = ref1 * header->invInertiaScale1[threadIndex];

	const PxVec3 raPrev(raInvMass0.x, raInvMass0.y, raInvMass0.z);
	const PxVec3 rbPrev(rbInvMass1.x, rbInvMass1.y, rbInvMass1.z);

	const PxVec3 ra = deltaQ0.rotate(raPrev);
	const PxVec3 rb = deltaQ1.rotate(rbPrev);

	const PxVec3 raMotion = (ra + motion0.bottom) - raPrev;
	const PxVec3 rbMotion = (rb + motion1.bottom) - rbPrev;

	const uchar4 rowCounts_breakable_orthoAxisCount = header->rowCounts_breakable_orthoAxisCount[threadIndex];

	const PxU32 rowCount = rowCounts_breakable_orthoAxisCount.x;

	const PxReal cfm = header->cfm[threadIndex];

	PxVec3 li0(0.f, 0.f, 0.f);
	PxVec3 li1(0.f, 0.f, 0.f);
	PxVec3 ai0(0.f, 0.f, 0.f);
	PxVec3 ai1(0.f, 0.f, 0.f);

	for (PxU32 i = 0; i < rowCount; ++i)
	{
		PxgTGSBlockSolverConstraint1DCon& ccon = baseCon[i];
		const PxgArticulationBlockResponse& response = responses[i];

		const float4 _clinVel0_coeff0W = ccon.lin0XYZ_initBiasOrCoeff0[threadIndex];//V3LoadA(c.lin0);
		const float4 _clinVel1_coeff1W = ccon.lin1XYZ_biasScaleOrCoeff1[threadIndex];//V3LoadA(c.lin1);
		const float4 _cangVel0_coeff2W = ccon.ang0XYZ_velMultiplierOrCoeff2[threadIndex];//V3LoadA(c.ang0);
		const float4 _cangVel1_coeff3W = ccon.ang1XYZ_velTargetOrCoeff3[threadIndex];//V3LoadA(c.ang1);

		const PxVec3 clinVel0(_clinVel0_coeff0W.x, _clinVel0_coeff0W.y, _clinVel0_coeff0W.z);
		const PxVec3 clinVel1(_clinVel1_coeff1W.x, _clinVel1_coeff1W.y, _clinVel1_coeff1W.z);
		const PxVec3 cangVel0(_cangVel0_coeff2W.x, _cangVel0_coeff2W.y, _cangVel0_coeff2W.z);
		const PxVec3 cangVel1(_cangVel1_coeff3W.x, _cangVel1_coeff3W.y, _cangVel1_coeff3W.z);

		const PxU32 flags = ccon.flags[threadIndex];
		const bool isSpringConstraint = (flags & DY_SC_FLAG_SPRING);

		const PxReal resp0 = ref0 * ccon.resp0[threadIndex];
		const PxReal resp1 = ref1 * ccon.resp1[threadIndex];
		
		float unitResponse = resp0 + resp1;
		unitResponse += cfm;

		//https://omniverse-jirasw.nvidia.com/browse/PX-4383
		const PxReal minRowResponse = DY_ARTICULATION_MIN_RESPONSE;
		const PxReal recipResponse = Dy::computeRecipUnitResponse(unitResponse, minRowResponse);

		const bool isAccelerationSpring = (flags & DY_SC_FLAG_ACCELERATION_SPRING);

		const PxReal coeff0 = _clinVel0_coeff0W.w;
		const PxReal coeff1 = _clinVel1_coeff1W.w;
		const PxReal coeff2 = _cangVel0_coeff2W.w;
		const PxReal coeff3 = _cangVel1_coeff3W.w;
		const PxReal geometricError = ccon.geometricError[threadIndex];

		PxReal biasScale, velMultiplier, initBias, targetVel;
		Dy::compute1dConstraintSolverConstantsTGS(isSpringConstraint, isAccelerationSpring, geometricError,
			unitResponse, recipResponse, coeff0, coeff1, coeff2,
			coeff3, biasScale, velMultiplier, initBias, targetVel);

		// If biasScale is zero, initBias must also be zero.
		// biasScale is enforced to zero before the velocity itertaion (e.g., in conclude1DBlockTGS), but not initBias.
		// Thus, explicitly resetting it to zero here for velocity iteration.
		initBias = (biasScale == 0.f) ? 0.f : initBias;

		const PxReal errorDelta = Dy::computeResolvedGeometricErrorTGS(raMotion, rbMotion, clinVel0, clinVel1,
			motion0.top, motion1.top, cangVel0, cangVel1,
			ccon.angularErrorScale[threadIndex],
			isSpringConstraint, targetVel, elapsedTime);


		const PxReal maxBias = ccon.maxBias[threadIndex];

		const PxReal vMul = isSpringConstraint ? velMultiplier : recipResponse * velMultiplier;
		const float appliedForce = ccon.appliedForce[threadIndex];

		const PxReal unclampedBias = initBias + errorDelta * biasScale;
		const PxReal minBias = Dy::computeMinBiasTGS(flags, maxBias);
		const PxReal bias = PxClamp(unclampedBias, minBias, maxBias);

		const PxReal constant = isSpringConstraint ? (bias + targetVel) : recipResponse * (bias + targetVel);

		const float maxImpulse = ccon.maxImpulse[threadIndex];//FLoad(c.maxImpulse);
		const float minImpulse = ccon.minImpulse[threadIndex];//FLoad(c.minImpulse);

		const float v0 = linVel0.dot(clinVel0) + angVel0.dot(cangVel0);//V3MulAdd(linVel0, clinVel0, V3Mul(angVel0, cangVel0));
		const float v1 = linVel1.dot(clinVel1) + angVel1.dot(cangVel1);//V3MulAdd(linVel1, clinVel1, V3Mul(angVel1, cangVel1));

		const float normalVel = v0 - v1;

		const float unclampedForce = appliedForce + (vMul * normalVel + constant);//FMulAdd(iMul, appliedForce, FMulAdd(vMul, normalVel, constant));
		const float clampedForce = fminf(maxImpulse, fmaxf(minImpulse, unclampedForce));//FMin(maxImpulse, (FMax(minImpulse, unclampedForce)));
		const float deltaF = clampedForce - appliedForce;//FSub(clampedForce, appliedForce);

		ccon.appliedForce[threadIndex] = clampedForce; // FStore(clampedForce);
		if(residualReportingEnabled)
			ccon.residual[threadIndex] = PxgErrorAccumulator::calculateResidual(deltaF, vMul);

		li0 = clinVel0 * deltaF + li0;
		ai0 = cangVel0 * deltaF + ai0;
		li1 = clinVel1 * deltaF + li1;
		ai1 = cangVel1 * deltaF + ai1;

		PxVec3 linVa = ref0 * PxVec3(response.deltaRALin_x[threadIndex], response.deltaRALin_y[threadIndex], response.deltaRALin_z[threadIndex]);
		PxVec3 angVa = ref0 * PxVec3(response.deltaRAAng_x[threadIndex], response.deltaRAAng_y[threadIndex], response.deltaRAAng_z[threadIndex]);
		PxVec3 linVb = ref1 * PxVec3(response.deltaRBLin_x[threadIndex], response.deltaRBLin_y[threadIndex], response.deltaRBLin_z[threadIndex]);
		PxVec3 angVb = ref1 * PxVec3(response.deltaRBAng_x[threadIndex], response.deltaRBAng_y[threadIndex], response.deltaRBAng_z[threadIndex]);

		linVel0 = linVa * deltaF + linVel0;
		angVel0 = angVa * deltaF + angVel0;

		linVel1 = linVb * deltaF + linVel1;
		angVel1 = angVb * deltaF + angVel1;
	}

	vel0.top = angVel0; vel0.bottom = linVel0;
	vel1.top = angVel1; vel1.bottom = linVel1;

	impluse0.top = li0 * invMass0; impluse0.bottom = ai0 * invInertiaScale0;
	impluse1.top = li1 * invMass1; impluse1.bottom = ai1 * invInertiaScale1;
}

static __device__ void conclude1DBlockTGS(const PxgBlockConstraintBatch& batch, const PxU32 threadIndex, const PxgTGSBlockSolverConstraint1DHeader* PX_RESTRICT headers, PxgTGSBlockSolverConstraint1DCon* PX_RESTRICT rows)
{
	using namespace physx;

	const PxgTGSBlockSolverConstraint1DHeader* PX_RESTRICT  header = &headers[batch.mConstraintBatchIndex];
	PxgTGSBlockSolverConstraint1DCon* PX_RESTRICT base = &rows[batch.startConstraintIndex];

	const uchar4 rowCount = header->rowCounts_breakable_orthoAxisCount[threadIndex];

	for (PxU32 i = 0; i<rowCount.x; i++)
	{
		PxgTGSBlockSolverConstraint1DCon& c = base[i];

		// Previous data, biasScale, initBias, velTarget, and velMultiplier are repacked using other coefficients.
		// See "PxgTGSBlockSolverConstraint1DCon" for details.

		if(!(c.flags[threadIndex] & DY_SC_FLAG_KEEP_BIAS))
		{
			c.lin1XYZ_biasScaleOrCoeff1[threadIndex].w = 0.f; // setting biasScale to zero. Make sure to enforce
															  // initialBias to be zero, when biasScale is zero.
		}
		if(c.flags[threadIndex] & DY_SC_FLAG_SPRING)
		{
			// see CPU version for an explanation
			c.lin0XYZ_initBiasOrCoeff0[threadIndex].w = 0.f;
			c.lin1XYZ_biasScaleOrCoeff1[threadIndex].w = 0.f;

			c.ang0XYZ_velMultiplierOrCoeff2[threadIndex].w = 0.0f;
			c.ang1XYZ_velTargetOrCoeff3[threadIndex].w = 0.0f;
		}
	}
}


static __device__ void writeBack1DBlockTGS(const PxgBlockConstraintBatch& batch, const PxU32 threadIndex, const PxgTGSBlockSolverConstraint1DHeader* PX_RESTRICT headers,
	PxgTGSBlockSolverConstraint1DCon* PX_RESTRICT rowsCon, PxgConstraintWriteback* constraintWriteBacks)
{
	const PxgTGSBlockSolverConstraint1DHeader* PX_RESTRICT  header = &headers[batch.mConstraintBatchIndex];
	PxgTGSBlockSolverConstraint1DCon* conBase = &rowsCon[batch.startConstraintIndex];

	PxU32 forceWritebackOffset = header->writeBackOffset[threadIndex];

	const uchar4 rowCounts_breakable_orthoAxisCount = header->rowCounts_breakable_orthoAxisCount[threadIndex];

	const PxU8 breakable = rowCounts_breakable_orthoAxisCount.y;

	const PxU32 numRows = rowCounts_breakable_orthoAxisCount.x;

	if (forceWritebackOffset != 0xFFFFFFFF)
	{
		PxgConstraintWriteback& writeback = constraintWriteBacks[forceWritebackOffset];

		PxVec3 linVel(0), angVel(0);
		PxReal constraintErrorSq = 0.0f;
		for (PxU32 i = 0; i < numRows; ++i)
		{
			PxgTGSBlockSolverConstraint1DCon& con = conBase[i];
		
			if (con.flags[threadIndex] & DY_SC_FLAG_OUTPUT_FORCE)
			{
				const float4 lin0XYZ_ErrorW = con.lin0XYZ_initBiasOrCoeff0[threadIndex];
				const float4 ang0WriteBack_VMW = con.ang0XYZ_velMultiplierOrCoeff2[threadIndex];

				const PxVec3 lin0(lin0XYZ_ErrorW.x, lin0XYZ_ErrorW.y, lin0XYZ_ErrorW.z);
				const PxVec3 ang0WriteBack(ang0WriteBack_VMW.x, ang0WriteBack_VMW.y, ang0WriteBack_VMW.z);
				const PxReal appliedForce = con.appliedForce[threadIndex];
				linVel += lin0 * appliedForce;
				angVel += ang0WriteBack *appliedForce;
			}

			PxReal err = con.residual[threadIndex];
			constraintErrorSq += err * err;
		}

		const float4 body0WorldOffset_InvMass0D0 = header->rAWorld_invMass0D0[threadIndex];
		const PxVec3 body0WorldOffset(body0WorldOffset_InvMass0D0.x, body0WorldOffset_InvMass0D0.y, body0WorldOffset_InvMass0D0.z);
		angVel -= body0WorldOffset.cross(linVel);


		const PxU32 broken = breakable ? PxU32((linVel.magnitude() > header->linBreakImpulse[threadIndex]) || (angVel.magnitude() > header->angBreakImpulse[threadIndex])) : 0;
		writeback.angularImpulse_residual = make_float4(angVel.x, angVel.y, angVel.z, constraintErrorSq);
		writeback.linearImpulse_broken = make_float4(linVel.x, linVel.y, linVel.z, broken ? -0.0f : 0.0f);
	}

}




#endif