bool CollisionImport::ImportRigidBody(bhkRigidBodyRef body, INode* node)
{
   if (body == NULL)
      return false;

   int lyr = body->GetLayer();
   //body->GetLayerCopy(lyr);
   int msys = body->GetMotionSystem();
   int qtype = body->GetQualityType();
   float mass = body->GetMass();
   float lindamp = body->GetLinearDamping();
   float angdamp = body->GetAngularDamping();
   float frict = body->GetFriction();
   float resti = body->GetRestitution();
   float maxlinvel = body->GetMaxLinearVelocity();
   float maxangvel = body->GetMaxAngularVelocity();
   float pendepth = body->GetPenetrationDepth();
   Vector3 center = TOVECTOR3(body->GetCenter());

   // Update node
   npSetProp(node, NP_HVK_LAYER, lyr);
   //npSetProp(node, NP_HVK_MATERIAL, mtl);
   npSetProp(node, NP_HVK_MOTION_SYSTEM, msys);
   npSetProp(node, NP_HVK_QUALITY_TYPE, qtype);
   npSetProp(node, NP_HVK_MASS, mass);
   npSetProp(node, NP_HVK_LINEAR_DAMPING, lindamp);
   npSetProp(node, NP_HVK_ANGULAR_DAMPING, angdamp);
   npSetProp(node, NP_HVK_FRICTION, frict);
   npSetProp(node, NP_HVK_RESTITUTION, resti);
   npSetProp(node, NP_HVK_MAX_LINEAR_VELOCITY, maxlinvel);
   npSetProp(node, NP_HVK_MAX_ANGULAR_VELOCITY, maxangvel);
   npSetProp(node, NP_HVK_PENETRATION_DEPTH, pendepth);
   npSetProp(node, NP_HVK_CENTER, center);

   npSetCollision(node, true);
   return true;
}
void HavokImport::createRagdollRigidBody(INode* n, INode* parent, INode* ragdollParent, bhkRigidBodyRef rbody) {

	const int MaxChar = 512;
	char buffer[MaxChar];

	//TSTR name(A2THelper(buffer, parent->GetName().c_str(), _countof(buffer)));
	n->SetName(FormatText(TEXT("Ragdoll_%s"), parent->GetName()));

	Object *pObj = n->GetObjectRef();
	IDerivedObject *dobj = nullptr;
	if (n->SuperClassID() == GEN_DERIVOB_CLASS_ID)
		dobj = static_cast<IDerivedObject*>(pObj);
	else {
		dobj = CreateDerivedObject(pObj);
	}

	MotionSystem msys = rbody->GetMotionSystem(); //?
	MotionQuality qtype = rbody->GetQualityType();
	float mass = rbody->GetMass();
	float lindamp = rbody->GetLinearDamping();
	float angdamp = rbody->GetAngularDamping();
	float frict = rbody->GetFriction();
	float resti = rbody->GetRestitution();
	float maxlinvel = rbody->GetMaxLinearVelocity();
	float maxangvel = rbody->GetMaxAngularVelocity();
	float pendepth = rbody->GetPenetrationDepth();
	InertiaMatrix im = rbody->GetInertia();

	Modifier* rbMod = (Modifier*)CreateInstance(OSM_CLASS_ID, HK_RIGIDBODY_MODIFIER_CLASS_ID);
	if (IParamBlock2* rbParameters = rbMod->GetParamBlockByID(PB_RB_MOD_PBLOCK)) {
		//These are fundamental parameters
		rbParameters->SetValue(PA_RB_MOD_MASS, 0, mass, 0);
		rbParameters->SetValue(PA_RB_MOD_RESTITUTION, 0, resti, 0);
		rbParameters->SetValue(PA_RB_MOD_FRICTION, 0, frict, 0);
		rbParameters->SetValue(PA_RB_MOD_INERTIA_TENSOR, 0, Point3(im[0][0],im[1][1],im[2][2]), 0);

		rbParameters->SetValue(PA_RB_MOD_LINEAR_DAMPING, 0, lindamp, 0);
		rbParameters->SetValue(PA_RB_MOD_CHANGE_ANGULAR_DAMPING, 0, angdamp, 0);

		rbParameters->SetValue(PA_RB_MOD_MAX_LINEAR_VELOCITY, 0, maxlinvel, 0);
		rbParameters->SetValue(PA_RB_MOD_MAX_ANGULAR_VELOCITY, 0, maxangvel, 0);

		rbParameters->SetValue(PA_RB_MOD_ALLOWED_PENETRATION_DEPTH, 0, pendepth, 0);


		rbParameters->SetValue(PA_RB_MOD_QUALITY_TYPE, 0, QT_FIXED, 0);
		rbParameters->SetValue(PA_RB_MOD_SOLVER_DEACTIVATION, 0, SD_LOW, 0);
		rbParameters->SetValue(PA_RB_MOD_DEACTIVATOR_TYPE, 0, DT_LOW, 0);

		/*body->SetMotionSystem(MotionSystem::MO_SYS_BOX);
		body->SetDeactivatorType(DeactivatorType::DEACTIVATOR_NEVER);
		body->SetSolverDeactivation(SolverDeactivation::SOLVER_DEACTIVATION_LOW);
		body->SetQualityType(MO_QUAL_FIXED);*/


		/*switch (qtype) {
		case MO_QUAL_INVALID:
			break;
		case MO_QUAL_FIXED:
			rbParameters->SetValue(PA_RB_MOD_QUALITY_TYPE, 0, QT_FIXED, 0);
			break;
		case MO_QUAL_KEYFRAMED:
			rbParameters->SetValue(PA_RB_MOD_QUALITY_TYPE, 0, QT_KEYFRAMED, 0);
			break;
		case MO_QUAL_DEBRIS:
			rbParameters->SetValue(PA_RB_MOD_QUALITY_TYPE, 0, QT_DEBRIS, 0);
			break;
		case MO_QUAL_MOVING:
			rbParameters->SetValue(PA_RB_MOD_QUALITY_TYPE, 0, QT_MOVING, 0);
			break;
		case MO_QUAL_CRITICAL:
			rbParameters->SetValue(PA_RB_MOD_QUALITY_TYPE, 0, QT_CRITICAL, 0);
			break;
		case MO_QUAL_BULLET:
			rbParameters->SetValue(PA_RB_MOD_QUALITY_TYPE, 0, QT_BULLET, 0);
			break;
		case MO_QUAL_USER:
			break;
		case MO_QUAL_CHARACTER:
			break;
		case MO_QUAL_KEYFRAMED_REPORT:
			rbParameters->SetValue(PA_RB_MOD_QUALITY_TYPE, 0, QT_KEYFRAMED_REPORTING, 0);
			break;
		}*/


	}

	//Link Rigid Body to parent Rigid Body
	ICustAttribContainer* cc = rbMod->GetCustAttribContainer();

	if (!cc)
	{
		rbMod->AllocCustAttribContainer();
		cc = rbMod->GetCustAttribContainer();
	}
	CustAttrib* c = (CustAttrib*)CreateInstance(CUST_ATTRIB_CLASS_ID, Class_ID(0x6e663460, 0x32682c72));
	IParamBlock2* custModParameters = c->GetParamBlock(0);
	custModParameters->SetValue(0, 0, parent, 0);

	cc->InsertCustAttrib(0, c);

	Modifier* constraintMod = nullptr;

	vector< bhkSerializableRef > constraints = rbody->GetConstraints();
	//Rigid Body constraints
	if (ragdollParent) {

		for (vector< bhkSerializableRef >::iterator it = constraints.begin(); it != constraints.end(); ) {
			bhkConstraintRef constraint = bhkConstraintRef(*it);
			if (constraint->IsDerivedType(bhkLimitedHingeConstraint::TYPE)) {
				bhkLimitedHingeConstraintRef limitedHingeConstraint = bhkLimitedHingeConstraintRef(*it);
				LimitedHingeDescriptor lh = limitedHingeConstraint->GetLimitedHinge();
				constraintMod = (Modifier*)CreateInstance(OSM_CLASS_ID, HK_CONSTRAINT_HINGE_CLASS_ID);
				if (IParamBlock2* constraintParameters = constraintMod->GetParamBlockByID(PB_CONSTRAINT_MOD_COMMON_SPACES_PARAMS)) {
					constraintParameters->SetValue(PA_CONSTRAINT_MOD_PARENT_NODE, 0, ragdollParent, 0);
					constraintParameters->SetValue(PA_CONSTRAINT_MOD_PARENT_ROTATION_LOCK, 0, 0, 0);

					Point3 origin(0, 0, 0);

					Matrix3 parentRotation(TOPOINT3(lh.perp2AxleInA1), TOPOINT3(lh.perp2AxleInA2), TOPOINT3(lh.axleA), origin);
					Matrix3 childRotation(TOPOINT3(lh.perp2AxleInB1), TOPOINT3(lh.perp2AxleInB2), TOPOINT3(lh.axleB), origin);

					//Matrix3 parentRotation(true);
					//MatrixFromNormal(TOPOINT3(lh.axleA), parentRotation);
					//Matrix3 childRotation(true);
					//MatrixFromNormal(TOPOINT3(lh.axleB), childRotation);

					constraintParameters->SetValue(PA_CONSTRAINT_MOD_CHILD_SPACE_ROTATION, 0, childRotation, 0);
					constraintParameters->SetValue(PA_CONSTRAINT_MOD_PARENT_SPACE_ROTATION, 0, parentRotation, 0);
				}
				if (IParamBlock2* constraintParameters = constraintMod->GetParamBlockByID(PB_HINGE_MOD_PBLOCK)) {
					constraintParameters->SetValue(PA_HINGE_MOD_IS_LIMITED, 0, 1, 0);
					constraintParameters->SetValue(PA_HINGE_MOD_LIMIT_MIN, 0, TODEG(lh.minAngle), 0);
					constraintParameters->SetValue(PA_HINGE_MOD_LIMIT_MAX, 0, TODEG(lh.maxAngle), 0);
					constraintParameters->SetValue(PA_HINGE_MOD_MAX_FRICTION_TORQUE, 0, lh.maxFriction, 0);
					//	constraintParameters->SetValue(PA_HINGE_MOD_MOTOR_TYPE, 0, lh.motor., 0);
				}
			}
			else if (constraint->IsDerivedType(bhkRagdollConstraint::TYPE)) {
				bhkRagdollConstraintRef ragdollConstraint = bhkRagdollConstraintRef(*it);
				RagdollDescriptor rag = ragdollConstraint->GetRagdoll();
				constraintMod = (Modifier*)CreateInstance(OSM_CLASS_ID, HK_CONSTRAINT_RAGDOLL_CLASS_ID);
				if (IParamBlock2* constraintParameters = constraintMod->GetParamBlockByID(PB_CONSTRAINT_MOD_COMMON_SPACES_PARAMS)) {
					constraintParameters->SetValue(PA_CONSTRAINT_MOD_PARENT_NODE, 0, ragdollParent, 0);
					constraintParameters->SetValue(PA_CONSTRAINT_MOD_PARENT_ROTATION_LOCK, 0, 0, 0);
					
					//TOVECTOR3(rag.twistA);
					//MatrixFromNormal(TOPOINT3(rag.twistA), parentRotation);
					Point3 origin(0,0,0);
					Matrix3 parentRotation(TOPOINT3(rag.planeA),TOPOINT3(rag.motorA),TOPOINT3(rag.twistA),origin);
					
					//TOVECTOR3(rag.twistB);
					//MatrixFromNormal(TOPOINT3(rag.twistB), childRotation);
					Matrix3 childRotation(TOPOINT3(rag.planeB), TOPOINT3(rag.motorB), TOPOINT3(rag.twistB), origin);

					constraintParameters->SetValue(PA_CONSTRAINT_MOD_CHILD_SPACE_TRANSLATION, 0, TOPOINT3(rag.pivotB), 0);
					constraintParameters->SetValue(PA_CONSTRAINT_MOD_CHILD_SPACE_ROTATION, 0, childRotation, 0);
					constraintParameters->SetValue(PA_CONSTRAINT_MOD_PARENT_SPACE_TRANSLATION, 0, TOPOINT3(rag.pivotA), 0);
					constraintParameters->SetValue(PA_CONSTRAINT_MOD_PARENT_SPACE_ROTATION, 0, parentRotation, 0);
				}
				if (IParamBlock2* constraintParameters = constraintMod->GetParamBlockByID(PB_RAGDOLL_MOD_PBLOCK)) {
					constraintParameters->SetValue(PA_RAGDOLL_MOD_CONE_ANGLE, 0, TODEG(rag.coneMaxAngle), 0);
					constraintParameters->SetValue(PA_RAGDOLL_MOD_PLANE_MIN, 0, TODEG(rag.planeMinAngle), 0);
					constraintParameters->SetValue(PA_RAGDOLL_MOD_PLANE_MAX, 0, TODEG(rag.planeMaxAngle), 0);
					constraintParameters->SetValue(PA_RAGDOLL_MOD_TWIST_MIN, 0, TODEG(rag.twistMinAngle), 0);
					constraintParameters->SetValue(PA_RAGDOLL_MOD_TWIST_MAX, 0, TODEG(rag.twistMaxAngle), 0);
					constraintParameters->SetValue(PA_RAGDOLL_MOD_MAX_FRICTION_TORQUE, 0, rag.maxFriction, 0);
				}
			}
			else if (constraint->IsDerivedType(bhkMalleableConstraint::TYPE)) {
				bhkMalleableConstraintRef malleableConstraint = bhkMalleableConstraintRef(*it);
				if (malleableConstraint->GetConstraintType() == (unsigned int)2) {
					LimitedHingeDescriptor lh = malleableConstraint->GetLimitedHinge();
					constraintMod = (Modifier*)CreateInstance(OSM_CLASS_ID, HK_CONSTRAINT_HINGE_CLASS_ID);
					if (IParamBlock2* constraintParameters = constraintMod->GetParamBlockByID(PB_CONSTRAINT_MOD_COMMON_SPACES_PARAMS)) {
						constraintParameters->SetValue(PA_CONSTRAINT_MOD_PARENT_NODE, 0, ragdollParent, 0);
						constraintParameters->SetValue(PA_CONSTRAINT_MOD_PARENT_ROTATION_LOCK, 0, 0, 0);
						Point3 origin(0, 0, 0);

						Matrix3 parentRotation(TOPOINT3(lh.perp2AxleInA1), TOPOINT3(lh.perp2AxleInA2), TOPOINT3(lh.axleA), origin);
						Matrix3 childRotation(TOPOINT3(lh.perp2AxleInB1), TOPOINT3(lh.perp2AxleInB2), TOPOINT3(lh.axleB), origin);

						//Matrix3 parentRotation(true);
						//MatrixFromNormal(TOPOINT3(lh.axleA), parentRotation);
						//Matrix3 childRotation(true);
						//MatrixFromNormal(TOPOINT3(lh.axleB), childRotation);

						constraintParameters->SetValue(PA_CONSTRAINT_MOD_CHILD_SPACE_ROTATION, 0, childRotation, 0);
						constraintParameters->SetValue(PA_CONSTRAINT_MOD_PARENT_SPACE_ROTATION, 0, parentRotation, 0);
					}
					if (IParamBlock2* constraintParameters = constraintMod->GetParamBlockByID(PB_HINGE_MOD_PBLOCK)) {
						constraintParameters->SetValue(PA_HINGE_MOD_IS_LIMITED, 0, 1, 0);
						constraintParameters->SetValue(PA_HINGE_MOD_LIMIT_MIN, 0, TODEG(lh.minAngle), 0);
						constraintParameters->SetValue(PA_HINGE_MOD_LIMIT_MAX, 0, TODEG(lh.maxAngle), 0);
						constraintParameters->SetValue(PA_HINGE_MOD_MAX_FRICTION_TORQUE, 0, lh.maxFriction, 0);
						//	constraintParameters->SetValue(PA_HINGE_MOD_MOTOR_TYPE, 0, lh.motor., 0);
					}
				}
				else if (malleableConstraint->GetConstraintType() == (unsigned int)7) {
					RagdollDescriptor rag = malleableConstraint->GetRagdoll();
					constraintMod = (Modifier*)CreateInstance(OSM_CLASS_ID, HK_CONSTRAINT_RAGDOLL_CLASS_ID);
					if (IParamBlock2* constraintParameters = constraintMod->GetParamBlockByID(PB_CONSTRAINT_MOD_COMMON_SPACES_PARAMS)) {
						constraintParameters->SetValue(PA_CONSTRAINT_MOD_PARENT_NODE, 0, ragdollParent, 0);
						constraintParameters->SetValue(PA_CONSTRAINT_MOD_PARENT_ROTATION_LOCK, 0, 0, 0);
						//TOVECTOR3(rag.twistA);
						//MatrixFromNormal(TOPOINT3(rag.twistA), parentRotation);
						Point3 origin(0, 0, 0);
						Matrix3 parentRotation(TOPOINT3(rag.planeA), TOPOINT3(rag.motorA), TOPOINT3(rag.twistA), origin);

						//TOVECTOR3(rag.twistB);
						//MatrixFromNormal(TOPOINT3(rag.twistB), childRotation);
						Matrix3 childRotation(TOPOINT3(rag.planeB), TOPOINT3(rag.motorB), TOPOINT3(rag.twistB), origin);


						constraintParameters->SetValue(PA_CONSTRAINT_MOD_CHILD_SPACE_TRANSLATION, 0, TOPOINT3(rag.pivotB), 0);
						constraintParameters->SetValue(PA_CONSTRAINT_MOD_CHILD_SPACE_ROTATION, 0, childRotation, 0);
						constraintParameters->SetValue(PA_CONSTRAINT_MOD_PARENT_SPACE_TRANSLATION, 0, TOPOINT3(rag.pivotA), 0);
						constraintParameters->SetValue(PA_CONSTRAINT_MOD_PARENT_SPACE_ROTATION, 0, parentRotation, 0);
					}
					if (IParamBlock2* constraintParameters = constraintMod->GetParamBlockByID(PB_RAGDOLL_MOD_PBLOCK)) {
						constraintParameters->SetValue(PA_RAGDOLL_MOD_CONE_ANGLE, 0, TODEG(rag.coneMaxAngle), 0);
						constraintParameters->SetValue(PA_RAGDOLL_MOD_PLANE_MIN, 0, TODEG(rag.planeMinAngle), 0);
						constraintParameters->SetValue(PA_RAGDOLL_MOD_PLANE_MAX, 0, TODEG(rag.planeMaxAngle), 0);
						constraintParameters->SetValue(PA_RAGDOLL_MOD_TWIST_MIN, 0, TODEG(rag.twistMinAngle), 0);
						constraintParameters->SetValue(PA_RAGDOLL_MOD_TWIST_MAX, 0, TODEG(rag.twistMaxAngle), 0);
						constraintParameters->SetValue(PA_RAGDOLL_MOD_MAX_FRICTION_TORQUE, 0, rag.maxFriction, 0);
					}
				}
			}
			++it;
		}
	}

	dobj->SetAFlag(A_LOCK_TARGET);
	dobj->AddModifier(rbMod);
	if (constraintMod)
		dobj->AddModifier(constraintMod);
	dobj->ClearAFlag(A_LOCK_TARGET);
	n->SetObjectRef(dobj);

}