//////////////////////////////////////////////////////////////////////
//
//	Articulated Entity
//	
//	File: articulatedentity.cpp
//	Description : ArticulatedEntity class implementation
//
//	History:
//	-:Created by Anton Knyazev
//
//////////////////////////////////////////////////////////////////////

#include "StdAfx.h"

#include "bvtree.h"
#include "geometry.h"
#include "singleboxtree.h"
#include "cylindergeom.h"
#include "rigidbody.h"
#include "physicalplaceholder.h"
#include "physicalentity.h" 
#include "geoman.h"
#include "physicalworld.h"
#include "rigidentity.h"
#include "articulatedentity.h"

int __ae_step=0; // for debugging

CArticulatedEntity::CArticulatedEntity(CPhysicalWorld *pWorld) : CRigidEntity(pWorld)
{
	m_nJoints = 0;
	m_bGrounded = 0;
	m_acc.Set(0,0,0); 
	m_wacc.Set(0,0,0);
	m_joints = new ae_joint[m_nJointsAlloc=4];
	m_nJoints = 0;
	m_offsPivot.Set(0,0,0);
	m_iSimClass = 4;
	m_bAwake = 0;
	m_simTime = 0;
	m_simTimeAux = 10.0f;
	m_scaleBounceResponse = 1;
	m_posPivot.Set(0,0,0);
	m_bIaReady = 0;
	m_nPartsAlloc = 4;
	m_parts = new geom[m_nPartsAlloc];
	m_infos = new ae_part_info[m_nPartsAlloc];
	m_bCheckCollisions = 0;
	m_bCollisionResp = 0;
	m_maxAllowedStep = 0.02f;
	m_bInheritVel = 0;
	m_pHost = 0;
	m_posHostPivot.zero(); m_qHostPivot.SetIdentity();
	m_bPartPosForced = 0;
	m_bExertImpulse = 0;
	m_Ya_vec[0].Set(0,0,0);
	m_Ya_vec[0].Set(0,0,0);
	m_nBodyContacts = m_nDynContacts = m_bInGroup = 0;
	m_nCollLyingMode = 5;
	m_gravity.Set(0,0,-7.5f);
	m_Emin = sqr(0.025f);
	m_damping = 0.1f;
	m_gravityLyingMode.Set(0,0,-5.0f);
	m_dampingLyingMode = 0.5f;
	m_EminLyingMode = sqr(0.05f);
	m_maxPenetrationCur = 0;
	m_iSimType = 1;
	m_iSimTypeLyingMode = 1;
	m_iSimTypeCur = 1;
	m_iSimTypeOverride = 0;
	m_bFastLimbs = 0;
	m_bExpandHinges = 0;
	m_bUsingUnproj = 0;
	m_softness[0] = 0.01f;
	m_softness[1] = 0.05f;
	m_softness[2] = 0.001f;
	m_softness[3] = 0.01f;
	m_bUpdateBodies = 1;
	m_bCanSweep = 0;
	m_iVarPart0 = 0;
	m_nRoots = 0;
	m_lockJoints = 0;
	m_flags &= ~pef_never_break;
	m_bIgnoreCommands = 0;
}

CArticulatedEntity::~CArticulatedEntity()
{
	if (m_joints) delete[] m_joints;
	if (m_infos) delete[] m_infos;
}

void CArticulatedEntity::AlertNeighbourhoodND()
{
	if (m_pHost)
		m_pHost->Release();
	m_pHost = 0;
	CRigidEntity::AlertNeighbourhoodND();
}


int CArticulatedEntity::AddGeometry(phys_geometry *pgeom, pe_geomparams* _params,int id, int bThreadSafe)
{
	if (pgeom && fabs_tpl(_params->mass)+fabs_tpl(_params->density)!=0 && (pgeom->V<=0 || pgeom->Ibody.x<0 || pgeom->Ibody.y<0 || pgeom->Ibody.z<0))	{
		char errmsg[256];
		_snprintf(errmsg,256,"CArticulatedEntity::AddGeometry: (%s at %.1f,%.1f,%.1f) Trying to add bad geometry",
			m_pWorld->m_pRenderer ? m_pWorld->m_pRenderer->GetForeignName(m_pForeignData,m_iForeignData,m_iForeignFlags):"", m_pos.x,m_pos.y,m_pos.z);
		VALIDATOR_LOG(m_pWorld->m_pLog,errmsg);
		return -1;
	}

	if (!pgeom)
		return 0;
	ChangeRequest<pe_geomparams> req(this,m_pWorld,_params,bThreadSafe,pgeom,id);
	if (req.IsQueued()) {
		WriteLock lock(m_lockPartIdx);
		if (id<0)
			*((int*)req.GetQueuedStruct()-1) = id = m_iLastIdx++;
		else
			m_iLastIdx = max(m_iLastIdx,id+1);
		return id;
	}

	if (_params->type!=pe_articgeomparams::type_id) {
		pe_articgeomparams params = *(pe_geomparams*)_params;
		return AddGeometry(pgeom,&params,id,bThreadSafe);
	}
	pe_articgeomparams *params = (pe_articgeomparams*)_params;

	int res,i,j,nPartsAlloc=m_nPartsAlloc,nParts=m_nParts,idx;
	if ((res = CPhysicalEntity::AddGeometry(pgeom,params,id,1))<0 || m_nParts==nParts && params->flags & geom_proxy)
		return res;

	idx = m_nParts-1;
	float V=pgeom->V*cube(params->scale), M=params->mass ? params->mass : params->density*V;
	Vec3 bodypos = m_pos+m_qrot*(params->pos+params->q*pgeom->origin); 
	quaternionf bodyq = m_qrot*params->q*pgeom->q;
	//if (M<=0) M=0.1f;
	m_parts[idx].mass = M;
	m_parts[idx].flags |= geom_monitor_contacts;
	if (nPartsAlloc < m_nPartsAlloc) {
		ae_part_info *pInfos = m_infos;
		memcpy(m_infos = new ae_part_info[m_nPartsAlloc], pInfos, sizeof(ae_part_info)*nPartsAlloc);
		for(i=0;i<m_nParts;i++) {
			m_infos[i].pos=m_parts[i].pos; m_infos[i].q=m_parts[i].q; m_infos[i].scale=m_parts[i].scale;
			m_infos[i].BBox[0]=m_parts[i].BBox[0]; m_infos[i].BBox[1]=m_parts[i].BBox[1];
			m_parts[i].pNewCoords = (coord_block_BBox*)&m_infos[i].pos;
		}
		if (pInfos) delete[] pInfos;
	}

	for(i=0;i<m_nJoints && m_joints[i].idbody!=params->idbody;i++);
	if (i==m_nJoints) {
		if (m_nJoints==m_nJointsAlloc) {
			ae_joint *pJoints = m_joints;
			memcpy(m_joints=new ae_joint[m_nJointsAlloc+=4], pJoints, sizeof(ae_joint)*m_nJoints);
			if (pJoints) delete[] pJoints;
		}
		m_joints[m_nJoints].iStartPart = idx;
		m_joints[m_nJoints].nParts = 0;
		m_joints[m_nJoints++].idbody = params->idbody;
	}
	if (m_joints[i].iStartPart+m_joints[i].nParts<idx) {
		geom tmppart = m_parts[idx];
		for(; idx>m_joints[i].iStartPart+m_joints[i].nParts; idx--) {
			m_parts[idx] = m_parts[idx-1]; m_infos[idx] = m_infos[idx-1];
			m_parts[idx].pNewCoords = (coord_block_BBox*)&m_infos[idx];
		}
		m_parts[idx] = tmppart;
		m_parts[idx].pNewCoords = (coord_block_BBox*)&m_infos[idx];
		for(j=0;j<m_nJoints;j++) if (m_joints[j].iStartPart>=idx)
			m_joints[j].iStartPart++;
	}
	m_joints[i].nParts++;

	if (M>0) {
		// pivot positions are not recalculated
		if (m_body.M==0)
			m_body.Create(bodypos,pgeom->Ibody*sqr(params->scale)*cube(params->scale),bodyq, V,M, m_qrot,m_pos);
		else
			m_body.Add(bodypos,pgeom->Ibody*sqr(params->scale)*cube(params->scale),bodyq,V,M);	

		if (m_joints[i].body.M==0) {
			m_joints[i].body.Create(bodypos,pgeom->Ibody*sqr(params->scale)*cube(params->scale),bodyq, V,M, m_qrot*params->q,bodypos);
			m_joints[i].body.softness[0] = m_softness[0];
			m_joints[i].body.softness[1] = m_softness[1];
		} else
			m_joints[i].body.Add(bodypos,pgeom->Ibody*sqr(params->scale)*cube(params->scale),bodyq,V,M);	
	}	else if (m_joints[i].nParts==1)
		m_joints[i].body.pos = bodypos;
	m_infos[idx].iJoint = i;
	m_infos[idx].idbody = params->idbody;
	m_joints[i].quat = m_joints[i].body.q*m_joints[i].body.qfb;
	m_infos[idx].q0 = !m_joints[i].quat*(m_qrot*m_parts[idx].q);
	m_infos[idx].pos0 = (m_pos+m_qrot*m_parts[idx].pos-m_joints[i].body.pos)*m_joints[i].quat;
	m_infos[idx].pos=m_parts[idx].pos; m_infos[idx].q=m_parts[idx].q; m_infos[idx].scale=m_parts[idx].scale;
	m_infos[idx].BBox[0]=m_parts[idx].BBox[0]; m_infos[idx].BBox[1]=m_parts[idx].BBox[1];
	m_parts[idx].pNewCoords = (coord_block_BBox*)&m_infos[idx].pos;
	
	return res;
}

void CArticulatedEntity::RemoveGeometry(int id, int bThreadSafe)
{
	ChangeRequest<void> req(this,m_pWorld,0,bThreadSafe,0,id);
	if (req.IsQueued())
		return;

	int i,j;
	for(i=0;i<m_nParts && m_parts[i].id!=id;i++);
	if (i==m_nParts) return;
	phys_geometry *pgeom = m_parts[i].pPhysGeom;

	if (m_parts[i].mass>0) {
		Vec3 bodypos = m_pos + m_qrot*(m_parts[i].pos+pgeom->origin); 
		quaternionf bodyq = m_qrot*m_parts[i].q*pgeom->q;
		m_body.Add(bodypos,-pgeom->Ibody,bodyq,-pgeom->V,-m_parts[i].mass);
		m_joints[m_infos[i].iJoint].body.Add(bodypos,-pgeom->Ibody,bodyq,-pgeom->V,-m_parts[i].mass);
		if (m_body.M<1E-8) m_body.M = 0;
		if (m_joints[m_infos[i].iJoint].body.M<1E-8) m_joints[m_infos[i].iJoint].body.M = 0;
		m_joints[m_infos[i].iJoint].nParts--;
		for(j=0;j<m_nJoints;j++) if (m_joints[j].iStartPart>i)
			m_joints[j].iStartPart--;
		memmove(m_infos+i, m_infos+i+1, (m_nParts-i-1)*sizeof(m_infos[0]));
	}

	CPhysicalEntity::RemoveGeometry(id,1);

	for(i=0;i<m_nParts;i++)
		m_parts[i].pNewCoords = (coord_block_BBox*)&m_infos[i].pos;
}

void CArticulatedEntity::RecomputeMassDistribution(int ipart,int bMassChanged)
{
	int i,iStart,iEnd;
	float V,M0=m_body.M;
	Vec3 bodypos;
	quaternionf bodyq;
	Matrix33 R;

	if (ipart>=0) {
		i = m_infos[ipart].iJoint;
		if (m_joints[i].nParts==1 && !bMassChanged) {
			m_joints[i].quat = m_parts[ipart].q*!m_infos[ipart].q0;
			m_joints[i].body.q = m_qrot*m_joints[i].quat*!m_joints[i].body.qfb;
			m_joints[i].body.pos = m_qrot*(m_parts[ipart].pos-m_joints[i].quat*m_infos[ipart].pos0)+m_pos;
			R = Matrix33(m_joints[i].body.q);
			m_joints[i].I = R*m_joints[i].body.Ibody*R.T();
			return;
		}
		m_joints[i].body.zero();
		iStart = m_joints[i].iStartPart; iEnd = m_joints[i].iStartPart+m_joints[i].nParts-1;
	}	else {
		m_body.zero();
		for(i=0;i<m_nJoints;i++) m_joints[i].body.zero();
		iStart = 0; iEnd = m_nParts-1;
		for(i=0;i<m_nJoints;i++) {
			m_joints[i].prev_v = m_joints[i].body.v;
			m_joints[i].prev_w = m_joints[i].body.w;
		}
	}

	for(i=iStart; i<=iEnd; i++) {
		V = m_parts[i].pPhysGeom->V*cube(m_parts[i].scale);
		bodypos = m_pos + m_qrot*(m_parts[i].pos+m_parts[i].q*m_parts[i].pPhysGeom->origin); 
		bodyq = m_qrot*m_parts[i].q*m_parts[i].pPhysGeom->q;

		if (m_parts[i].mass>0) {
			if (ipart<0) {
				if (m_body.M==0)
					m_body.Create(bodypos,m_parts[i].pPhysGeom->Ibody*sqr(m_parts[i].scale)*cube(m_parts[i].scale),bodyq, V,m_parts[i].mass, m_qrot,m_pos);
				else
					m_body.Add(bodypos,m_parts[i].pPhysGeom->Ibody*sqr(m_parts[i].scale)*cube(m_parts[i].scale),bodyq, V,m_parts[i].mass);
			}

			if (m_joints[m_infos[i].iJoint].body.M==0) {
				m_joints[m_infos[i].iJoint].body.Create(bodypos,m_parts[i].pPhysGeom->Ibody*sqr(m_parts[i].scale)*cube(m_parts[i].scale),bodyq, 
					V,m_parts[i].mass, m_qrot*m_parts[i].q,bodypos);
				m_joints[m_infos[i].iJoint].body.softness[0] = m_softness[0];
				m_joints[m_infos[i].iJoint].body.softness[1] = m_softness[1];
			} else
				m_joints[m_infos[i].iJoint].body.Add(bodypos,m_parts[i].pPhysGeom->Ibody*sqr(m_parts[i].scale)*cube(m_parts[i].scale),bodyq, 
					V,m_parts[i].mass);	
		}
	}

	if (ipart<0) {
		if (M0>0)
			M0 = m_body.M/M0;
		for(i=0;i<m_nJoints;i++) {
			m_joints[i].body.P = (m_joints[i].body.v=m_joints[i].prev_v)*m_body.M; 
			m_joints[i].body.L = m_joints[i].body.q*(m_joints[i].body.Ibody*(!m_joints[i].body.q*(m_joints[i].body.w=m_joints[i].prev_w)));
			m_joints[i].Pext*=M0; m_joints[i].Lext*=M0;
			m_joints[i].Pimpact*=M0; m_joints[i].Limpact*=M0;
		}
	}
}


int CArticulatedEntity::SetParams(pe_params *_params, int bThreadSafe)
{
	ChangeRequest<pe_params> req(this,m_pWorld,_params,bThreadSafe);
	if (req.IsQueued())
		return 1+(m_bProcessed>>PENT_QUEUED_BIT);

	Vec3 prevpos = m_pos;
	quaternionf prevq = m_qrot;

	if (_params->type==pe_params_articulated_body::type_id) {
		if (((pe_params_articulated_body*)_params)->bGrounded==100) {
			m_bIgnoreCommands=0; return 1;
		}
	}
	if (m_bIgnoreCommands)
		return 1;

	if (CRigidEntity::SetParams(_params,1)) {
		if (_params->type==pe_params_pos::type_id) {
			pe_params_pos *params = (pe_params_pos*)_params;
			int i=-1;
			{ WriteLock lock(m_lockUpdate);
				if ((prevq.v-m_qrot.v).len2()>0 && (m_joints[0].quat0.v-m_qrot.v).len2()>0) {
					if (m_nJoints>0) {
						/*if (m_bGrounded) {
							if ((m_joints[0].flags & all_angles_locked)!=all_angles_locked)	{
								Vec3 dangle; int j;
								quaternionf qalpha(m_joints[0].q+m_joints[0].qext); 
								(!m_qrot*m_joints[0].quat0*qalpha).get_Euler_angles_xyz(dangle);
								for(j=0;j<3;j++) if (!(m_joints[0].flags & angle0_locked<<j))
									m_joints[0].q[j] = dangle[i]-m_joints[0].qext[j];
							}
							m_joints[0].quat0 = m_qrot;
						} else {
							m_qrot.get_Euler_angles_xyz(m_joints[0].q);
							m_joints[0].qext.zero();
						}*/
						if (prevq.w==1.0f)
							prevq = m_joints[0].quat0;
						m_offsPivot = (m_qrot*!prevq)*m_offsPivot;
						m_posPivot = m_pos + m_offsPivot;
						for(i=0;i<m_nJoints;i++) if (m_joints[i].iParent<0)	{
							m_joints[i].quat0 = m_qrot; m_joints[i].bQuat0Changed = 1;
							if (m_joints[i].flags & joint_rotate_pivot)
								m_joints[i].pivot[0] = (m_qrot*!prevq)*m_joints[i].pivot[0];
						}
						i = 0;
					}
				}
				if (!m_bPartPosForced) {
					m_qrot.SetIdentity(); m_qNew.SetIdentity();
				}
				float diff = (prevpos-m_pos).len2();
				if (diff>0) {
					m_posPivot = m_pos + m_offsPivot; i = 0;
				}
				if (diff>sqr(0.003f) && m_body.M>0 && !(m_flags & aef_recorded_physics) && !m_bGrounded && m_bCollisionResp && m_iLastLog>0) 
					for(i=0;i<m_nJoints;i++) {
						/*m_joints[i].body.v.zero(); m_joints[i].body.P.zero();
						m_joints[i].body.w.zero(); m_joints[i].body.L.zero();
						m_joints[i].dq.zero();
						m_joints[i].Pimpact.zero(); m_joints[i].Limpact.zero();*/
						m_joints[i].body.pos += m_pos-prevpos;
					}
			}
			if (i>=0 && params->bRecalcBounds) {
				for(;i<m_nJoints;i++) SyncBodyWithJoint(i);
				ComputeBBox(m_BBoxNew);
				UpdatePosition(0);
			}
		}
		if (_params->type==pe_params_part::type_id)	{
			if (!is_unused(((pe_params_part*)_params)->pos) && !(m_nJoints==0 || m_joints[0].flags & joint_rotate_pivot))
				m_bPartPosForced = 1;
			if (!m_pHost && ((pe_params_part*)_params)->bRecalcBBox && m_body.Minv==0 && m_nParts) {
				CPhysicalEntity **pentlist;
				int nEnts=m_pWorld->GetEntitiesAround(m_BBox[0],m_BBox[1],pentlist,ent_sleeping_rigid|ent_living|ent_independent|ent_triggers,this);
				for(--nEnts;nEnts>=0;nEnts--) if (pentlist[nEnts]!=this)
					pentlist[nEnts]->Awake();
			}
		}
		return 1;
	}

	if (_params->type==pe_params_joint::type_id) {
		pe_params_joint *params = (pe_params_joint*)_params;
		m_bPartPosForced = 0;
		int i,j,op[2]={-1,-1},nChanges=0,bAnimated=m_bCollisionResp^1|iszero(m_body.M);
		for(i=0;i<m_nJoints;i++) {
			if (m_joints[i].idbody==params->op[0]) op[0]=i;
			if (m_joints[i].idbody==params->op[1]) op[1]=i;
		}
		if (op[0]<0 && params->op[0]>=0 || op[1]<0) 
			return 0;

		quaternionf qparent, qchild = m_joints[op[1]].body.q*m_joints[op[1]].body.qfb;
		Vec3 posparent, poschild = m_joints[op[1]].body.pos;
		if (op[0]>=0) {
			qparent = m_joints[op[0]].body.q*m_joints[op[0]].body.qfb;
			posparent = m_joints[op[0]].body.pos;
		} else {
			qparent.SetIdentity(); 
			posparent = m_posPivot;
		}

		if (!is_unused(params->flags)) {
			m_joints[op[1]].flags = params->flags | m_joints[op[1]].flags & params->flags & (angle0_limit_reached|angle0_gimbal_locked)*7;
			for(i=0;i<3;i++) if (params->flags & angle0_locked<<i)
				m_joints[op[1]].dq[i] = 0;
		}
		if (!is_unused(params->pivot)) {
			Vec3 pivot = params->pivot;
			if (params->flagsPivot & 8)
				pivot = m_parts[m_joints[op[0]].iStartPart].q*pivot + m_parts[m_joints[op[0]].iStartPart].pos;
			pivot = m_qrot*pivot + m_pos;
			if (params->flagsPivot & 1) {
				if (params->op[0]!=-1 || params->flagsPivot & 4)
					m_joints[op[1]].pivot[0] = (pivot-posparent)*qparent;
				else m_joints[op[1]].pivot[0].Set(0,0,0);
			}								
			if (params->flagsPivot & 2)	{
				if (m_bGrounded || params->op[0]!=-1 || params->flagsPivot & 4)
					m_joints[op[1]].pivot[1] = (pivot-poschild)*qchild;
				else m_joints[op[1]].pivot[1].Set(0,0,0);
			}
			if (params->flagsPivot & 4)
				m_joints[op[1]].flags |= joint_rotate_pivot;
		}
		
		if (!is_unused(params->nSelfCollidingParts)) {
			for(i=0;i<m_nParts;i++) for(j=0;j<params->nSelfCollidingParts;j++) if (m_parts[i].id==params->pSelfCollidingParts[j])
				m_joints[op[1]].selfCollMask |= getmask(i);
		}

		float rdt = !is_unused(params->ranimationTimeStep) ? params->ranimationTimeStep : 
							 (!is_unused(params->animationTimeStep) ? 1.0f/params->animationTimeStep : 0);
		for(i=0;i<3;i++) {
			if (!is_unused(params->limits[0][i])) m_joints[op[1]].limits[0][i] = params->limits[0][i];
			if (!is_unused(params->limits[1][i])) m_joints[op[1]].limits[1][i] = params->limits[1][i];
			if (!is_unused(params->bounciness[i])) m_joints[op[1]].bounciness[i] = params->bounciness[i];
			if (!is_unused(params->ks[i])) m_joints[op[1]].ks[i] = params->ks[i];
			if (!is_unused(params->kd[i])) 
				m_joints[op[1]].kd[i] = ((m_joints[op[1]].flags & angle0_auto_kd<<i) ? 2.0f*sqrt_tpl(max(1E-10f,m_joints[op[1]].ks[i])) : 1.0f)*params->kd[i];
			if (!is_unused(params->qdashpot[i])) m_joints[op[1]].qdashpot[i] = params->qdashpot[i];
			if (!is_unused(params->kdashpot[i])) m_joints[op[1]].kdashpot[i] = params->kdashpot[i];
			if (!is_unused(params->q[i])) m_joints[op[1]].prev_q[i] = m_joints[op[1]].q[i] = params->q[i];
			if (!is_unused(params->qtarget[i])) m_joints[op[1]].q0[i] = params->qtarget[i];
			if (!is_unused(params->qext[i])) { 
				m_joints[op[1]].dqext[i] = (params->qext[i]-m_joints[op[1]].qext[i])*rdt;
				m_joints[op[1]].qext[i] = params->qext[i]; nChanges++; 
				if (!(m_joints[op[1]].flags & angle0_locked<<i) && 
						isneg(m_joints[op[1]].limits[0][i]-m_joints[op[1]].qext[i]) + isneg(m_joints[op[1]].qext[i]-m_joints[op[1]].limits[1][i]) + 
						isneg(m_joints[op[1]].limits[1][i]-m_joints[op[1]].limits[1][i]) < 2) 
				{	// qext violates limits; adjust the limits
					float diff[2];
					diff[0] = m_joints[op[1]].limits[0][i]-m_joints[op[1]].qext[i];
					diff[1] = m_joints[op[1]].limits[1][i]-m_joints[op[1]].qext[i];
					diff[0] -= (int)(diff[0]/(2*g_PI))*2*g_PI;
					diff[1] -= (int)(diff[1]/(2*g_PI))*2*g_PI;
					m_joints[op[1]].limits[isneg(fabs_tpl(diff[1])-fabs_tpl(diff[0]))][i] = m_joints[op[1]].qext[i];
				}
			}
		}
		if (params->pMtx0) 
			params->q0 = quaternionf(*params->pMtx0/params->pMtx0->GetRow(0).len());

		if (!is_unused(params->q0)) { 
			m_joints[op[1]].quat0 = params->q0; m_joints[op[1]].bQuat0Changed = 0; nChanges++; 
		}
		ENTITY_VALIDATE("CArticulatedEntity:SetParams(params_joint)",params);

		if (m_joints[op[1]].iParent!=op[0]) {
			if (!is_unused(params->qext[0])) {
				//(!(qparent*m_joints[op[1]].quat0)*qchild).GetEulerAngles_XYZ(m_joints[op[1]].q); //EULER_IVO 
				m_joints[op[1]].q = Ang3::GetAnglesXYZ( Matrix33(!(qparent*m_joints[op[1]].quat0)*qchild) );
				m_joints[op[1]].prev_q = (m_joints[op[1]].q -= m_joints[op[1]].qext);
			} else 
				//(!(qparent*m_joints[op[1]].quat0)*qchild).GetEulerAngles_XYZ(m_joints[op[1]].qext);	//EULER_IVO 
				m_joints[op[1]].qext = Ang3::GetAnglesXYZ( Matrix33(!(qparent*m_joints[op[1]].quat0)*qchild) );

			if (op[1]!=op[0]+1) {
				int nChildren;
				for(nChildren=m_joints[op[1]].nChildren+1,i=1; i<nChildren; nChildren+=m_joints[op[1]+i++].nChildren);
				ae_joint jbuf,*pjbuf=nChildren>1 ? new ae_joint[nChildren] : &jbuf;
				memcpy(pjbuf, m_joints+op[1], nChildren*sizeof(ae_joint));
				if (op[1]<op[0]) {
					for(i=1; i<nChildren; i++) pjbuf[i].iParent += op[0]-op[1]+1-nChildren;
					for(i=op[1]+nChildren; i<m_nJoints; i++) if (m_joints[i].iParent>op[1] && m_joints[i].iParent<=op[0]) 
						m_joints[i].iParent -= nChildren;
					memmove(m_joints+op[1], m_joints+op[1]+nChildren, (op[0]-op[1]-nChildren+1)*sizeof(ae_joint)); 
					op[0] -= nChildren;
				} else {
					for(i=1;i<nChildren;i++) pjbuf[i].iParent -= op[1]-op[0]-1;
					for(i=op[0]+1; i<op[1]; i++) if (m_joints[i].iParent>op[0]) m_joints[i].iParent += nChildren;
					memmove(m_joints+op[0]+1+nChildren, m_joints+op[0]+1, (op[1]-op[0]-1)*sizeof(ae_joint));
				}
				memcpy(m_joints+op[0]+1, pjbuf, nChildren*sizeof(ae_joint));
				if (pjbuf!=&jbuf) delete[] pjbuf;
				JointListUpdated();
			}
			op[1] = op[0]+1;
			m_joints[op[1]].iParent = op[0];
			m_joints[op[1]].q0 = m_joints[op[1]].q;
			if (op[0]>=0) {
				m_joints[op[0]].nChildren++;
				m_joints[op[0]+1].iLevel = m_joints[op[0]].iLevel+1;
			} else m_joints[op[0]+1].iLevel = 0;
			for(i=op[0]; i>=0; i=m_joints[i].iParent) 
				m_joints[i].nChildrenTree += m_joints[op[0]+1].nChildrenTree+1;
			if (params->op[0]==-1 && !m_bGrounded && !(params->flagsPivot & 4)) {
				m_posPivot = m_joints[op[1]].body.pos;
				m_offsPivot = m_posPivot-m_pos;
			}
			(m_joints[op[1]].flags &= ~joint_expand_hinge) |= joint_expand_hinge & -m_bExpandHinges;
			MARK_UNUSED(m_joints[op[1]].hingePivot[0]);
			nChanges++;
		}	else {
			for(i=0;i<3;i++) if (!is_unused(params->q[i])) {
				m_joints[op[1]].q[i] = params->q[i]; nChanges++;
			}
			if (nChanges) {
				m_joints[op[1]].q0 = m_joints[op[1]].q;
				if (!params->bNoUpdate)	{
					for(i=op[1];i<=op[1]+m_joints[op[1]].nChildrenTree;i++) SyncBodyWithJoint(i);
					UpdatePosition(0);
				}
			}
		}
		m_bUpdateBodies = bAnimated;
		for(m_nRoots=i=0;i<m_nJoints;i++)
			m_nRoots += max(0,-m_joints[i].iParent);
		return 1;
	}

	if (_params->type==pe_params_articulated_body::type_id) {
		pe_params_articulated_body *params = (pe_params_articulated_body*)_params;
		if (!is_unused(params->pivot)) {
			Vec3 offs = m_posPivot;
			m_posPivot = m_pos+(m_offsPivot=params->pivot); offs = m_posPivot-offs;
			for(int i=0;i<m_nJoints;i++) m_joints[i].body.pos += offs;
		}
		if (!is_unused(params->bGrounded)) {
			m_bGrounded = params->bGrounded;
			if (m_nJoints>0 && m_joints[0].iParent==-1) {
				quaternionf qroot = m_joints[0].body.q*m_joints[0].body.qfb;
				if (m_bGrounded) {
					if (!is_unused(params->pivot)) {
						m_joints[0].pivot[1] = params->pivot-m_joints[0].body.pos;
						m_offsPivot -= m_joints[0].pivot[1]; m_posPivot += m_joints[0].pivot[1];
						m_joints[0].pivot[1] = m_joints[0].pivot[1]*qroot;
					}
				} else {
					m_joints[0].pivot[1] = qroot*m_joints[0].pivot[1];
					m_offsPivot -= m_joints[0].pivot[1]; m_posPivot -= m_joints[0].pivot[1];
					m_joints[0].pivot[1].Set(0,0,0);
					m_joints[0].flags &= ~all_angles_locked;
					m_joints[0].limits[0].Set(-1E15,-1E15,-1E15);
					m_joints[0].limits[1].Set(1E15,1E15,1E15);
					m_joints[0].ks.Set(0,0,0); m_joints[0].kd.Set(0,0,0);
					m_acc.Set(0,0,0); m_body.w.Set(0,0,0); m_wacc.Set(0,0,0);
				}
			}
			m_bIaReady = 0; m_simTime = 0; m_bAwake = 1;
		}
		if (!is_unused(params->v) && (params->v-m_body.v).len2()>0) { m_body.v = params->v; m_simTime = 0; m_bAwake = 1; }
		if (!is_unused(params->a) && (params->a-m_acc).len2()>0) { m_acc = params->a; m_simTime = 0; m_bAwake = 1; }
		if (!is_unused(params->w) && (params->w-m_body.w).len2()>0) { 
			if (m_bGrounded)
				m_body.w = params->w; 
			else if (m_bIaReady) {
				Matrix33 basis_inv = GetMtxFromBasis(m_joints[0].rotaxes);
				basis_inv.Invert(); m_joints[0].dq = basis_inv*m_body.w;
			}
			m_simTime = 0; m_bAwake = 1;
		}
		if (!is_unused(params->wa) && (params->wa-m_wacc).len2()>0) { m_wacc = params->wa; m_simTime = 0; }
		if (!is_unused(params->scaleBounceResponse)) m_scaleBounceResponse = params->scaleBounceResponse;
		if (params->bApply_dqext) {
			for(int i=0;i<m_nJoints;i++)
				m_joints[i].dq += m_joints[i].dqext;
		}
		if (!is_unused(params->bAwake))
			if (!(m_bAwake = params->bAwake))
				m_simTime = 10.0f;
		int bRecalcPos = 0;
		if (!is_unused(params->pHost)) { 
			if (m_pHost) {
				m_pHost->Release(); m_pHost = 0;
			}
			if (params->pHost) {
				(m_pHost = ((CPhysicalPlaceholder*)params->pHost)->GetEntity())->AddRef(); 
				bRecalcPos = 1; 
			}
		}
		if (!is_unused(params->bInheritVel)) { m_bInheritVel = params->bInheritVel; bRecalcPos = 1; }
		if (!is_unused(params->posHostPivot)) { m_posHostPivot = params->posHostPivot; bRecalcPos = 1; }
		if (!is_unused(params->qHostPivot)) { m_qHostPivot = params->qHostPivot; bRecalcPos = 1; }
		if (bRecalcPos)
			SyncWithHost(params->bRecalcJoints,0);
		if (!is_unused(params->bCheckCollisions)) m_bCheckCollisions = params->bCheckCollisions;
		if (!is_unused(params->bCollisionResp)) m_bCollisionResp = params->bCollisionResp;

		if (!is_unused(params->nCollLyingMode)) m_nCollLyingMode = params->nCollLyingMode;
		if (!is_unused(params->gravityLyingMode)) m_gravityLyingMode = params->gravityLyingMode;
		if (!is_unused(params->dampingLyingMode)) m_dampingLyingMode = params->dampingLyingMode;
		if (!is_unused(params->minEnergyLyingMode)) m_EminLyingMode = params->minEnergyLyingMode;
		if (!is_unused(params->iSimType)) m_iSimType = params->iSimType;
		if (!is_unused(params->iSimTypeLyingMode)) m_iSimTypeLyingMode = params->iSimTypeLyingMode;
		if (!is_unused(params->bExpandHinges)) m_bExpandHinges = params->bExpandHinges;

		return 1;
	}

	return 0;
}


int CArticulatedEntity::GetParams(pe_params *_params)
{
	int res = CRigidEntity::GetParams(_params);
	if (res)
		return res;

	if (_params->type==pe_params_joint::type_id) {
		pe_params_joint *params = (pe_params_joint*)_params;
		int i; for(i=0;i<m_nJoints && m_joints[i].idbody!=params->op[1];i++);
		if (i>=m_nJoints)
			return 0;
		params->flags = m_joints[i].flags;
		params->pivot = (m_joints[i].body.pos + (m_joints[i].body.q*m_joints[i].body.qfb)*m_joints[i].pivot[1] - m_pos)*m_qrot;
		params->q0 = m_joints[i].quat0;
		params->qtarget = m_joints[i].q0;
		if (params->pMtx0) *params->pMtx0 = Matrix33(m_joints[i].quat0);
		params->limits[0] = m_joints[i].limits[0];
		params->limits[1] = m_joints[i].limits[1];
		params->bounciness = m_joints[i].bounciness;
		params->ks = m_joints[i].ks;
		params->kd = m_joints[i].kd;
		params->q = m_joints[i].q;
		params->qext = m_joints[i].qext;
		params->op[0] = m_joints[i].iParent>=0 ? m_joints[m_joints[i].iParent].idbody : -1;
		return 1;
	}

	if (_params->type==pe_params_articulated_body::type_id) {
		pe_params_articulated_body *params = (pe_params_articulated_body*)_params;
		params->bGrounded = m_bGrounded;
		params->pivot = m_offsPivot;
		params->a = m_acc;
		params->wa = m_wacc;
		params->w = m_body.w;
		params->v = m_body.v;
		params->scaleBounceResponse = m_scaleBounceResponse;
		params->pHost = m_pHost;
		params->bInheritVel = m_bInheritVel;
		params->posHostPivot = m_posHostPivot;
		params->qHostPivot = m_qHostPivot;
		params->bCheckCollisions = m_bCheckCollisions;
		params->bCollisionResp = m_bCollisionResp;
		params->nCollLyingMode = m_nCollLyingMode;
		params->gravityLyingMode = m_gravityLyingMode;
		params->dampingLyingMode = m_dampingLyingMode;
		params->minEnergyLyingMode = m_EminLyingMode;
		params->iSimType = m_iSimType;
		params->iSimTypeLyingMode = m_iSimTypeLyingMode;
		params->bExpandHinges = m_bExpandHinges;
		params->nRoots = m_nRoots;
		return 1;
	}

	return 0;
}


int CArticulatedEntity::GetStatus(pe_status* _status)
{
	if (_status->type==pe_status_joint::type_id) {
		pe_status_joint *status = (pe_status_joint*)_status;
		int i; 
		if (!is_unused(status->partid)) {
			for(i=0;i<m_nParts && m_parts[i].id!=status->partid;i++);
			if (i>=m_nParts) return 0;
			status->idChildBody = m_joints[i = m_infos[i].iJoint].idbody;
		} else if (!is_unused(status->idChildBody)) {
			for(i=0;i<m_nJoints && m_joints[i].idbody!=status->idChildBody;i++);
			if (i>=m_nJoints) return 0;
		}	else
			return 0;
		ReadLock lock(m_lockJoints);
		status->flags = m_joints[i].flags;
		status->q = m_joints[i].prev_q;
		status->qext = m_joints[i].qext;
		status->dq = Ang3(m_joints[i].prev_dq);
		status->quat0 = m_joints[i].quat0;
		return 1;
	}

	if (_status->type==pe_status_dynamics::type_id) {
		int i; pe_status_dynamics *status = (pe_status_dynamics*)_status;
		if (!is_unused(status->partid))
			for(i=0; i<m_nParts && m_parts[i].id!=status->partid; i++);
		else i = status->ipart;
		if ((unsigned int)i>=(unsigned int)m_nParts) {
			status->mass = 0;
			if (m_body.M>0) {
				for(i=0,status->centerOfMass.zero(); i<m_nJoints; i++) {
					status->centerOfMass += m_joints[i].body.pos*m_joints[i].body.M;
					status->mass += m_joints[i].body.M;
				}
				status->centerOfMass /= status->mass;
			}	else
				status->centerOfMass = m_pos; 
			status->v = m_body.v;
			status->w = m_body.w;
			status->a = m_gravity; status->wa.zero();
			status->submergedFraction = m_submergedFraction;
			status->nContacts = m_nBodyContacts;
			for(i=0,status->energy=0; i<m_nJoints; i++)
				status->energy += m_joints[i].body.M*m_joints[i].body.v.len2() + m_joints[i].body.L*m_joints[i].body.w;
			status->energy *= 0.5f;
			return i;
		}

		ReadLock lock(m_lockJoints);
		RigidBody *pbody = &m_joints[i=m_infos[i].iJoint].body;
		status->v = m_joints[i].prev_v;
		status->w = m_joints[i].prev_w;
		status->a = m_gravity + m_joints[i].Fcollision*pbody->Minv;
		status->wa = pbody->Iinv*(m_joints[i].Tcollision - (status->w^m_joints[i].prev_qrot*(pbody->Ibody*(m_joints[i].prev_qrot*status->w))));
		status->centerOfMass = pbody->pos;
		status->submergedFraction = m_submergedFraction;
		status->mass = pbody->M;
		status->energy = (pbody->M*pbody->v.len2()+pbody->L*pbody->w)*0.5f;
		status->nContacts = m_joints[i].bHasExtContacts;
		return 1;
	}

	int res = CRigidEntity::GetStatus(_status);
	if (res) {
		if (_status->type==pe_status_awake::type_id && m_body.M==0)
			return 0;
		if (_status->type==pe_status_caps::type_id) {
			pe_status_caps *status = (pe_status_caps*)_status;
			status->bCanAlterOrientation = 1;
		}
	}
	return res;
}


int CArticulatedEntity::Action(pe_action *_action, int bThreadSafe)
{
	if (_action->type==pe_action_add_constraint::type_id)
		return CRigidEntity::Action(_action,bThreadSafe);

	ChangeRequest<pe_action> req(this,m_pWorld,_action,bThreadSafe);
	if (req.IsQueued())
		return 1;

	if (_action->type==pe_action_impulse::type_id) {
		pe_action_impulse *action = (pe_action_impulse*)_action;
		int i,j;
		if (m_bCollisionResp && action->impulse.len2()<sqr(m_body.M*0.04f) && action->iSource==0 && !(m_flags & aef_recorded_physics) 
				|| action->iSource==4)
			return 1;
		if (is_unused(action->ipart))
			for(i=0; i<m_nParts && m_parts[i].id!=action->partid; i++);
		else i = action->ipart;
		ENTITY_VALIDATE("CArticulatedEntity:Action(action_impulse)",action);

		box bbox; Vec3 posloc;
		if ((unsigned int)i>=(unsigned int)m_nParts) {
			if (is_unused(action->point)) 
				i = m_joints[0].iStartPart;
			else {
				for(i=0;i<m_nParts;i++) {
					m_parts[i].pPhysGeomProxy->pGeom->GetBBox(&bbox);
					posloc = (m_qrot*m_parts[i].q)*bbox.center + m_qrot*m_parts[i].pos + m_pos;
					posloc = bbox.Basis*((action->point-posloc)*(m_qrot*m_parts[i].q));
					for(j=0;j<3 && posloc[j]<bbox.size[j];j++);
					if (j==3) break;
				}
				if ((unsigned int)i>=(unsigned int)m_nParts) return 0;
			}
		}

		if ((unsigned int)i>=m_nParts || (unsigned int)(j=m_infos[i].iJoint)>=m_nJoints || m_joints[j].flags & joint_ignore_impulses)
			return 0;
		if (action->iSource==0) {
			if (action->impulse.len2()<sqr(m_joints[j].body.M*0.04f))
				return 1;
			/*if (m_flags & (pef_monitor_impulses | pef_log_impulses)) {
				EventPhysImpulse event;
				event.pEntity=this; event.pForeignData=m_pForeignData; event.iForeignData=m_iForeignData;
				event.ai = *action;
				if (!m_pWorld->OnEvent(m_flags,&event))
					return 1;
			}*/
			m_timeIdle = 0;
		}

		Vec3 P(ZERO),L(ZERO);
		if (!is_unused(action->impulse))
			P += action->impulse;
		if (!is_unused(action->angImpulse)) 
			L += action->angImpulse;
		else if (!is_unused(action->point)) {
			// make sure that the point is not too far from the part's bounding box
			m_parts[i].pPhysGeomProxy->pGeom->GetBBox(&bbox);
			posloc = (m_qrot*m_parts[i].q)*bbox.center + m_qrot*m_parts[i].pos + m_pos;
			posloc = bbox.Basis*((action->point-posloc)*(m_qrot*m_parts[i].q));
			if (fabs_tpl(posloc.x)<bbox.size.x*3 && fabs_tpl(posloc.y)<bbox.size.y*3 && fabs_tpl(posloc.z)<bbox.size.z*3) {
				// recompute body's com, since sometimes only parts are in sync with animation, not joint bodies
				Vec3 bodypos = m_parts[i].pos+m_pos-(m_parts[i].q*!m_infos[i].q0)*m_infos[i].pos0;
				L += action->point-bodypos ^ action->impulse;
			}
		}
		if (P.len2()>sqr(m_pWorld->m_vars.maxVel*m_body.M))
			P.normalize() *= m_pWorld->m_vars.maxVel*m_body.M;
		if (!(m_flags & aef_recorded_physics))
			m_joints[j].Pimpact += P, m_joints[j].Limpact += L;
		else {
			Matrix33 R = (Matrix33)(m_parts[i].q*!m_infos[i].q0*!m_joints[j].body.qfb);
			m_joints[j].prev_v=m_joints[j].body.v = (m_joints[j].body.P+=P)*m_joints[j].body.Minv;
			m_joints[j].prev_w=m_joints[j].body.w = (R*m_joints[j].body.Ibody_inv*R.T())*(m_joints[j].body.L+=L);
		}

		if (action->iSource!=1) {
			m_bAwake = 1; m_simTime = 0;
			for(i=0;i<m_nJoints;i++) 
			if (m_iSimClass==1) {
				m_iSimClass = 2;	m_pWorld->RepositionEntity(this, 2);
			}
		}

		CPhysicalEntity::Action(_action,1);

		return 1;
	}

	if (_action->type==pe_action_reset::type_id) {
		/*{ WriteLock lock(m_lockJoints);
			for(int idx=0;idx<m_nJoints;idx++) {
				m_joints[idx].q=Ang3(ZERO);
				if (m_joints[idx].bQuat0Changed)
					m_joints[idx].quat0.SetIdentity();
				m_joints[idx].dq.Set(0,0,0);
				m_joints[idx].body.v.Set(0,0,0); m_joints[idx].body.w.Set(0,0,0);
				m_joints[idx].body.P.Set(0,0,0); m_joints[idx].body.L.Set(0,0,0);
				m_joints[idx].prev_q.Set(0,0,0); m_joints[idx].prev_dq.zero();
				m_joints[idx].prev_v.zero(); m_joints[idx].prev_w.zero();
				m_joints[idx].Fcollision.zero(); m_joints[idx].Tcollision.zero();
				SyncBodyWithJoint(idx);
			}
		}*/
		ComputeBBox(m_BBoxNew);
		UpdatePosition(m_pWorld->RepositionEntity(this,1,m_BBoxNew));
		m_body.v.Set(0,0,0); m_body.w.Set(0,0,0);
		m_body.P.Set(0,0,0); m_body.L.Set(0,0,0);
		return CRigidEntity::Action(_action,1);
	}

	if (_action->type==pe_action_set_velocity::type_id) {
		pe_action_set_velocity *action = (pe_action_set_velocity*)_action;
		if (m_nJoints<=0)
			return 0;

		if (!is_unused(action->ipart) || !is_unused(action->partid)) {
			int i;
			if (!is_unused(action->ipart))
				i = action->ipart;
			else for(i=0;i<m_nParts && m_parts[i].id!=action->partid;i++);
			if ((unsigned int)i>=(unsigned int)m_nParts)
				return 0;
			i = m_infos[i].iJoint;
			if (!is_unused(action->v)) 
				m_joints[i].body.P = (m_joints[i].body.v=action->v)*m_joints[i].body.M;
			if (!is_unused(action->w))
				m_joints[i].body.L = m_joints[i].body.q*(m_joints[i].body.Ibody*(!m_joints[i].body.q*(m_joints[i].body.w=action->w)));
			if (m_joints[i].body.v.len2() > sqr(m_pWorld->m_vars.maxVel)) {
				m_joints[i].body.v.normalize() *= m_pWorld->m_vars.maxVel;
				m_joints[i].body.P = m_joints[i].body.v*m_body.M;
			}
			if (m_joints[i].body.w.len2() > sqr(m_maxw)) {
				m_joints[i].body.w.normalize() *= m_maxw;
				m_joints[i].body.L = m_joints[i].body.q*(m_joints[i].body.Ibody*(!m_joints[i].body.q*m_joints[i].body.w));
			}
			if ((m_joints[i].bAwake = m_joints[i].body.v.len2()+m_joints[i].body.w.len2()>0) && !m_bAwake) {
				if (m_body.Minv==0)
					m_body.v.x = 1E-10f;
				Awake();
			}
		}	else if (!is_unused(action->v)) {
			pe_status_dynamics sd;
			GetStatus(&sd);

			pe_action_impulse ai;
			ai.ipart = m_joints[0].iStartPart;
			ai.point = sd.centerOfMass;
			ai.impulse = (action->v-m_body.v)*m_body.M;
			Action(&ai);
		}
		return 1;
	}

	return CRigidEntity::Action(_action,1);
}


int CArticulatedEntity::GetPotentialColliders(CPhysicalEntity **&pentlist, float dt)
{
	pentlist = m_pCollEntList; return m_nCollEnts;
}

int CArticulatedEntity::CheckSelfCollision(int ipart0,int ipart1)
{
	return m_joints[m_infos[ipart0].iJoint].selfCollMask & getmask(ipart1);
}

RigidBody *CArticulatedEntity::GetRigidBody(int ipart,int bModify)
{
	return (unsigned int)ipart<(unsigned int)m_nParts ? &m_joints[m_infos[ipart].iJoint].body : &m_body;
}

void CArticulatedEntity::UpdatePosition(int bGridLocked)
{
	int i;
	WriteLock lock(m_lockUpdate); 

	m_pos = m_pNewCoords->pos; m_qrot = m_pNewCoords->q;
	for(i=0; i<m_nParts; i++) {
		m_parts[i].pos = m_parts[i].pNewCoords->pos; m_parts[i].q = m_parts[i].pNewCoords->q;
		m_parts[i].BBox[0] = m_parts[i].pNewCoords->BBox[0]; m_parts[i].BBox[1] = m_parts[i].pNewCoords->BBox[1];
	}
	m_BBox[0]=m_BBoxNew[0]; m_BBox[1]=m_BBoxNew[1];
	AtomicAdd(&m_pWorld->m_lockGrid,-bGridLocked);
}

void CArticulatedEntity::UpdateJointDyn()
{
	WriteLock lock(m_lockJoints); 
	for(int i=0;i<m_nJoints; i++) {
		m_joints[i].prev_q = m_joints[i].q;
		m_joints[i].prev_dq = m_joints[i].dq;
		m_joints[i].prev_pos = m_joints[i].body.pos;
		m_joints[i].prev_qrot = m_joints[i].body.q;
		m_joints[i].prev_v = m_joints[i].body.v;
		m_joints[i].prev_w = m_joints[i].body.w;
	}
}


void CArticulatedEntity::SyncWithHost(int bRecalcJoints, float time_interval)
{
	if (m_pHost) {
		if (m_pHost->m_iSimClass==7) {
			m_pHost->Release(); m_pHost=0; return;
		}
		int i,ipart;
		pe_status_pos sp;
		m_pHost->GetStatus(&sp);
		m_posPivot = sp.q*m_posHostPivot + sp.pos;
		pe_params_pos pp;
		pp.pos = m_posPivot - m_offsPivot;
		pp.q = m_pHost->m_qrot*m_qHostPivot;
		pp.bRecalcBounds = 0;
		SetParams(&pp,1);
		if (m_bInheritVel && time_interval>0) {
			pe_status_dynamics sd;
			m_pHost->GetStatus(&sd);
			float rdt = 1.0f/time_interval;
			m_acc = (sd.v-m_body.v)*rdt;
			m_wacc = (sd.w-m_body.w)*rdt;
			m_body.v = sd.v;
			m_body.w = sd.w;
		}
		if (bRecalcJoints)
			for(i=0;i<m_nJoints;i++) SyncBodyWithJoint(i);
		else for(i=0;i<m_nJoints;i++) {
			ipart = m_joints[i].iStartPart;
			m_joints[i].quat = m_parts[ipart].q*!m_infos[ipart].q0;
			m_joints[i].body.q = pp.q*m_joints[i].quat*!m_joints[i].body.qfb;
			m_joints[i].body.pos = pp.q*(m_parts[ipart].pos-m_joints[i].quat*m_infos[ipart].pos0)+m_pos;
		}
		ComputeBBox(m_BBoxNew);
		UpdatePosition(m_pWorld->RepositionEntity(this,1,m_BBoxNew));
	}
}


void CArticulatedEntity::UpdateJointRotationAxes(int idx)
{
	quaternionf q_parent = m_joints[idx].iParent>=0 ? m_joints[m_joints[idx].iParent].quat : m_qNew;
	SetBasisTFromMtx(m_joints[idx].rotaxes, Matrix33(q_parent*m_joints[idx].quat0));

	float angle,cosa,sina;
	angle = m_joints[idx].q[1]+m_joints[idx].qext[1]; cosa = cos_tpl(angle); sina = sin_tpl(angle);
	m_joints[idx].rotaxes[0] = m_joints[idx].rotaxes[0].GetRotated(m_joints[idx].rotaxes[1], cosa,sina); // rot x around y - pitch
	angle = m_joints[idx].q[2]+m_joints[idx].qext[2]; cosa = cos_tpl(angle); sina = sin_tpl(angle);
	m_joints[idx].rotaxes[0] = m_joints[idx].rotaxes[0].GetRotated(m_joints[idx].rotaxes[2], cosa,sina); // rot x around z - yaw
	m_joints[idx].rotaxes[1] = m_joints[idx].rotaxes[1].GetRotated(m_joints[idx].rotaxes[2], cosa,sina); // rot y around z
}

void CArticulatedEntity::CheckForGimbalLock(int idx)
{
	int i;
	m_joints[idx].flags &= ~(angle0_gimbal_locked*7);
	if (!(m_joints[idx].flags & angle0_locked*5) && sqr(m_joints[idx].rotaxes[0]*m_joints[idx].rotaxes[2])>0.999f*0.999f) {
		if (!(m_joints[idx].flags & angle0_locked*7) && cry_fabsf(m_joints[idx].limits[1][0]-m_joints[idx].limits[0][0])>10 &&
				fabsf(m_joints[idx].limits[1][1]-m_joints[idx].limits[0][1])>10 && fabsf(m_joints[idx].limits[1][2]-m_joints[idx].limits[0][2])>10)
		{ // if joint is 3dof w/o limits, just rotate its quat0 to avoid gimbal lock
			//m_joints[idx].quat0 *= quaternionf(g_PI/6,m_joints[idx].rotaxes[1]);
			m_joints[idx].quat0 *= Quat::CreateRotationAA((float)g_PI/6,Vec3(0,1,0));//(m_joints[idx].rotaxes[0]+m_joints[idx].rotaxes[1])*(1.0f/sqrt2));
			m_joints[idx].bQuat0Changed = 1;
			SyncJointWithBody(idx,3);
		} else {
			i = iszero((int)m_joints[idx].flags & angle0_limit_reached) << 1; // gimbal lock angle that already reached its limit, if this is the case
			m_joints[idx].flags |= angle0_gimbal_locked<<i;
			m_joints[idx].dq[i^2] += m_joints[idx].dq[i];
			m_joints[idx].dq[i] = 0;
			m_joints[idx].rotaxes[i] = m_joints[idx].rotaxes[i^2]*0.99f+m_joints[idx].rotaxes[1]*0.01f;
		}
	}
}

void CArticulatedEntity::SyncBodyWithJoint(int idx, int flags)
{
	Vec3 pos_parent,pivot,wpivot;
	quaternionf q_parent,q;
	Matrix33 R;
	int i;
	if (m_joints[idx].iParent>=0) {
		q_parent = m_joints[m_joints[idx].iParent].quat;
		pos_parent = m_joints[m_joints[idx].iParent].body.pos;
		pivot = pos_parent + q_parent*m_joints[idx].pivot[0];
	} else {
		q_parent = m_qNew; pos_parent = pivot = m_posPivot;
	}

	if (flags & 1) { // sync geometry
		UpdateJointRotationAxes(idx);

	//	m_joints[idx].quat = q_parent*m_joints[idx].quat0*quaternionf(m_joints[idx].q+m_joints[idx].qext);
		m_joints[idx].quat = q_parent*m_joints[idx].quat0*Quat::CreateRotationXYZ(m_joints[idx].q+m_joints[idx].qext);
		m_joints[idx].body.q = m_joints[idx].quat*!m_joints[idx].body.qfb;
		m_joints[idx].body.pos = pos_parent + q_parent*m_joints[idx].pivot[0] - m_joints[idx].quat*m_joints[idx].pivot[1];
		R = Matrix33(m_joints[idx].body.q);
		m_joints[idx].body.Iinv = R*m_joints[idx].body.Ibody_inv*R.T();
		m_joints[idx].I = R*m_joints[idx].body.Ibody*R.T();

		for(i=m_joints[idx].iStartPart; i<m_joints[idx].iStartPart+m_joints[idx].nParts; i++) {
			m_infos[i].q = m_joints[idx].quat*m_infos[i].q0;
			m_infos[i].pos = m_joints[idx].quat*m_infos[i].pos0+m_joints[idx].body.pos-m_posNew;
		}
	}

	if (flags & 2) { // sync velocities
		if (m_joints[idx].iParent>=0) {
			m_joints[idx].body.w = m_joints[m_joints[idx].iParent].body.w;
			m_joints[idx].body.v = m_joints[m_joints[idx].iParent].body.v + 
				(m_joints[m_joints[idx].iParent].body.w^m_joints[idx].body.pos-m_joints[m_joints[idx].iParent].body.pos);
		} else {
			m_joints[idx].body.v = m_body.v;
			if (m_bGrounded) {
				m_joints[idx].body.w = m_body.w;
				m_joints[idx].body.v += m_body.w^m_joints[idx].body.pos-m_posPivot;
			}	else m_joints[idx].body.w.Set(0,0,0);
		}

		for(i=0,wpivot.Set(0,0,0); i<3; i++)
			wpivot += m_joints[idx].rotaxes[i]*(m_joints[idx].dq[i]+m_joints[idx].dqext[i]*(m_bCollisionResp^1));
		m_joints[idx].body.v += wpivot^m_joints[idx].body.pos-pivot;
		m_joints[idx].body.w += wpivot;
		m_joints[idx].body.P = m_joints[idx].body.v*m_joints[idx].body.M;
		m_joints[idx].body.L = m_joints[idx].I*m_joints[idx].body.w;
	}
}

void CArticulatedEntity::SyncJointWithBody(int idx, int flags)
{
	if (flags & 1) { // sync angles
		quaternionf qparent = m_joints[idx].iParent>=0 ? m_joints[m_joints[idx].iParent].quat : m_qNew;
		m_joints[idx].quat = m_joints[idx].body.q*m_joints[idx].body.qfb;
		//(!(qparent*m_joints[idx].quat0)*m_joints[idx].quat).GetEulerAngles_XYZ(m_joints[idx].q); //EULER_IVO
		m_joints[idx].q = Ang3::GetAnglesXYZ( Matrix33(!(qparent*m_joints[idx].quat0)*m_joints[idx].quat) );
		m_joints[idx].q -= m_joints[idx].qext;
		UpdateJointRotationAxes(idx);
		CheckForGimbalLock(idx);
	}

	if (flags & 2) { // sync velocities
		Vec3 wrel = m_joints[idx].iParent>=0 ? m_joints[m_joints[idx].iParent].body.w : m_body.w;
		wrel = m_joints[idx].body.w - wrel;
		if (!(m_joints[idx].flags & (angle0_locked|angle0_gimbal_locked)*7)) {
			Matrix33 Basis_inv = GetMtxFromBasis(m_joints[idx].rotaxes);
			Basis_inv.Invert(); m_joints[idx].dq = wrel*Basis_inv;
		} else {
			int i,j,iAxes[3];
			for(i=j=0;i<3;i++) if (!(m_joints[idx].flags & (angle0_locked|angle0_gimbal_locked)<<i))
				iAxes[j++] = i;
			m_joints[idx].dq.Set(0,0,0);
			if (j==1)
				m_joints[idx].dq[iAxes[0]] = m_joints[idx].rotaxes[iAxes[0]]*wrel;
			else if (j==2) {
				if (!(m_joints[idx].flags & (angle0_locked|angle0_gimbal_locked)<<1)) {
					// only x and z axes can be nonorthogonal, so just project wrel on active axes in other cases
					m_joints[idx].dq[iAxes[0]] = m_joints[idx].rotaxes[iAxes[0]]*wrel;
					m_joints[idx].dq[iAxes[1]] = m_joints[idx].rotaxes[iAxes[1]]*wrel;
				} else {
					Vec3 axisz,axisy;
					axisz = (m_joints[idx].rotaxes[iAxes[0]]^m_joints[idx].rotaxes[iAxes[1]]).normalized();
					axisy = axisz ^ m_joints[idx].rotaxes[iAxes[0]];
					m_joints[idx].dq[iAxes[1]] = (axisy*wrel)/(axisy*m_joints[idx].rotaxes[iAxes[1]]);
					m_joints[idx].dq[iAxes[0]] = wrel*m_joints[idx].rotaxes[iAxes[0]] - 
						(m_joints[idx].rotaxes[iAxes[0]]*m_joints[idx].rotaxes[iAxes[1]])*m_joints[idx].dq[iAxes[1]];
				}
			}
		}
	}
}


float CArticulatedEntity::CalcEnergy(float time_interval)
{
	float E=0,vmax=0,Emax=m_body.M*sqr(m_pWorld->m_vars.maxVel)*2; 
	int i; Vec3 v;
	entity_contact *pContact;

	for(i=0; i<m_nJoints; i++) m_joints[i].ddq.zero();
	if (time_interval>0) for(pContact=m_pContactStart; pContact!=CONTACT_END(m_pContactStart); pContact=pContact->next) {
		v = m_joints[m_infos[pContact->ipart[0]].iJoint].ddq;
		m_joints[m_infos[pContact->ipart[0]].iJoint].ddq += 
			pContact->n*max(0.0f,max(0.0f,pContact->vreq*pContact->n-max(0.0f,pContact->vrel*pContact->n))-v*pContact->n);
		if (pContact->pbody[1]->Minv==0 && pContact->pent[1]->m_iSimClass>1) {
			RigidBody *pbody = pContact->pbody[1];
			vmax = max(vmax,(pbody->v + (pbody->w^pbody->pos-pContact->pt[1])).len2());
			Emax = m_body.M*sqr(10.0f)*2; // since we'll allow some extra energy, make sure we restrict the upper limit
		}
	}

	for(i=0; i<m_nJoints; i++)
		E += m_joints[i].body.M*(m_joints[i].body.v+m_joints[i].ddq).len2() + m_joints[i].body.L*m_joints[i].body.w;

	if (time_interval>0) {
		masktype constraint_mask;
		v.Set(0,0,0);
		for(i=0,constraint_mask=0; i<m_nColliders; constraint_mask |= m_pColliderConstraints[i++]);
		for(i=0; i<NMASKBITS && getmask(i)<=constraint_mask; i++) 
		if (constraint_mask & getmask(i) && (m_pConstraints[i].flags & contact_constraint_3dof || m_pConstraintInfos[i].flags & constraint_rope))	{
			v += m_pConstraints[i].n*max(0.0f,max(0.0f,m_pConstraints[i].vreq*m_pConstraints[i].n-max(0.0f,m_pConstraints[i].vrel*m_pConstraints[i].n))-
				m_pConstraints[i].n*v);
			if (m_pConstraints[i].pbody[1]->Minv==0 && m_pConstraints[i].pent[1]->m_iSimClass>1) {
				RigidBody *pbody = m_pConstraints[i].pbody[1];
				vmax = max(vmax,(pbody->v + (pbody->w^pbody->pos-m_pConstraints[i].pt[1])).len2());
			}
		}
		E += m_body.M*(v.len2()+vmax);
		return min_safe(E,Emax);
	}

	return E;
}


float CArticulatedEntity::GetDamping(float time_interval)
{
	float damping = CRigidEntity::GetDamping(time_interval);
	if (m_nBodyContacts>=m_nCollLyingMode)
		damping = min(1.0f-m_dampingLyingMode*time_interval, damping);
	return damping;
}


int CArticulatedEntity::IsAwake(int ipart)
{
	return (unsigned int)ipart<(unsigned int)m_nParts ? m_joints[m_infos[ipart].iJoint].bAwake : m_bAwake;
}


int CArticulatedEntity::GetUnprojAxis(int idx, Vec3 &axis)
{
	axis.Set(0,0,0);
	if (!(m_joints[idx].flags & (angle0_limit_reached|angle0_locked|angle0_gimbal_locked)*6)) // y and z axes are not locked
		return 1;
	else {
		int i; for(i=2; i>=0 && m_joints[idx].flags & (angle0_limit_reached|angle0_locked|angle0_gimbal_locked)<<i; i--);
		if (i>=0)	{
			axis = m_joints[idx].rotaxes[i]; // free axis from list: z y x
			return 1;
		}
	}
	return 0;
}


int CArticulatedEntity::StepJoint(int idx, float time_interval,int &bBounced,int bFlying)
{
	int i,j,idxpivot,ncont,itmax,curidx,sgq,bSelfColl,bFreeFall=isneg(-m_nBodyContacts)^1;
	float qlim[2],dq,curq,e,diff[2],tdiff, minEnergy = m_nBodyContacts>=m_nCollLyingMode ? m_EminLyingMode : m_Emin;
	Ang3 q;
	Vec3 n,ptsrc,ptdst,axis,axis0,pivot,prevpt;
	quaternionf qrot;
	Matrix33 R;
	geom_contact *pcontacts;
	geom_world_data gwd;
	intersection_params ip;
	box bbox;

	{ WriteLock lock(m_lockJoints); 
		m_joints[idx].prev_q = m_joints[idx].q;
		m_joints[idx].prev_dq = m_joints[idx].dq;
		m_joints[idx].prev_pos = m_joints[idx].body.pos;
		m_joints[idx].prev_qrot = m_joints[idx].body.q;
		m_joints[idx].prev_v = m_joints[idx].body.v;
		m_joints[idx].prev_w = m_joints[idx].body.w;
		m_joints[idx].bHasExtContacts = 0;
	}
	m_joints[idx].vSleep.zero();
	m_joints[idx].wSleep.zero();

	if (m_iSimTypeCur && m_bCollisionResp) {
		e = (m_joints[idx].body.v.len2() + (m_joints[idx].body.L*m_joints[idx].body.w)*m_joints[idx].body.Minv)*0.5f + m_joints[idx].body.Eunproj;
		m_bAwake += (m_joints[idx].bAwake = isneg(minEnergy-e));
		m_joints[idx].bAwake |= m_bFloating|m_bUsingUnproj|bFreeFall;
		if (!m_joints[idx].bAwake) {
			for(i=m_joints[idx].iParent; i>=0; i=m_joints[i].iParent)
				m_joints[idx].bAwake |= m_joints[i].bAwake;
		}

		if (m_joints[idx].bAwake) {
			m_joints[idx].body.Step(time_interval);
			if ((m_bGrounded|idx)==0)
				m_posNew = (m_posPivot = m_joints[0].body.pos)-m_offsPivot;
			SyncJointWithBody(idx,3);
		}
		//m_joints[idx].body.q.getmatrix(R); //Q2M_IVO
		R = Matrix33(m_joints[idx].body.q);
		m_joints[idx].I = R*m_joints[idx].body.Ibody*R.T();
		m_joints[idx].dv_body.zero(); m_joints[idx].dw_body.zero();

		m_joints[idx].flags &= ~(angle0_limit_reached*7|joint_dashpot_reached);
		q = m_joints[idx].q + m_joints[idx].qext;
		for(i=0;i<3;i++) if (!(m_joints[idx].flags & (angle0_locked|angle0_gimbal_locked)<<i)) {
			qlim[0] = m_joints[idx].limits[0][i]; qlim[1] = m_joints[idx].limits[1][i];
			if (isneg(qlim[0]-q[i]) + isneg(q[i]-qlim[1]) + isneg(qlim[1]-qlim[0]) < 2) {
				m_joints[idx].flags |= angle0_limit_reached<<i;
				diff[0] = q[i]-qlim[0]; tdiff = diff[0]-sgn(diff[0])*(2*g_PI);
				if (fabsf(tdiff)<fabsf(diff[0])) 
					diff[0] = tdiff;
				diff[1] = q[i]-qlim[1]; tdiff = diff[1]-sgn(diff[1])*(2*g_PI);
				if (fabsf(tdiff)<fabsf(diff[1]))
					diff[1] = tdiff;
				m_joints[idx].dq_limit[i] = diff[isneg(fabsf(diff[1])-fabsf(diff[0]))];
			}	else 
				m_joints[idx].flags |= joint_dashpot_reached & -isneg(min(fabs_tpl(qlim[0]-q[i]),fabs_tpl(qlim[1]-q[i]))-m_joints[idx].qdashpot[i]);
		}

		for(i=m_joints[idx].iStartPart; i<m_joints[idx].iStartPart+m_joints[idx].nParts; i++) {
			(m_infos[i].q = m_joints[idx].quat*m_infos[i].q0).Normalize();
			m_infos[i].pos = m_joints[idx].quat*m_infos[i].pos0+m_joints[idx].body.pos-m_posNew;
		}
	}	
	else 
	{
		for(i=0;i<3;i++) if (!(m_joints[idx].flags & (angle0_locked|angle0_gimbal_locked)<<i)) {
			if (fabsf(m_joints[idx].dq[i])<1E-5f && fabsf(m_joints[idx].q[i])<0.01f) 
				m_joints[idx].dq[i] = 0;
			qlim[0] = m_joints[idx].limits[0][i]; qlim[1] = m_joints[idx].limits[1][i];
			//qlim[1] += 2*g_PI*isneg(qlim[1]-qlim[0]);
			if (m_joints[idx].limits[0][i] > m_joints[idx].limits[1][i]) {
				sgq = sgn(m_joints[idx].q[i]+m_joints[idx].qext[i]);
				qlim[sgq+1>>1] += (sgq<<1)*g_PI;
			}
			m_joints[idx].flags &= ~joint_dashpot_reached;
			dq = m_joints[idx].dq[i]*time_interval;	sgq = sgn(dq);
			curq = m_joints[idx].q[i]+m_joints[idx].qext[i];
			if ((curq + dq - qlim[sgq+1>>1])*sgq > 0)	{	// we'll breach the limit that lies ahead along movement (ignore the other one)
				dq = qlim[sgq+1>>1]-curq+sgq*0.01f;
				dq = sgnnz(dq)*min(fabsf(dq),0.1f+(m_bCollisionResp^1)); // limit angle snapping in full simulation mode
				m_joints[idx].dq_limit[i] = m_joints[idx].dq[i];
				m_joints[idx].flags |= angle0_limit_reached<<i;
				m_joints[idx].dq_req[i] = -m_joints[idx].dq[i]*m_joints[idx].bounciness[i];
				bBounced++;
			} else if (sgq!=0) {
				m_joints[idx].flags &= ~(angle0_limit_reached<<i);
				// if angle passes through 0 during this step, make it snap to 0 and also clamp dq if it's small enough
				if (m_joints[idx].ks[i]!=0 && (m_joints[idx].q[i]+dq)*m_joints[idx].q[i]<0) {
					dq = -m_joints[idx].q[i]; 
					if (fabs_tpl(m_joints[idx].dq[i])<0.1f)
						m_joints[idx].dq[i] = 0;
				}
				m_joints[idx].flags |= joint_dashpot_reached & -isneg(fabs_tpl(curq-qlim[sgq+1>>1])-m_joints[idx].qdashpot[i]);
			}
			m_joints[idx].q[i] += min(fabsf(dq),1.2f)*sgn(dq);
			//while(m_joints[idx].q[i]>g_PI) m_joints[idx].q[i]-=2*g_PI;
			//while(m_joints[idx].q[i]<-g_PI) m_joints[idx].q[i]+=2*g_PI;
		}	else 
			m_joints[idx].dq[i] = 0;

		SyncBodyWithJoint(idx,1);
		m_joints[idx].body.w = m_joints[idx].body.Iinv*m_joints[idx].body.L;
		m_joints[idx].dv_body.zero(); m_joints[idx].dw_body.zero();
		/*m_joints[idx].body.Step(time_interval);
		m_joints[idx].dv_body = m_joints[idx].body.v;
		m_joints[idx].dw_body = m_joints[idx].body.w;
		SyncBodyWithJoint(idx);*/
		m_joints[idx].dv_body = m_joints[idx].body.v-m_joints[idx].dv_body;
		m_joints[idx].dw_body = m_joints[idx].body.w-m_joints[idx].dw_body;
		m_joints[idx].ddq.Set(0,0,0);

		e = (m_joints[idx].body.v.len2() + (m_joints[idx].body.L*m_joints[idx].body.w)*m_joints[idx].body.Minv)*0.5f;
		m_bAwake += (m_joints[idx].bAwake = isneg(minEnergy-e));
		m_joints[idx].bAwake |= m_bFloating;

		if (!m_joints[idx].bAwake && m_bCollisionResp) {
			for(i=m_joints[idx].iParent; i>=0; i=m_joints[i].iParent)
				m_joints[idx].bAwake |= m_joints[i].bAwake;
			if (!m_joints[idx].bAwake) {
				m_joints[idx].q = m_joints[idx].prev_q;
				m_joints[idx].dq.Set(0,0,0);
				SyncBodyWithJoint(idx);
				m_joints[idx].body.P.Set(0,0,0); m_joints[idx].body.L.Set(0,0,0);
				m_joints[idx].body.v.Set(0,0,0); m_joints[idx].body.w.Set(0,0,0);
				if (idx==0) {
					m_posNew = m_prev_pos; m_posPivot = m_posNew+m_offsPivot;
					m_body.v.zero();
				}
			}
		}

		CheckForGimbalLock(idx);
	}

	// force persistant contacts to lie in contact plane
	pivot = m_joints[idx].iParent<0 ? m_posPivot : 
		m_joints[m_joints[idx].iParent].body.pos + m_joints[m_joints[idx].iParent].quat*m_joints[idx].pivot[0];
	/*if (m_joints[idx].iContacts) {
		for(i=0; !(m_joints[idx].iContacts & getmask(i)); i++);
		n = m_pContacts[i].pbody[m_pContacts[i].iNormal]->q*m_pContacts[i].nloc;
		e = m_pWorld->m_vars.maxContactGap*0.5f;
		ptsrc = m_pContacts[i].pbody[0]->q*m_pContacts[i].ptloc[0] + m_pContacts[i].pbody[0]->pos;
		ptdst = m_pContacts[i].pbody[1]->q*m_pContacts[i].ptloc[1] + m_pContacts[i].pbody[1]->pos + n*e;
		if (idx==0)
			m_posPivot += n*(n*(ptdst-ptsrc)+e);
		else {
			axis = ptsrc-pivot ^ n*(n*(ptdst-ptsrc));
			for(i=0;i<3;i++) if (m_joints[idx].flags & (angle0_limit_reached|angle0_locked|angle0_gimbal_locked)<<i)
				axis -= m_joints[idx].rotaxes[i]*(axis*m_joints[idx].rotaxes[i]);
			axis.normalize();
			angle = RotatePointToPlane(ptsrc, axis,pivot, n,ptdst+n*e);
			if (angle<0.1f) {
				qrot = quaternionf(angle, axis);
				m_joints[idx].body.q = qrot*m_joints[idx].body.q; 
				m_joints[idx].body.pos = pivot + qrot*(m_joints[idx].body.pos-pivot);
			}
		}
	}*/

	if (m_bCheckCollisions) {// && m_joints[idx].bAwake) {
		// check for new contacts; unproject if necessary; register new contacts
		Vec3 sweep(0);
		if (/*m_iSimTypeCur==0 &&*/ bFlying) {
			ip.iUnprojectionMode = 0; ip.time_interval = time_interval*1.6f;
			gwd.v = m_body.v; gwd.w.zero();	sweep = m_body.v*time_interval;
			n = m_BBox[1]-m_BBox[0];
			ip.maxUnproj = max(max(n.x,n.y),n.z);
			ip.vrel_min = ip.maxUnproj/ip.time_interval*0.1f;
		} else if (m_iSimTypeCur==0 && m_joints[idx].nChildren==0 && (m_joints[idx].body.w.len2()>sqr(6) || m_joints[idx].body.v.len2()>sqr(4)) &&
			m_joints[idx].body.Minv>m_body.Minv*10.0f) 
		{
			ip.iUnprojectionMode = 1; ip.centerOfRotation = pivot;
			ip.time_interval = 0.6f;
			if (!GetUnprojAxis(idx,ip.axisOfRotation)) {
				ip.iUnprojectionMode = 0; ip.vrel_min = 1E10;
			}
		}	else {
			ip.iUnprojectionMode = 0; ip.vrel_min = 1E10; ip.bNoAreaContacts = true;
		}
		idxpivot = idx;	itmax = -1; 
		//ip.bStopAtFirstTri = /*m_timeStepFull>m_maxAllowedStep ||*/ m_joints[idx].body.M<m_body.M*0.2f && m_joints[idx].nChildren>0; 
		//if (m_joints[idx].iParent>=0)
		ip.ptOutsidePivot[0] = m_joints[idx].iParent>=0 ? pivot : m_joints[idx].body.pos;
		ncont = CheckForNewContacts(&gwd,&ip, itmax, sweep, m_joints[idx].iStartPart,m_joints[idx].nParts);
		pcontacts = ip.pGlobalContacts;
		WriteLockCond lock(*ip.plock,0); lock.SetActive(isneg(-ncont));
		/*if (ncont>0 && itmax<0 && ip.iUnprojectionMode==1) {
			// find parent that has good free axes
			axis0 = (pcontacts[0].center-ip.centerOfRotation^pcontacts[0].dir).normalized();
			for(i=3,idxpivot=m_joints[idxpivot].iParent; i>0 && idxpivot>0; i--,idxpivot=m_joints[idxpivot].iParent) {
				for(i=0,axis=axis0;i<3;i++) if (m_joints[idxpivot].flags & (angle0_limit_reached|angle0_locked|angle0_gimbal_locked)<<i)
					axis -= m_joints[idxpivot].rotaxes[i]*(axis*m_joints[idx].rotaxes[i]);
				if (axis.len2()>0.5f*0.5f) {
					ip.centerOfRotation = m_joints[idxpivot].body.pos + m_joints[idxpivot].quat*m_joints[idxpivot].pivot[1];
					ip.axisOfRotation = GetUnprojAxis(idxpivot);
					break;
				}
			}
			if (i*idxpivot>0)
				ncont = CheckForNewContacts(&gwd,&ip, itmax, m_joints[idx].iStartPart,m_joints[idx].nParts);
		}*/
		if (itmax>=0) {
			if (pcontacts[itmax].iUnprojMode) {
				//qrot = quaternionf(pcontacts[itmax].t, pcontacts[itmax].dir);
				qrot.SetRotationAA(pcontacts[itmax].t, pcontacts[itmax].dir);
				for(i=idxpivot;i<=idx;i++) { // note: update of parent will possibly invalidate its registered contacts, is it crucial?
					m_joints[i].body.q = qrot*m_joints[i].body.q; 
					m_joints[i].body.pos = ip.centerOfRotation + qrot*(m_joints[i].body.pos-ip.centerOfRotation);
					SyncJointWithBody(i,3);
				}
				if (!m_bCollisionResp) {
					for(i=0;i<3;i++) 
					if (!(m_joints[idxpivot].flags & (angle0_locked|angle0_gimbal_locked|angle0_limit_reached)<<i) && 
								m_joints[idxpivot].rotaxes[i]*pcontacts[itmax].dir*sgnnz(m_joints[idxpivot].dq[i])<-0.5f)
						m_joints[idxpivot].dq[i] = 0;
				}
			} else {
				axis = pcontacts[itmax].dir*pcontacts[itmax].t;
				for(i=0;i<m_nJoints;i++) m_joints[i].body.pos += axis;
				m_posPivot += axis; m_posNew += axis;	m_pos += axis;
			}
		}/*	else if (m_joints[idx].iContacts) {
			SyncJointWithBody(idx); SyncBodyWithJoint(idx,2);
		}*/


		if (m_bCollisionResp) {	// register new contacts
			for(j=m_joints[idx].iStartPart+m_joints[idx].nParts-1; j>=m_joints[idx].iStartPart; j--) {
				m_parts[j].pPhysGeomProxy->pGeom->GetBBox(&bbox);
				bbox.Basis *= Matrix33(!(m_qNew*m_infos[j].q));
				Vec3 sz = (bbox.size*bbox.Basis.Fabs())*m_parts[j].scale;
				m_infos[j].BBox[0]=m_infos[j].BBox[1] = m_posNew + m_qNew*(m_infos[j].pos + m_infos[j].q*bbox.center*m_parts[j].scale);
				m_infos[j].BBox[0] -= sz; m_infos[j].BBox[1] += sz;
			}	j++;
			if (m_nParts>3)
				m_parts[j].minContactDist = max(max(bbox.size.x,bbox.size.y),bbox.size.z)*m_parts[j].scale*0.4f;

			for(i=0;i<ncont;i++) if (pcontacts[i].vel==0) { 
				bSelfColl = g_CurColliders[i]==this;
				e = sqr(m_parts[m_joints[idx].iStartPart].minContactDist);
				if (bSelfColl)
					e *= sqr(1.5f);
				j = 0;
				if (idx>0)
					for(;j<pcontacts[i].nborderpt && (pcontacts[i].ptborder[j]-pcontacts[i].ptborder[0]).len2()<e; j++);
				if (j==pcontacts[i].nborderpt/*idx>0*/) // register border center only for penetration contacts in marginal joints
					RegisterContactPoint(i, pcontacts[i].center, pcontacts, pcontacts[i].iPrim[0],pcontacts[i].iFeature[0], 
						pcontacts[i].iPrim[1],pcontacts[i].iFeature[1], contact_new|contact_last, pcontacts[i].t);
				else {
					//m_parts[m_joints[idx].iStartPart].pPhysGeomProxy->pGeom->GetBBox(&bbox);
					//e = sqr(max(max(bbox.size.x,bbox.size.y),bbox.size.z)*0.2f);
					sgq = contact_last;
					for(j=0,prevpt.zero(); j<pcontacts[i].nborderpt; j++)	if ((pcontacts[i].ptborder[j]-prevpt).len2()>e) {
						RegisterContactPoint(i, pcontacts[i].ptborder[j], pcontacts, pcontacts[i].iPrim[0],pcontacts[i].iFeature[0], 
							pcontacts[i].iPrim[1],pcontacts[i].iFeature[1], contact_new|sgq, pcontacts[i].t);
						prevpt = pcontacts[i].ptborder[j]; sgq = 0;
					}
				}
				if (!bSelfColl)
					m_maxPenetrationCur = max(m_maxPenetrationCur,(float)pcontacts[i].t);
			} else if (pcontacts[i].parea) { 
				// register only points that are sufficiently far away from each other
				//m_parts[m_joints[idx].iStartPart].pPhysGeomProxy->pGeom->GetBBox(&bbox);
				e = sqr(max(max(bbox.size.x,bbox.size.y),bbox.size.z)*0.8f*(iszero(idx)^1)); prevpt.zero();
				sgq = contact_last;
				for(j=0;j<pcontacts[i].parea->npt;j++) if ((pcontacts[i].parea->pt[j]-prevpt).len2()>e) {
					RegisterContactPoint(i, pcontacts[i].parea->pt[j], pcontacts, pcontacts[i].parea->piPrim[0][j], pcontacts[i].parea->piFeature[0][j], 
						pcontacts[i].parea->piPrim[1][j],pcontacts[i].parea->piFeature[1][j], contact_new|sgq);
					prevpt = pcontacts[i].parea->pt[j];	sgq = 0;
				}
			} else
				RegisterContactPoint(i, pcontacts[i].pt, pcontacts, pcontacts[i].iPrim[0],pcontacts[i].iFeature[0], 
					pcontacts[i].iPrim[1],pcontacts[i].iFeature[1], contact_new|contact_last);
		}
		CleanupAfterContactsCheck();
	}

	// iterate children
	for(i=0,curidx=idx+1; i<m_joints[idx].nChildren; i++)
		curidx = StepJoint(curidx, time_interval,bBounced,bFlying);
	return curidx;
}


float CArticulatedEntity::GetMaxTimeStep(float time_interval)
{
	return CRigidEntity::GetMaxTimeStep(time_interval);
}

int CArticulatedEntity::Step(float time_interval)
{
	int i,j,bBounced=0;
	Vec3 Y_vec[2]; vectornf Y(6,Y_vec[0]);
	float Iabuf[39]; matrixf Ia(6,6,0,_align16(Iabuf)); 
	Vec3 Za_vec[2]; vectornf Za(6,Za_vec[0]);
	Vec3 dv,dw;
	Vec3 gravity;
	int bWasAwake = m_bAwake;
	float maxPenetrationPrev = m_maxPenetrationCur;
	box bbox;
	pe_params_buoyancy pb[4];
	if (m_nBodyContacts>=m_nCollLyingMode) {
		gravity = m_gravityLyingMode;
		m_iSimTypeCur = m_iSimTypeLyingMode;
	} else {
		gravity = m_nBodyContacts ? m_gravity : m_gravityFreefall;
		m_iSimTypeCur = m_iSimType | m_bFloating;
	}

	for(i=m_nColliders-1;i>=0;i--)
		if (m_pColliders[i]->m_iSimClass==7)
			RemoveCollider(m_pColliders[i]);
	if (m_pHost && m_pHost->m_iSimClass==7)
		m_pHost->Release(), m_pHost=0;
	if (m_bUpdateBodies & m_iSimTypeCur)
		for(i=0;i<m_nJoints;i++)
			SyncBodyWithJoint(i);
	m_bUpdateBodies = 0;
	m_iLastLog = max(1,m_iLastLog);

	if (m_timeStepPerformed > m_timeStepFull-0.001f || m_flags & pef_disabled)
		return 1;
	m_timeStepPerformed += time_interval;
	m_lastTimeStep = time_interval;
	m_dampingEx = 0;

	SyncWithHost(0,time_interval);

	if (m_body.M==0 || m_flags & aef_recorded_physics) {
		if (!m_pHost) {
			for(dv.x=0,i=0;i<m_nJoints;i++) 
				dv.x += m_joints[i].body.v.len2()+m_joints[i].body.w.len2();
			if (m_bAwake = dv.x>0) for(i=0;i<m_nJoints;i++) {
				m_joints[i].body.Step(time_interval);
				SyncJointWithBody(i,3);
			}
			if (m_pStructure && m_pStructure->autoDetachmentDist>0) for(i=m_pStructure->nJoints-1;i>=0;i--)
				if ((m_joints[m_infos[m_pStructure->pJoints[i].ipart[0]].iJoint].body.pos-m_pos).len2()>sqr(m_pStructure->autoDetachmentDist)) {
					if (i!=m_pStructure->nJoints-1)
						m_pStructure->pJoints[i] = m_pStructure->pJoints[m_pStructure->nJoints-1];
					--m_pStructure->nJoints;
					m_pWorld->MarkEntityAsDeforming(this);
				}
		}
		m_nCollEnts = m_pWorld->GetEntitiesAround(m_BBox[0],m_BBox[1], m_pCollEntList, 
			ent_sleeping_rigid|ent_rigid|ent_living|ent_independent|ent_triggers, this);
		for(i=0;i<m_nCollEnts;i++)
			m_pCollEntList[i]->Awake();
		return 1;
	}
	if (!m_bAwake && !m_bCheckCollisions || m_nRoots>1 || (!m_bCheckCollisions && !m_bGrounded) || m_flags & aef_recorded_physics)
		return 1;

	FUNCTION_PROFILER( GetISystem(),PROFILE_PHYSICS );
	PHYS_ENTITY_PROFILER

	ComputeBBox(m_BBoxNew);
	if (m_bCheckCollisions) {
		Vec3 sz = m_BBoxNew[1]-m_BBoxNew[0];
		float szmax = max(max(sz.x,sz.y),sz.z)*0.3f;
		if (m_body.v.len2()*sqr(time_interval) > szmax)
			szmax = m_body.v.len()*time_interval;
		sz.Set(szmax,szmax,szmax);
		m_nCollEnts = m_pWorld->GetEntitiesAround(m_BBox[0]-sz,m_BBox[1]+sz, m_pCollEntList, 
			ent_terrain|ent_static|ent_sleeping_rigid|ent_rigid|ent_living|ent_independent|ent_sort_by_mass|ent_triggers, this);

		for(i=0;i<m_nCollEnts;i++) if (m_pCollEntList[i]->m_iSimClass>2)
			if (m_pCollEntList[i]->GetType()!=PE_ARTICULATED) {
				m_pCollEntList[i]->Awake();
				if (!m_pWorld->m_vars.bMultiplayer || m_pCollEntList[i]->m_iSimClass!=3)
					m_pWorld->ScheduleForStep(m_pCollEntList[i]),m_nNonRigidNeighbours++;
			} else if (m_iSimClass==2 && m_body.v.len2()>sqr(5.0f))
				FakeRayCollision(m_pCollEntList[i], time_interval);

		if (m_iSimClass<=2) {
			for(i=j=0;i<m_nCollEnts;i++) 
			if (m_pCollEntList[i]==this || m_pCollEntList[i]->m_iSimClass<=2 && (m_pCollEntList[i]->m_iGroup==m_iGroup && m_pCollEntList[i]->m_bMoved || 
					!m_pCollEntList[i]->IsAwake() || m_pWorld->m_pGroupNums[m_pCollEntList[i]->m_iGroup]<m_pWorld->m_pGroupNums[m_iGroup]))
				m_pCollEntList[j++] = m_pCollEntList[i];
			m_nCollEnts = j;
		} else
			m_nCollEnts = 0;
	} else 
		m_nCollEnts = 0;

	for(i=m_bFastLimbs=0; i<m_nJoints; i++) 
		m_bFastLimbs |= isneg(sqr(0.9f)-m_joints[i].body.w.len2()*sqr(time_interval));
	/*{
		m_parts[m_joints[i].iStartPart].pPhysGeomProxy->pGeom->GetBBox(&bbox);
		r = sqr(max(max(bbox.size.x,bbox.size.y),bbox.size.z)*m_parts[m_joints[i].iStartPart].scale);
		m_bFastLimbs |= isneg(r-(m_joints[i].body.v.len2()+m_joints[i].body.w.len2()*r)*sqr(time_interval));
	}*/
	m_iSimTypeCur &= m_bFastLimbs^1;
	m_iSimTypeCur |= m_iSimTypeOverride;

	m_qNew.SetIdentity();
	if (m_bPartPosForced) 
		for(i=0;i<m_nJoints;i++) {
			j = m_joints[i].iStartPart;
			m_joints[i].quat = m_qrot*m_parts[j].q*!m_infos[j].q0;
			m_joints[i].body.pos = m_parts[j].pos-m_joints[i].quat*m_infos[j].pos0+m_pos;
			m_joints[i].body.q = m_joints[i].quat*!m_joints[i].body.qfb;
			SyncJointWithBody(i);
			m_joints[i].qext += m_joints[i].q;
			m_joints[i].q.Set(0,0,0);
			for (j=0;j<3;j++) if (!(m_joints[i].flags & angle0_locked<<j) && 
					isneg(m_joints[i].limits[0][j]-m_joints[i].qext[j]) + isneg(m_joints[i].qext[j]-m_joints[i].limits[1][j]) + 
					isneg(m_joints[i].limits[1][j]-m_joints[i].limits[1][j]) < 2) 
			{	// qext violates limits; adjust the limits
				float diff[2];
				diff[0] = m_joints[i].limits[0][j]-m_joints[i].qext[j];
				diff[1] = m_joints[i].limits[1][j]-m_joints[i].qext[j];
				diff[0] -= (int)(diff[0]/(2*g_PI))*2*g_PI;
				diff[1] -= (int)(diff[1]/(2*g_PI))*2*g_PI;
				m_joints[i].limits[isneg(fabs_tpl(diff[1])-fabs_tpl(diff[0]))][j] = m_joints[i].qext[j];
			}
		}
	m_bPartPosForced = 0;
	m_prev_pos = m_pos;
	m_prev_vel = m_body.v;
	if (!m_bGrounded)
		if (m_iSimTypeCur==0)
			m_posPivot += m_body.v*time_interval;
		else
			m_posPivot = m_joints[0].body.pos;
	m_body.pos = m_posPivot;
	m_posNew = m_posPivot - m_offsPivot;
	m_bAwake = isneg(m_simTime-2.5f) | (iszero(m_nBodyContacts) & (m_bGrounded^1) & (m_bFloating^1));
	m_simTime += time_interval;
	m_simTimeAux += time_interval;
	m_maxPenetrationCur = 0;
	StepJoint(0, time_interval, bBounced, iszero(m_nBodyContacts));
	m_bAwake = isneg(-m_bAwake);
	m_nBodyContacts = m_nDynContacts = 0;

	if (m_bCollisionResp)	{
		VerifyExistingContacts(m_pWorld->m_vars.maxContactGap);
		for(i=0;i<m_nJoints;i++) 
			m_joints[i].body.P = (m_joints[i].body.v+=gravity*time_interval)*m_joints[i].body.M;
		/*m_joints[i].body.w -= m_joints[i].dw_body;
			m_joints[i].body.v -= m_joints[i].dv_body-gravity*time_interval;
			m_joints[i].body.P = m_joints[i].body.v*m_joints[i].body.M;
			m_joints[i].body.L = m_joints[i].I*m_joints[i].body.w;
		}*/

		ComputeBBox(m_BBoxNew);
		UpdatePosition(m_pWorld->RepositionEntity(this,1,m_BBoxNew));
		gravity = m_gravity;
		i = PostStepNotify(time_interval,pb,sizeof(pb)/sizeof(pb[0]));
		ApplyBuoyancy(time_interval,m_gravityFreefall,pb,i);
		if ((m_gravity-gravity).len2()>0)
			m_gravityLyingMode = m_gravity;
		for(i=0;i<m_nJoints;i++) if (m_joints[i].Pimpact.len2()+m_joints[i].Limpact.len2()>0) {
			m_joints[i].body.P += m_joints[i].Pimpact; 
			m_joints[i].body.L += m_joints[i].Limpact; 
			m_joints[i].body.v = m_joints[i].body.P*m_joints[i].body.Minv;
			m_joints[i].body.w = m_joints[i].body.Iinv*m_joints[i].body.L;
			m_joints[i].Pimpact.zero(); m_joints[i].Limpact.zero();
		}
		m_bAwake = bWasAwake;
		m_bSteppedBack = 0;

		// do not request step back if we were in deep penetration state initially
		i = m_iSimTypeCur | isneg(m_maxPenetrationCur-0.07f) | isneg(0.07f-maxPenetrationPrev);
		if (!i)
			m_simTimeAux = 0;
		return i | isneg(3-m_nStepBackCount);
	}

	if (!m_joints[0].fs)
		for(i=0;i<m_nJoints;i++) {
			m_joints[i].fs = (featherstone_data*)_align16(m_joints[i].fsbuf = new char[sizeof(featherstone_data)+16]);
			m_joints[i].fs->Q.zero(); m_joints[i].fs->Ya_vec[0].zero(); m_joints[i].fs->Ya_vec[1].zero();
			IdentityBasis((Vec3*)m_joints[i].fs->qinv);
		}

	if (bBounced && m_bIaReady) {	// apply bounce impulses at joints that reached their limits
		bBounced=0; CollectPendingImpulses(0,bBounced);
		if (bBounced) {
			PropagateImpulses(dv = m_M0inv*-m_Ya_vec[0]);
			if (!m_bGrounded)
				m_body.v += dv;
		}
	}

	if (m_simTime>10) {
		for(i=0;i<m_nJoints;i++) {
			m_joints[i].q=Ang3(ZERO); 
			m_joints[i].dq.zero();
		}
		m_bAwake = 0;
	}

	if (!m_bAwake)
		return 1;

	CalcBodyIa(0, Ia);
	m_M0inv = GetMtxStrided<float,6,1>(Ia.data+3);
	Matrix33 M0host; M0host.SetZero();
	if (m_bGrounded) {
		if (m_pHost && m_bExertImpulse) {
			float Ihost_inv[6][6]; // TODO: support full spatial matrices if the host provides them
			memset(Ihost_inv, 0, sizeof(float)*36);
			m_pHost->GetSpatialContactMatrix(m_posPivot, -1, Ihost_inv);
			M0host = GetMtxStrided<float,6,1>(&Ihost_inv[3][0]);
		}
		if (M0host.IsZero())
			m_M0inv.SetZero();
		else {
			//(m_M0inv += M0host.invert()).invert();
			M0host.Invert(); (m_M0inv+=M0host).Invert();
		} 
	} else 
		m_M0inv.Invert();

	m_bIaReady = 1;
	Za.zero(); CalcBodyZa(0, time_interval,Za);
	CalcBodiesIinv(m_bCollisionResp);

	for(i=0;i<m_nJoints;i++) { // apply external impulses
		m_joints[i].Pext += m_joints[i].Pimpact;	m_joints[i].Lext += m_joints[i].Limpact;
		m_joints[i].Pimpact.zero();	m_joints[i].Limpact.zero();
	}
	CollectPendingImpulses(0,bBounced);

	dv = m_M0inv*(-Za_vec[0]-m_Ya_vec[0]); dw.zero(); 
	if (m_bGrounded)
		dv += m_acc*time_interval; dw += m_wacc*time_interval;
	CalcVelocityChanges(time_interval, dv,dw);
	if (!m_bGrounded)	{
		m_body.v += dv; 
		m_body.w.zero();
	}
	for(i=0;i<m_nJoints;i++)
		SyncBodyWithJoint(i,3);	// synchronize body geometry as well as dynamics to accomodate potential changes in joint limits

	if (m_bGrounded && m_pHost && m_bExertImpulse) {
		pe_action_impulse ai;
		ai.impulse = -Za_vec[0]-m_Ya_vec[0];
		ai.point = m_posPivot;
		m_pHost->Action(&ai,1);
	}

	ComputeBBox(m_BBoxNew);
	UpdatePosition(m_pWorld->RepositionEntity(this,1,m_BBoxNew));
	UpdateJointDyn();
	gravity = m_gravity;
	PostStepNotify(time_interval,pb,sizeof(pb)/sizeof(pb[0]));
	if ((m_gravity-gravity).len2()>0)
		m_gravityLyingMode = m_gravity;

	return isneg(m_timeStepFull-m_timeStepPerformed-0.001f);
}


void CArticulatedEntity::StepBack(float time_interval)
{
	if (m_simTime>0) {
		Matrix33 R;
		m_posNew = m_prev_pos;
		m_posPivot = m_posNew+m_offsPivot;
		m_body.P = (m_body.v=m_prev_vel)*m_body.M;

		for(int i=0;i<m_nJoints;i++) {
			m_joints[i].q = m_joints[i].prev_q;
			m_joints[i].dq = m_joints[i].prev_dq;
			m_joints[i].body.pos = m_joints[i].prev_pos;
			m_joints[i].body.q = m_joints[i].prev_qrot;
			m_joints[i].body.v = m_joints[i].prev_v;
			m_joints[i].body.w = m_joints[i].prev_w;

			m_joints[i].quat = m_joints[i].body.q*m_joints[i].body.qfb;
			R = Matrix33(m_joints[i].body.q);
			m_joints[i].I = R*m_joints[i].body.Ibody*R.T();
			m_joints[i].body.Iinv = R*m_joints[i].body.Ibody_inv*R.T();
			m_joints[i].body.P = m_joints[i].body.v*m_joints[i].body.M; 
			m_joints[i].body.L = m_joints[i].I*m_joints[i].body.w;

			UpdateJointRotationAxes(i);
			for(int j=m_joints[i].iStartPart; j<m_joints[i].iStartPart+m_joints[i].nParts; j++) {
				m_infos[j].q = m_joints[i].quat*m_infos[j].q0;
				m_infos[j].pos = m_joints[i].quat*m_infos[j].pos0+m_joints[i].body.pos-m_posNew;
			}
			//SyncBodyWithJoint(i);
		}
		m_bSteppedBack = 1;

		//UpdateContactsAfterStepBack(time_interval);
		ComputeBBox(m_BBoxNew);
		UpdatePosition(m_pWorld->RepositionEntity(this,1,m_BBoxNew));
	}
}


void CArticulatedEntity::JointListUpdated()
{
	int i,j;
	for(i=0;i<m_nParts;i++) {
		for(j=0;j<m_nJoints && m_joints[j].idbody!=m_infos[i].idbody;j++);
		m_infos[i].iJoint = j;
	}
	for(i=0;i<m_nJoints;i++)
		m_joints[i].iLevel = m_joints[i].iParent>=0 ? m_joints[m_joints[i].iParent].iLevel+1 : 0;
}


void CArticulatedEntity::BreakableConstraintsUpdated()
{
	CRigidEntity::BreakableConstraintsUpdated();
	for(int i=0;i<m_nParts;i++)
		m_parts[i].flags |= geom_monitor_contacts;
}


int CArticulatedEntity::GetStateSnapshot(CStream &stm,float time_back,int flags)
{
	stm.WriteNumberInBits(SNAPSHOT_VERSION,4);
	stm.Write((unsigned char)m_nJoints);
	stm.Write(m_pos);
	stm.Write(m_body.v);
	stm.Write(m_bAwake!=0);
	int i = m_iSimClass-1;
	stm.WriteNumberInBits(i,2);

#ifdef DEBUG_BONES_SYNC
	stm.WriteBits((uint8*)&m_qrot,sizeof(m_qrot)*8);
	for(i=0;i<m_nParts;i++) {
		stm.Write(m_parts[i].pos);
		stm.WriteBits((uint8*)&m_parts[i].q,sizeof(m_parts[i].q)*8);
	}
	return 1;
#endif

	if (m_bPartPosForced) for(i=0;i<m_nJoints;i++) {
		int j = m_joints[i].iStartPart;
		m_joints[i].quat = m_qrot*m_parts[j].q*!m_infos[j].q0;
		m_joints[i].body.pos = m_parts[j].pos-m_joints[i].quat*m_infos[j].pos0+m_pos;
		m_joints[i].body.q = m_joints[i].quat*!m_joints[i].body.qfb;
	}

	for(i=0;i<m_nJoints;i++) {
		stm.Write(m_joints[i].bAwake!=0);
		stm.Write((Vec3)m_joints[i].q);
		stm.Write(m_joints[i].dq);

		stm.Write((Vec3)m_joints[i].qext);
		stm.Write(m_joints[i].body.pos);
		stm.WriteBits((uint8*)&m_joints[i].body.q,sizeof(m_joints[i].body.q)*8);
		if (m_joints[i].bQuat0Changed) {
			stm.Write(true);
			stm.WriteBits((uint8*)&m_joints[i].quat0, sizeof(m_joints[i].quat0)*8);
		} else stm.Write(false);
		if (m_joints[i].body.P.len2()+m_joints[i].body.L.len2()>0) {
			stm.Write(true); 
			stm.Write(m_joints[i].body.P);
			stm.Write(m_joints[i].body.L);
		}	else stm.Write(false);
	}

	WriteContacts(stm,flags);

	return 1;
}

int CArticulatedEntity::SetStateFromSnapshot(CStream &stm, int flags)
{
	int i=0,j,ver=0; 
	bool bnz;
	Matrix33 R;
	unsigned char nJoints;

	stm.ReadNumberInBits(ver,4);
	if (ver!=SNAPSHOT_VERSION)
		return 0;
	stm.Read(nJoints);
	if (nJoints!=m_nJoints)
		return 0;

	if (!(flags & ssf_no_update)) {
		stm.Read(m_posNew);
		stm.Read(m_body.v);
		stm.Read(bnz); m_bAwake = bnz ? 1:0;
		stm.ReadNumberInBits(i, 2); m_iSimClass=i+1;

#ifdef DEBUG_BONES_SYNC
stm.ReadBits((uint8*)&m_qrot,sizeof(m_qrot)*8);
for(i=0;i<m_nParts;i++) {
	stm.Read(m_parts[i].pos);
	stm.ReadBits((uint8*)&m_parts[i].q,sizeof(m_parts[i].q)*8);
}
#else
		for(i=0;i<m_nJoints;i++) {
			stm.Read(bnz);
			m_joints[i].bAwake = bnz ? 1:0;
			stm.Read(m_joints[i].q);
			stm.Read(m_joints[i].dq);
			//SyncBodyWithJoint(i);

			stm.Read(m_joints[i].qext);
			stm.Read(m_joints[i].body.pos);
			stm.ReadBits((uint8*)&m_joints[i].body.q,sizeof(m_joints[i].body.q)*8);
			stm.Read(bnz); if (bnz)	{
				stm.ReadBits((uint8*)&m_joints[i].quat0,sizeof(m_joints[i].quat0)*8);
				m_joints[i].bQuat0Changed = 1;
			} else m_joints[i].bQuat0Changed = 0;
			stm.Read(bnz); if (bnz)	{
				stm.Read(m_joints[i].body.P);
				stm.Read(m_joints[i].body.L);
			}	else {
				m_joints[i].body.P.zero();
				m_joints[i].body.L.zero();
			}
			m_body.UpdateState();

			UpdateJointRotationAxes(i);
			m_joints[i].quat = m_joints[i].body.q*m_joints[i].body.qfb;
			//m_joints[i].body.q.getmatrix(R); //Q2M_IVO
			R = Matrix33(m_joints[i].body.q);
			m_joints[i].I = R*m_joints[i].body.Ibody*R.T();

			for(j=m_joints[i].iStartPart; j<m_joints[i].iStartPart+m_joints[i].nParts; j++) {
				m_infos[j].q = m_joints[i].quat*m_infos[j].q0;
				m_infos[j].pos = m_joints[i].quat*m_infos[j].pos0+m_joints[i].body.pos-m_posNew;
			}
		}
#endif

		m_bUpdateBodies = 0;
		ComputeBBox(m_BBoxNew);
		UpdatePosition(m_pWorld->RepositionEntity(this,1,m_BBoxNew));
	} else {
		masktype dummy;
		stm.Seek(stm.GetReadPos()+2*sizeof(Vec3)*8+3);
#ifdef DEBUG_BONES_SYNC
stm.Seek(stm.GetReadPos()+sizeof(quaternionf)*8+m_nParts*(sizeof(quaternionf)+sizeof(Vec3))*8);
#else
		for(i=0;i<m_nJoints;i++) {
			stm.Seek(stm.GetReadPos()+1+4*sizeof(Vec3)*8+sizeof(quaternionf)*8);
			stm.Read(bnz); if (bnz) 
				stm.Seek(stm.GetReadPos()+sizeof(quaternionf)*8);
			stm.Read(bnz); if (bnz) 
				stm.Seek(stm.GetReadPos()+2*sizeof(Vec3)*8);
			ReadPacked(stm,dummy);
		}
#endif
	}

#ifndef DEBUG_BONES_SYNC
	ReadContacts(stm,flags);
#endif

	return 1;
}


int CArticulatedEntity::GetStateSnapshot(TSerialize ser, float time_back, int flags)
{
	bool awake = (m_bAwake != 0);
	ser.Value("awake", awake, 'bool');
	ser.Value("pos", m_pos, 'wrld'); 
	//ser.Value("rot", rot, 'ori1');
	//ser.Value("vel", m_body.v, NSerPolicy::AC_Velocity(20, 10));

	if (ser.GetSerializationTarget() != eST_Network) {
		ser.Value("vel", m_body.v, 'pAVl');
		ser.Value("numjoints", m_nJoints);
		for(int i=0;i<m_nJoints;i++)
		{
			ser.BeginGroup("joint");
			
			ser.Value("q", (Vec3&)m_joints[i].q);
			if (m_bAwake)
				ser.Value("dq", m_joints[i].dq);
			ser.Value("qext", (Vec3&)m_joints[i].qext);
			ser.Value("q0changed", m_joints[i].bQuat0Changed);
			if (m_joints[i].bQuat0Changed)
				ser.Value("quat0", m_joints[i].quat0);

			ser.EndGroup();
		}
		ser.Value("offsPivot", m_offsPivot);
	}
		
 	return 1;
}

int CArticulatedEntity::SetStateFromSnapshot(TSerialize ser, int flags)
{
	if (ser.GetSerializationTarget() == eST_Network)
	{
		bool awake;
		ser.Value("awake", awake, 'bool');
		ser.Value("pos", m_pos, 'wrld');
		//ser.Value("vel", m_body.v, NSerPolicy::AC_Velocity(20, 10));
		m_bAwake=awake;

		if (ser.IsReading())
		{
			m_iSimClass = 1+m_bAwake;

			ComputeBBox(m_BBoxNew);
			if (m_pWorld)
				UpdatePosition(m_pWorld->RepositionEntity(this,3,m_BBoxNew));

			m_posPivot = m_pos+m_offsPivot;
			if (m_nRoots==1) for(int i=0;i<m_nJoints;i++)
				SyncBodyWithJoint(i);
		}
	}
	else
	{
		bool awake = false;
		ser.Value("awake", awake, 'bool');
		m_bAwake = awake? 1 : 0;
		ser.Value("pos", m_pos, 'wrld'); 
		//if (m_bAwake)
			ser.Value("vel", m_body.v, 'pAVl');
		//else m_body.v.zero();
	}

	if (ser.GetSerializationTarget() != eST_Network)
	{
		int nJoints;
		ser.Value("numjoints", nJoints);
		if (nJoints!=m_nJoints)
			return 0;
		m_iSimClass = 1+m_bAwake;

		for(int i=0;i<m_nJoints;i++)
		{
			ser.BeginGroup( "joint" );

			ser.Value("q", (Vec3&)m_joints[i].q);
			if (m_bAwake)
				ser.Value("dq", m_joints[i].dq);
			else m_joints[i].dq.zero();
			ser.Value("qext", (Vec3&)m_joints[i].qext);
			ser.Value("q0changed", m_joints[i].bQuat0Changed);
			if (m_joints[i].bQuat0Changed)
				ser.Value("quat0", m_joints[i].quat0);
			m_joints[i].prev_q = m_joints[i].q;

			ser.EndGroup();
		}
		ser.Value("offsPivot", m_offsPivot);
		m_posPivot = m_pos+m_offsPivot;
		m_posNew = m_pos;
		for(int i=0;i<m_nJoints;i++)
			SyncBodyWithJoint(i);
		ComputeBBox(m_BBoxNew);
		UpdatePosition(m_pWorld->RepositionEntity(this,3,m_BBoxNew));

		m_iLastLog = m_pWorld->m_iLastLogPump;
		m_nEvents = 0;

		if (m_bProcessed>=PENT_QUEUED) {
			pe_params_articulated_body pab;
			pab.bGrounded = 100; m_bIgnoreCommands = 1;
			SetParams(&pab,0);
		}
	}

	return 1;
}


int CArticulatedEntity::CalcBodyZa(int idx, float time_interval, vectornf &Za_change)
{
	int i,j,curidx,nextidx,idir;
	float kd,tlim,qdashpot,dq_dashpot,k,qbuf[3]; 
	vectornf Q(3,m_joints[idx].fs->Q),Qtmp(3,qbuf);
	Vec3 d,r,pos_parent,Qscale;
	quaternionf q_parent;
	Vec3 Za_vec[2]; vectornf Za(6,Za_vec[0]);

	if (m_joints[idx].iParent>=0) {
		pos_parent = m_joints[m_joints[idx].iParent].body.pos;
		q_parent = m_joints[m_joints[idx].iParent].quat;
	} else {
		pos_parent = m_posPivot; q_parent = m_qNew;
	}
	d = m_joints[idx].body.pos - (pos_parent + q_parent*m_joints[idx].pivot[0]);
	r = m_joints[idx].body.pos - pos_parent;

	if (m_joints[idx].flags & joint_no_gravity)
		Za_vec[0].zero();
	else
		Za_vec[0] = m_gravity*-m_joints[idx].body.M*time_interval;
	Za_vec[1].zero() = (m_joints[idx].body.w^m_joints[idx].I*m_joints[idx].body.w)*time_interval;
	Za_vec[0] += m_joints[idx].dv_body*m_joints[idx].body.M;
	Za_vec[1] += m_joints[idx].I*m_joints[idx].dw_body;
	m_joints[idx].fs->Q.zero();
	m_joints[idx].ddq.zero();

	if (!(m_joints[idx].flags & joint_isolated_accelerations)) for(j=0;j<m_joints[idx].nPotentialAngles;j++) 
		Qscale[j] = 1.0f/m_joints[idx].fs->qinv[j*(m_joints[idx].nPotentialAngles+1)];
	else Qscale.Set(1.0f,1.0f,1.0f);

	// calculate torque vector and spatial joint axes
	for(j=0;j<m_joints[idx].nPotentialAngles;j++) {
		i = m_joints[idx].fs->axidx2qidx[j];
		kd = fabsf(m_joints[idx].dq[i])>15.0f ? 0 : m_joints[idx].kd[i];
		k = time_interval*Qscale[j];
		Q[j] = (m_joints[idx].ks[i]*m_joints[idx].q[i] + kd*m_joints[idx].dq[i])*-k;
		// if the joint is approaching limit (but haven't breached it yet) and is in dashpot area, damp the velocity
		if (fabsf(m_joints[idx].dq[i])>0.5f) {
			idir = isneg(m_joints[idx].dq[i])^1;
			tlim = m_joints[idx].q[i]-m_joints[idx].limits[idir][i];
			if (tlim<-g_PI) tlim += 2*g_PI;
			if (tlim>g_PI) tlim -= 2*g_PI;
			tlim *= idir*2-1;	qdashpot = m_joints[idx].qdashpot[i];
			dq_dashpot = min(fabsf(m_joints[idx].dq[i])*Qscale[j], (qdashpot-tlim)*m_joints[idx].kdashpot[i]*isneg(fabsf(tlim-2*qdashpot)-qdashpot)*k);
			Q[j] += dq_dashpot*(1-idir*2);
		}
	}
	if (!(m_joints[idx].flags & (angle0_locked|angle0_gimbal_locked)*5)) { // take into accout that x and z axes influence one another
		Q[m_joints[idx].fs->qidx2axidx[0]] += Q[m_joints[idx].fs->qidx2axidx[2]]*(m_joints[idx].rotaxes[0]*m_joints[idx].rotaxes[2]);
		Q[m_joints[idx].fs->qidx2axidx[2]] += Q[m_joints[idx].fs->qidx2axidx[0]]*(m_joints[idx].rotaxes[0]*m_joints[idx].rotaxes[2]);
	}
	if (m_joints[idx].flags & joint_isolated_accelerations)
		m_joints[idx].ddq = m_joints[idx].fs->Q;

	Vec3 Za_child_vec[2]; vectornf Za_child(6,Za_child_vec[0]);
	// iterate all children, accumulate Za
	for(i=0,curidx=idx+1; i<m_joints[idx].nChildren; i++) {
		nextidx = CalcBodyZa(curidx, time_interval, Za_child);
		Za += Za_child;	Za_vec[1] += Za_child_vec[0] ^ m_joints[idx].body.pos-m_joints[curidx].body.pos;
		curidx = nextidx;
	}

	// calculate Za changes for the parent
	Za_change = Za;
	vectornf ddq(m_joints[idx].nActiveAngles, m_joints[idx].ddq);
	if (m_joints[idx].nActiveAngles>0) {
		matrixf sT(Q.len = m_joints[idx].nActiveAngles,6,0, m_joints[idx].fs->s_vec[0][0]);
		matrixf qinv_down(m_joints[idx].nActiveAngles,m_joints[idx].nActiveAngles,0, m_joints[idx].fs->qinv_down);
		matrixf Ia_s(6,m_joints[idx].nActiveAngles,0, m_joints[idx].fs->Ia_s);
		if (!(m_joints[idx].flags & joint_isolated_accelerations))
			ddq = qinv_down*((Qtmp=Q)-=sT*Za);
		Za_change += Ia_s*ddq;
	}

	if (m_joints[idx].nActiveAngles<m_joints[idx].nPotentialAngles && !(m_joints[idx].flags & joint_isolated_accelerations)) {
		matrixf sT(Q.len = m_joints[idx].nPotentialAngles,6,0, m_joints[idx].fs->s_vec[0][0]);
		matrixf qinv(m_joints[idx].nPotentialAngles,m_joints[idx].nPotentialAngles,0, m_joints[idx].fs->qinv);
		matrixf qinv_sT(m_joints[idx].nPotentialAngles,6,0, m_joints[idx].fs->qinv_sT[0]);
		ddq = qinv*((Qtmp=Q)-=sT*Za);
	}

	return curidx;
}


int CArticulatedEntity::CalcBodyIa(int idx, matrixf& Ia_change)
{
	int i,j,curidx,nextidx;
	Vec3 d,r,pos_parent;
	quaternionf q_parent;

	if (m_joints[idx].iParent>=0) {
		pos_parent = m_joints[m_joints[idx].iParent].body.pos;
		q_parent = m_joints[m_joints[idx].iParent].quat;
	} else {
		pos_parent = m_posPivot; q_parent = m_qNew;
	}
	d = m_joints[idx].body.pos - (pos_parent + q_parent*m_joints[idx].pivot[0]);
	r = m_joints[idx].body.pos - pos_parent;

	// fill initial Ia
	matrixf Ia(6,6,0,m_joints[idx].fs->Ia[0]); Ia.zero();
	m_joints[idx].fs->Ia[0][3]=m_joints[idx].fs->Ia[1][4]=m_joints[idx].fs->Ia[2][5] = m_joints[idx].body.M;
	SetMtxStrided<float,6,1>(m_joints[idx].I, &m_joints[idx].fs->Ia[3][0]);

	// sort axes so that active angles' axes come first
	for (i=j=0;i<3;i++) if (!(m_joints[idx].flags & (angle0_locked|angle0_limit_reached|angle0_gimbal_locked)<<i)) {
		m_joints[idx].fs->qidx2axidx[i] = j; m_joints[idx].fs->axidx2qidx[j++] = i;
	} m_joints[idx].nActiveAngles = j;
	for (i=0;i<3;i++) if ((m_joints[idx].flags & (angle0_locked|angle0_limit_reached)<<i) == angle0_limit_reached<<i) {
		m_joints[idx].fs->qidx2axidx[i] = j; m_joints[idx].fs->axidx2qidx[j++] = i;
	} m_joints[idx].nPotentialAngles = j;

	for(j=0;j<m_joints[idx].nPotentialAngles;j++) {
		i = m_joints[idx].fs->axidx2qidx[j];
		m_joints[idx].fs->s_vec[j][0] = m_joints[idx].rotaxes[i]^d; 
		m_joints[idx].fs->s_vec[j][1] = m_joints[idx].rotaxes[i];
	}
	
	float buf[39]; matrixf Ia_child(6,6,0,_align16(buf));
	// iterate all children, accumulate Ia
	for(i=0,curidx=idx+1; i<m_joints[idx].nChildren; i++) {
		nextidx = CalcBodyIa(curidx, Ia_child);
		r = m_joints[idx].body.pos-m_joints[curidx].body.pos;
		LeftOffsetSpatialMatrix(Ia_child, -r);
		RightOffsetSpatialMatrix(Ia_child, r);
		Ia += Ia_child;
		curidx = nextidx;
	}

	// calculate Ia changes for the parent
	Ia_change = Ia;
	matrixf sT(m_joints[idx].nActiveAngles,6,0, m_joints[idx].fs->s_vec[0][0]);
	matrixf s(6,m_joints[idx].nActiveAngles,0, m_joints[idx].fs->s);
	matrixf Ia_s_qinv_sT(6,6,0, m_joints[idx].fs->Ia_s_qinv_sT[0]);
	matrixf qinv(m_joints[idx].nActiveAngles,m_joints[idx].nActiveAngles,0, m_joints[idx].fs->qinv);
	if (m_joints[idx].nActiveAngles>0) {
		SpatialTranspose(sT, s);
		matrixf Ia_s(6,m_joints[idx].nActiveAngles,0, m_joints[idx].fs->Ia_s);
		matrixf qinv_down(m_joints[idx].nActiveAngles,m_joints[idx].nActiveAngles,0, m_joints[idx].fs->qinv_down);
		matrixf Ia_s_qinv(6,m_joints[idx].nActiveAngles,0, buf);
		Ia_s = Ia*s; qinv = (qinv_down = sT*Ia_s).invert();
		Ia_s_qinv_sT = (Ia_s_qinv = Ia_s*qinv)*sT;
		Ia_change -= Ia_s_qinv_sT*Ia;
		Ia_s_qinv_sT *= -1.0f;
	} else Ia_s_qinv_sT.zero();
	for(i=0;i<6;i++) m_joints[idx].fs->Ia_s_qinv_sT[i][i] += 1;
	r = m_joints[idx].body.pos - pos_parent;
	LeftOffsetSpatialMatrix(Ia_s_qinv_sT, r);

	if (m_joints[idx].nPotentialAngles>0) {
		// precalculate matrices for velocity propagation up - use limited angles also 
		matrixf qinv_sT(m_joints[idx].nPotentialAngles,6,0, m_joints[idx].fs->qinv_sT[0]);
		matrixf qinv_sT_Ia(m_joints[idx].nPotentialAngles,6,0, m_joints[idx].fs->qinv_sT_Ia[0]);
		matrixf sT_Ia(m_joints[idx].nPotentialAngles,6,0, buf);
		if (m_joints[idx].nActiveAngles < m_joints[idx].nPotentialAngles) {
			sT.nRows = m_joints[idx].nPotentialAngles; SpatialTranspose(sT, s);
			(qinv = (sT_Ia = sT*Ia)*s).invert();
		}
		(qinv_sT = qinv*sT)*=-1.0f; qinv_sT_Ia = qinv_sT*Ia;
		//matrixf(6,6,0,m_joints[idx].s_qinv_sT_Ia[0]) = s*qinv_sT_Ia;
	}

	return curidx;
}


int CArticulatedEntity::CollectPendingImpulses(int idx,int &bNotZero)
{
	int i,j,curidx,newidx,bChildNotZero;
	bNotZero = 0;

	for(i=0;i<3;i++) if (!is_unused(m_joints[idx].dq_req[i])) {
		// note that articulation matrices from the previous frame are used, while rotaxes are already new; this introduces slight inconsistency
		//Matrix33 K; GetJointTorqueResponseMatrix(idx, K);
		//float resp = m_joints[idx].rotaxes[i]*K*m_joints[idx].rotaxes[i];
		float buf[39],buf1[39]; matrixf Mresp(6,6,0,_align16(buf)),MrespLoc(6,6,0,_align16(buf1));
		MrespLoc = matrixf(6,6,0,m_joints[idx].fs->Ia_s_qinv_sT[0]);
		for(j=0;j<6;j++) MrespLoc[j][j] -= 1.0f;
		Mresp = matrixf(6,6,0,m_joints[m_joints[idx].iParent].fs->Iinv[0])*MrespLoc;
		LeftOffsetSpatialMatrix(Mresp,m_joints[m_joints[idx].iParent].body.pos-m_joints[idx].body.pos);
		j = m_joints[idx].fs->qidx2axidx[i];
		Vec3 axis = m_joints[idx].rotaxes[i], dw = GetMtxStrided<float,6,1>(&Mresp[0][3])*axis, 
				 dv = GetMtxStrided<float,6,1>(&Mresp[3][3])*axis;
		float resp = -(Vec3(m_joints[idx].fs->qinv_sT[j]+3)*axis);
		resp -= Vec3(m_joints[idx].fs->qinv_sT_Ia[j])*dw + Vec3(m_joints[idx].fs->qinv_sT_Ia[j]+3)*dv;

		if (resp>0.1f*m_body.Minv) {
			resp = (m_joints[idx].dq_req[i]-m_joints[idx].dq[i])/resp*m_scaleBounceResponse;
			m_joints[idx].fs->Ya_vec[1] -= m_joints[idx].rotaxes[i]*resp;
			if (m_joints[idx].iParent>=0)
				m_joints[m_joints[idx].iParent].fs->Ya_vec[1] += m_joints[idx].rotaxes[i]*resp;
			bNotZero++;
		}
	}

	if (bNotZero += isneg(1E-6-m_joints[idx].Pext.len2()-m_joints[idx].Lext.len2())) {
		m_joints[idx].fs->Ya_vec[0] -= m_joints[idx].Pext; m_joints[idx].Pext.zero();
		m_joints[idx].fs->Ya_vec[1] -= m_joints[idx].Lext; m_joints[idx].Lext.zero();
	}

	for(i=0,curidx=idx+1; i<m_joints[idx].nChildren; i++) {
		newidx = CollectPendingImpulses(curidx, bChildNotZero);
		if (bChildNotZero) {
			matrixf Ia_s_qinv_sT(6,6,0,m_joints[curidx].fs->Ia_s_qinv_sT[0]);
			vectornf Ya(6,m_joints[idx].fs->Ya_vec[0]);
			vectornf Ya_child(6,m_joints[curidx].fs->Ya_vec[0]);
			Ya += Ia_s_qinv_sT*Ya_child;
			bNotZero += bChildNotZero;
		}
		curidx = newidx;
	}
	if (idx==0)
	{
#if 0
		vectornf(6,(float*)(m_Ya_vec[0])) = 
			::operator*((const matrix_tpl<float> &)matrixf(6,6,0,m_joints[0].fs->Ia_s_qinv_sT[0]), 
			(const vectorn_tpl<float>&)vectornf(6,m_joints[0].fs->Ya_vec[0]));
#else
		vectorn_tpl<float> tmp(6,m_joints[0].fs->Ya_vec[0]);
		vectornf(6,(float*)(m_Ya_vec[0])) = 
			::operator*((const matrix_tpl<float> &)matrixf(6,6,0,m_joints[0].fs->Ia_s_qinv_sT[0]), 
			tmp);
#endif
	}
	return curidx;
}


void CArticulatedEntity::PropagateImpulses(const Vec3 &dv, int bLockLimits)
{
	int idx,i,j,bHitsLimit;
	Vec3 pos_parent;

	for(idx=0; idx<m_nJoints; idx++) {
		vectornf dv_spat(6, m_joints[idx].fs->dv_vec[0]);
		if (m_joints[idx].iParent>=0) {
			m_joints[idx].fs->dv_vec[0] = m_joints[m_joints[idx].iParent].fs->dv_vec[0];
			m_joints[idx].fs->dv_vec[1] = m_joints[m_joints[idx].iParent].fs->dv_vec[1];
			pos_parent = m_joints[m_joints[idx].iParent].body.pos;
		} else {
			m_joints[idx].fs->dv_vec[0].zero(); m_joints[idx].fs->dv_vec[1] = dv; pos_parent = m_posPivot;
		}
		m_joints[idx].fs->dv_vec[1] += m_joints[idx].fs->dv_vec[0] ^ m_joints[idx].body.pos-pos_parent;

		if (m_joints[idx].nPotentialAngles>0) {
			vectornf ddq(m_joints[idx].nPotentialAngles, m_joints[idx].ddq);
			matrixf qinv_sT_Ia(m_joints[idx].nPotentialAngles,6,0, m_joints[idx].fs->qinv_sT_Ia[0]);
			matrixf qinv_sT(m_joints[idx].nPotentialAngles,6,0, m_joints[idx].fs->qinv_sT[0]);
			vectornf Ya(6,m_joints[idx].fs->Ya_vec[0]);
			matrixf s(6,m_joints[idx].nPotentialAngles,0, m_joints[idx].fs->s);
			ddq = qinv_sT_Ia*dv_spat;
			ddq += qinv_sT*Ya;

			for(i=0; i<m_joints[idx].nPotentialAngles; i++) {
				j = m_joints[idx].fs->axidx2qidx[i]; 
				if (!is_unused(m_joints[idx].dq_req[j])) {
					m_joints[idx].ddq[i] = m_joints[idx].dq_req[j]-m_joints[idx].dq[j];
					MARK_UNUSED m_joints[idx].dq_req[j];
				} else if (m_joints[idx].flags & angle0_limit_reached<<j) {
					bHitsLimit = isneg(-m_joints[idx].ddq[i]*m_joints[idx].dq_limit[j]);
					if (bHitsLimit || bLockLimits) 
						m_joints[idx].ddq[i] = 0;	// do not accelerate angle that reached its limit
					if (!bHitsLimit && bLockLimits)
						m_joints[idx].flags &= ~(angle0_limit_reached<<j);
				}
				m_joints[idx].dq[j] += m_joints[idx].ddq[i];
			}
			
			dv_spat += s*ddq;
		}
		m_joints[idx].fs->Ya_vec[0].zero(); m_joints[idx].fs->Ya_vec[1].zero();
	}
}


void CArticulatedEntity::CalcVelocityChanges(float time_interval, const Vec3 &dv,const Vec3 &dw)
{
	int idx,i,j;
	Vec3 pos_parent;

	for(idx=0; idx<m_nJoints; idx++) {
		vectornf dv_spat(6, m_joints[idx].fs->dv_vec[0]);
		if (m_joints[idx].iParent>=0) {
			m_joints[idx].fs->dv_vec[0] = m_joints[m_joints[idx].iParent].fs->dv_vec[0];
			m_joints[idx].fs->dv_vec[1] = m_joints[m_joints[idx].iParent].fs->dv_vec[1];
			pos_parent = m_joints[m_joints[idx].iParent].body.pos;
		} else {
			m_joints[idx].fs->dv_vec[0] = dw; m_joints[idx].fs->dv_vec[1] = dv; pos_parent = m_posPivot;
		}
		m_joints[idx].fs->dv_vec[1] += m_joints[idx].fs->dv_vec[0] ^ m_joints[idx].body.pos-pos_parent;

		if (m_joints[idx].nPotentialAngles>0) {
			vectornf ddq(m_joints[idx].nPotentialAngles, m_joints[idx].ddq);
			matrixf qinv_sT(m_joints[idx].nPotentialAngles,6,0, m_joints[idx].fs->qinv_sT[0]);
			vectornf Ya(6,m_joints[idx].fs->Ya_vec[0]);
			matrixf qinv_sT_Ia(m_joints[idx].nPotentialAngles,6,0, m_joints[idx].fs->qinv_sT_Ia[0]);
			matrixf s(6,m_joints[idx].nPotentialAngles,0, m_joints[idx].fs->s);

			ddq += qinv_sT_Ia*dv_spat;
			ddq += qinv_sT*Ya;

			for(i=0; i<m_joints[idx].nPotentialAngles; i++) {
				j = m_joints[idx].fs->axidx2qidx[i]; 
				if (m_joints[idx].flags & angle0_limit_reached<<j && m_joints[idx].ddq[i]*m_joints[idx].dq_limit[j]>0)
					m_joints[idx].ddq[i] = 0;	// do not accelerate angle that reached its limit
				m_joints[idx].dq[j] += m_joints[idx].ddq[i];
			}
			
			dv_spat += s*ddq;
		}
		m_joints[idx].fs->Ya_vec[0].zero(); m_joints[idx].fs->Ya_vec[1].zero();
	}
}


void CArticulatedEntity::OnContactResolved(entity_contact *pcontact, int iop, int iGroupId)
{
	if (iop<2 && pcontact->pent[iop^1]!=this) {
		m_nBodyContacts++;
		m_nDynContacts -= -pcontact->pent[iop^1]->m_iSimClass >> 31;
		m_joints[m_infos[pcontact->ipart[iop]].iJoint].bHasExtContacts = 1;
	}
	CPhysicalEntity::OnContactResolved(pcontact,iop,iGroupId);
	// force simtype 1 in case of character-vehicle interaction
	//m_iSimTypeOverride |= inrange(pcontact->pbody[iop^1]->Minv,1E-8f,m_body.Minv*0.1f);
}


void CArticulatedEntity::CalcBodiesIinv(int bLockLimits)
{
	int idx,i,nAngles;
	float Iinv_buf[39],aux_buf[39],Mright_buf[39],s_buf[18],qinv_sT_buf[18];
	Vec3 r,pos_parent;
	matrixf Iinv_parent(6,6,0,_align16(Iinv_buf)),Iaux(6,6,0,_align16(aux_buf));
	memset(Iinv_buf,0,sizeof(Iinv_buf));
	SetMtxStrided<float,6,1>(m_M0inv*-1.0f, Iinv_parent.data+18);

	for(idx=0; idx<m_nJoints; idx++) {
		if (m_joints[idx].iParent>=0) {
			Iinv_parent.data = m_joints[m_joints[idx].iParent].fs->Iinv[0];
			pos_parent = m_joints[m_joints[idx].iParent].body.pos;
		} else {
			Iinv_parent.data = _align16(Iinv_buf);
			pos_parent = m_posPivot;
		}
		matrixf Iinv(6,6,0,m_joints[idx].fs->Iinv[0]);
		matrixf s_qinv_sT_Ia(6,6,0, m_joints[idx].fs->s_qinv_sT_Ia[0]);
		r = m_joints[idx].body.pos-pos_parent;
		nAngles = bLockLimits ? m_joints[idx].nActiveAngles : m_joints[idx].nPotentialAngles;

		if (nAngles>0) {
			matrixf Mright(6,6,0,_align16(Mright_buf)),s(6,nAngles,0,m_joints[idx].fs->s);
			matrixf Ia(6,6,0, m_joints[idx].fs->Ia[0]);
			matrixf s_qinv_sT(6,6,0,m_joints[idx].fs->s_qinv_sT[0]);
			matrixf qinv_sT(nAngles,6,0,m_joints[idx].fs->qinv_sT[0]);

			if (nAngles!=m_joints[idx].nPotentialAngles) {
				matrixf sT(6,nAngles,0, m_joints[idx].fs->s_vec[0][0]);
				s.data = s_buf; SpatialTranspose(sT,s);
				qinv_sT.data = qinv_sT_buf;
				qinv_sT = matrixf(nAngles,nAngles,0,m_joints[idx].fs->qinv_down)*sT;
			}

			s_qinv_sT = s*qinv_sT; // already with '-' sign
			Iinv = s_qinv_sT*Ia; 
			for(i=0;i<6;i++) Iinv.data[i*7] += 1.0f;
			RightOffsetSpatialMatrix(Iinv, -r);
			s_qinv_sT_Ia = Iinv;

			//Mright = Iinv_parent;
			//LeftOffsetSpatialMatrix(Mright, -r);
			//RightOffsetSpatialMatrix(Mright, r);
			Iinv = (Iaux=Iinv)*Iinv_parent; //Mright;

			if (nAngles == m_joints[idx].nActiveAngles)
				Iinv = (Iaux=Iinv)*matrixf(6,6,0,m_joints[idx].fs->Ia_s_qinv_sT[0]);
			else {
				Mright = Ia*s_qinv_sT;
				for(i=0;i<6;i++) Mright.data[i*7] += 1.0f;
				LeftOffsetSpatialMatrix(Mright, r);
				Iinv = (Iaux=Iinv)*Mright;
			}
			Iinv += s_qinv_sT;
		} else {
			Iinv = Iinv_parent;
			LeftOffsetSpatialMatrix(Iinv, -r);
			RightOffsetSpatialMatrix(Iinv, r);
			s_qinv_sT_Ia.identity();
			RightOffsetSpatialMatrix(s_qinv_sT_Ia, -r);
		}
	}
}


void CArticulatedEntity::GetContactMatrix(const Vec3 &pt, int ipart, Matrix33 &K)
{
	if ((unsigned int)ipart<(unsigned int)m_nParts)
		m_joints[m_infos[ipart].iJoint].body.GetContactMatrix(pt-m_joints[m_infos[ipart].iJoint].body.pos,K);
}


void CArticulatedEntity::GetJointTorqueResponseMatrix(int idx, Matrix33 &K)
{
	K = GetMtxStrided<float,6,1>(&m_joints[idx].fs->Iinv[0][3])*-1.0f;	// Lw*-1
	if (m_joints[idx].iParent>=0) {
		Matrix33 ww_up = GetMtxStrided<float,6,1>(&m_joints[idx].fs->s_qinv_sT_Ia[0][0]),
							 vw_up = GetMtxStrided<float,6,1>(&m_joints[idx].fs->s_qinv_sT_Ia[0][3]),
							 LL_down = GetMtxStrided<float,6,1>(&m_joints[idx].fs->Ia_s_qinv_sT[3][3]),
							 LP_down = GetMtxStrided<float,6,1>(&m_joints[idx].fs->Ia_s_qinv_sT[0][3]),
							 Lv_parent = GetMtxStrided<float,6,1>(&m_joints[m_joints[idx].iParent].fs->Iinv[3][3]), 
							 Lw_parent = GetMtxStrided<float,6,1>(&m_joints[m_joints[idx].iParent].fs->Iinv[0][3]),
							 Pw_parent = GetMtxStrided<float,6,1>(&m_joints[m_joints[idx].iParent].fs->Iinv[0][0]);
		//Matrix33 rmtx;
		//(m_joints[idx].body.pos-m_joints[m_joints[idx].iParent].body.pos).crossproduct_matrix(rmtx);
		K += vw_up*Lv_parent;//-rmtx*Lw_parent);
		K += ww_up*Lw_parent;
		K += Pw_parent*LP_down;
		K += Lw_parent*LL_down;
		K -= Lw_parent;
	}
}


entity_contact *CArticulatedEntity::CreateConstraintContact(int idx)
{
	entity_contact *pcontact = (entity_contact*)AllocSolverTmpBuf(sizeof(entity_contact));
	if (!pcontact) 
		return 0;
	pcontact->flags = 0;
	pcontact->pent[0] = this;
	pcontact->pbody[0] = &m_joints[idx].body;
	pcontact->ipart[0] = m_joints[idx].iStartPart;
	if (m_joints[idx].iParent>=0) {
		pcontact->pent[1] = this;
		pcontact->pbody[1] = &m_joints[m_joints[idx].iParent].body;
		pcontact->ipart[1] = m_joints[m_joints[idx].iParent].iStartPart;
	} else {
		pcontact->pent[1] = &g_StaticPhysicalEntity;
		pcontact->pbody[1] = &g_StaticRigidBody;
		pcontact->ipart[1] = 0;
	}
	pcontact->friction = 0;
	pcontact->vreq.zero();
	pcontact->pt[0] = pcontact->pt[1] = m_joints[idx].body.pos + m_joints[idx].quat*m_joints[idx].pivot[1];
	pcontact->K.SetZero();

	return pcontact;
}


int CArticulatedEntity::RegisterContacts(float time_interval,int nMaxPlaneContacts)
{
	int idx,i,j,iAxes[3];
	masktype constraints_mask;
	entity_contact *pContact;
	Vec3 dw,dL;
	float kd,ks;
__ae_step++;
	if (m_flags & pef_log_collisions && m_nMaxEvents>0 && m_pWorld->m_iLastLogPump!=m_iLastLogColl) {
		m_nEvents=0; m_vcollMin=1E10f; m_iLastLogColl=m_pWorld->m_iLastLogPump;	
	}

	UpdateConstraints(time_interval);

	for(i=0;i<m_nJoints;i++) m_joints[i].pContact=m_joints[i].pContactEnd = CONTACT_END(m_joints[i].pContact);
	for(pContact=m_pContactStart; pContact!=CONTACT_END(m_pContactStart); pContact=pContact->next) if (!(pContact->flags & contact_2b_verified)) {
		i = m_infos[pContact->ipart[0]].iJoint;
		pContact->prevAux = m_joints[i].pContact->prevAux; pContact->nextAux = m_joints[i].pContact;
		m_joints[i].pContact->prevAux = pContact; m_joints[i].pContact = pContact;
	}

	for(i=0,constraints_mask=0;i<m_nColliders;i++)
		constraints_mask |= m_pColliderConstraints[i];

	for(idx=0;idx<m_nJoints;idx++) {
		m_joints[idx].body.softness[0] = m_softness[m_bInGroup*2+0];
		m_joints[idx].body.softness[1] = m_softness[m_bInGroup*2+1];
		if (m_joints[idx].pContact!=CONTACT_END(m_joints[idx].pContact))
			ArchiveContact(m_joints[idx].pContact);
		for(pContact=m_joints[idx].pContact; pContact!=CONTACT_END(m_joints[idx].pContact); pContact=pContact->nextAux)
			RegisterContact(pContact);

		for(i=0;i<NMASKBITS && getmask(i)<=constraints_mask;i++) 
		if (constraints_mask&getmask(i) && m_pConstraintInfos[i].bActive && 
				(unsigned int)(m_pConstraints[i].ipart[0]-m_joints[idx].iStartPart) < (unsigned int)m_joints[idx].nParts)
			RegisterContact(m_pConstraints+i);

		if (m_bGrounded || m_joints[idx].iParent>=0) {
			Vec3 pivot[2],axisDrift(ZERO),axisTens(ZERO);
			if (!(pContact = CreateConstraintContact(idx)))
				break;
			pContact->flags = contact_constraint_3dof;
			GetContactMatrix(pContact->pt[0], pContact->ipart[0], pContact->K);
			if (m_joints[idx].iParent>=0) {
				GetContactMatrix(pContact->pt[0], pContact->ipart[1], pContact->K);
				pivot[0] = m_joints[m_joints[idx].iParent].body.pos + m_joints[m_joints[idx].iParent].quat*m_joints[idx].pivot[0];
			} else 
				pivot[0] = m_posPivot;
			pivot[1] = m_joints[idx].body.pos + m_joints[idx].quat*m_joints[idx].pivot[1];
			if (m_iSimTypeCur && (pivot[0]-pivot[1]).len2()>sqr(0.003f)) {
				pContact->vreq = (pivot[0]-pivot[1])*10.0f;
				pContact->n = pContact->vreq.normalized();
				if (pContact->vreq.len2()>sqr(10.0f))
					pContact->vreq = pContact->n*10.0f;
				pContact->pt[1] = pivot[0];
			} else
				pContact->n.Set(0,0,1);
			RegisterContact(pContact);

			for(i=j=0,ks=kd=0;i<3;i++) if (!(m_joints[idx].flags & (angle0_locked|angle0_gimbal_locked)<<i)) {
				iAxes[j++] = i;
				axisTens += m_joints[idx].rotaxes[i]*(min(1.0f,max(-1.0f,m_joints[idx].q0[i]-m_joints[idx].q[i]-m_joints[idx].qext[i])));
				kd = max(kd, m_joints[idx].kd[i]);
				kd = max(kd, m_joints[idx].kdashpot[i]*(1^iszero((int)m_joints[idx].flags & joint_dashpot_reached))); 
				ks = max(ks, m_joints[idx].ks[i]);
			} else
				axisDrift += m_joints[idx].rotaxes[i]*min(1.0f,m_joints[idx].q0[i]-m_joints[idx].q[i]);
			if (!(pContact = CreateConstraintContact(idx)))
				break;
			pContact->K = m_joints[idx].body.Iinv;
			if (m_joints[idx].iParent>=0)
				pContact->K += m_joints[m_joints[idx].iParent].body.Iinv;
			dw = pContact->pbody[0]->w-pContact->pbody[1]->w;
			if ((unsigned int)j-1u<2u) {
				/*if (m_iSimTypeCur && j==1 && m_joints[idx].flags&joint_expand_hinge) {
					if (is_unused(m_joints[idx].hingePivot[0])) {
						CCylinderGeom cylgeom;
						cylinder cyl;
						box bbox;
						geom_world_data gwd[2];
						intersection_params ip;
						geom_contact *punproj;
						i = m_joints[idx].iStartPart;

						m_parts[i].pPhysGeomProxy->pGeom->GetBBox(&bbox);
						cyl.axis = m_joints[idx].rotaxes[iAxes[0]];
						cyl.center.zero();
						cyl.r = (bbox.size.x+bbox.size.y+bbox.size.z)*m_parts[i].scale*0.05f; cyl.hh = cyl.r*0.2f;
						cylgeom.CreateCylinder(&cyl);

						gwd[0].offset = m_pos + m_qrot*m_parts[i].pos;
						//(m_qrot*m_parts[i].q).getmatrix(gwd[0].R); //Q2M_IVO
						gwd[0].R = Matrix33(m_qrot*m_parts[i].q);
						gwd[0].scale = m_parts[i].scale;
						gwd[1].offset = pContact->pt[0];
						gwd[1].v = -m_joints[idx].rotaxes[iAxes[0]];
						ip.iUnprojectionMode = 0;
						ip.time_interval = 1E10;
						if (m_parts[i].pPhysGeomProxy->pGeom->Intersect(&cylgeom, gwd+1,gwd, &ip, punproj)) {
							WriteLockCond lockColl(*ip.plock,0); lockColl.SetActive();
							punproj->t -= cyl.hh;
							m_joints[idx].hingePivot[1] = m_joints[idx].pivot[1] + (m_joints[idx].rotaxes[iAxes[0]]*m_joints[idx].quat)*punproj->t;
							m_joints[idx].hingePivot[0] = m_joints[idx].iParent>=0 ?
								m_joints[idx].pivot[0] + (m_joints[idx].rotaxes[iAxes[0]]*m_joints[m_joints[idx].iParent].quat)*punproj->t :
								m_posPivot + m_joints[idx].rotaxes[iAxes[0]]*punproj->t;
						} else
							m_joints[idx].flags &= ~joint_expand_hinge;
					}

					if (m_joints[idx].flags & joint_expand_hinge) {
						pContact->flags = contact_constraint_3dof;
						pContact->pt[0] = m_joints[idx].body.pos + m_joints[idx].quat*m_joints[idx].hingePivot[1];
						GetContactMatrix(pContact->pt[0], pContact->ipart[0], pContact->K);
						if (m_joints[idx].iParent>=0) {
							GetContactMatrix(pContact->pt[0], pContact->ipart[1], pContact->K);
							pContact->pt[1] = m_joints[m_joints[idx].iParent].body.pos + m_joints[m_joints[idx].iParent].quat*m_joints[idx].hingePivot[0];
						} else 
							pContact->pt[1] = m_joints[idx].hingePivot[0];
						if ((pContact->pt[0]-pContact->pt[1]).len2()>sqr(0.003f)) {
							pContact->vreq = (pivot[0]-pivot[1])*10.0f;
							pContact->n = pContact->vreq.normalized();
							if (pContact->vreq.len2()>sqr(10.0f))
								pContact->vreq = pContact->n*10.0f;
						} else
							pContact->n.Set(0,0,1);
					}
				}*/

				if (j==2) { // 2 free axes, 1 axis locked
					pContact->flags = contact_constraint_2dof | contact_angular;
					pContact->n = (m_joints[idx].rotaxes[iAxes[0]]^m_joints[idx].rotaxes[iAxes[1]]).normalized();
					pContact->vreq = pContact->n*min(5.0f,(axisDrift*pContact->n)*6.0f);
					dw -= pContact->n*(pContact->n*dw);
				} else { // 1 free axis, 2 axes locked
					pContact->flags = contact_constraint_1dof | contact_angular;
					pContact->n = m_joints[idx].rotaxes[iAxes[0]];
					pContact->vreq = (axisDrift-pContact->n*(pContact->n*axisDrift))*6.0f;
					if (pContact->vreq.len2()>sqr(5.0f))
						pContact->vreq.normalize() *= 5.0f;
					dw = pContact->n*(pContact->n*dw);
				}
				RegisterContact(pContact);
			}	else 
				ks = 0;
			dL = pContact->K.GetInverted()*(dw*min(0.0f,time_interval*kd-1.0f)+axisTens*(time_interval*ks));
			pContact->pbody[0]->L += dL; pContact->pbody[1]->L -= dL; 
			pContact->pbody[0]->w = pContact->pbody[0]->Iinv*pContact->pbody[0]->L;
			pContact->pbody[1]->w = pContact->pbody[1]->Iinv*pContact->pbody[1]->L;

			for(i=0;i<3;i++) if (m_joints[idx].flags & angle0_limit_reached<<i) {
				if (!(pContact = CreateConstraintContact(idx)))
					break;
				pContact->K = m_joints[idx].body.Iinv;
				if (m_joints[idx].iParent>=0)
					pContact->K += m_joints[m_joints[idx].iParent].body.Iinv;
				pContact->flags = contact_angular;
				pContact->n = m_joints[idx].rotaxes[i]*-sgnnz(m_joints[idx].dq_limit[i]);
				if (m_iSimTypeCur)
					pContact->vreq = pContact->n*min(5.0f,fabsf(m_joints[idx].dq_limit[i])*4.0f);
				//pContact->vsep = -0.01f;
				RegisterContact(pContact);
			}
		}
	}

	return 0;
}

float __maxdiff = 0;

int CArticulatedEntity::Update(float time_interval, float damping)
{
	int i,j,nCollJoints=0,bPosChanged=0, bFloating=m_bFloating|iszero(m_gravity.len2()); 
	float dt,e,minEnergy = m_nBodyContacts>=m_nCollLyingMode ? m_EminLyingMode : m_Emin;
	m_bAwake = (iszero(m_nBodyContacts) & (bFloating^1)) | isneg(m_simTimeAux-0.5f);
	m_bUsingUnproj = 0;
	m_nStepBackCount = (m_nStepBackCount&-m_bSteppedBack)+m_bSteppedBack;

	if (m_flags & pef_log_collisions && m_nMaxEvents>0) {
		if (m_pWorld->m_iLastLogPump!=m_iLastLogColl) {
			m_nEvents=0; m_vcollMin=1E10f; m_iLastLogColl=m_pWorld->m_iLastLogPump;	
		}
		for(i=0;i<m_nEvents;i++) if (m_pEventsColl[i]->pEntContact) {
			m_pEventsColl[i]->normImpulse += ((entity_contact*)m_pEventsColl[i]->pEntContact)->Pspare;
			m_pEventsColl[i]->pEntContact = 0;
		}
	}

	if (m_body.M>0) {
		for(i=0;i<m_nJoints;i++) {
			m_joints[i].dq *= damping;
			m_joints[i].body.v*=damping; m_joints[i].body.w*=damping;
			m_joints[i].body.P*=damping; m_joints[i].body.L*=damping;

			e = (m_joints[i].body.v.len2() + (m_joints[i].body.L*m_joints[i].body.w)*m_joints[i].body.Minv)*0.5f + m_joints[i].body.Eunproj;
			m_bAwake |= (m_joints[i].bAwake = isneg(minEnergy-e));
			m_bUsingUnproj |= isneg(1E-8f-m_joints[i].body.Eunproj);
			m_joints[i].bAwake |= bFloating|m_bUsingUnproj;
			for(j=m_joints[i].iParent; j>=0; j=m_joints[j].iParent)
				m_joints[i].bAwake |= m_joints[j].bAwake;
			nCollJoints += m_joints[i].bHasExtContacts;

			if (m_iSimTypeCur && m_joints[i].body.Eunproj>0) {
				SyncJointWithBody(i,3);
				for(j=m_joints[i].iStartPart; j<m_joints[i].iStartPart+m_joints[i].nParts; j++) {
					(m_infos[j].q = m_joints[i].quat*m_infos[j].q0).Normalize();
					m_infos[j].pos = m_joints[i].quat*m_infos[j].pos0+m_joints[i].body.pos-m_pos;
				}
				bPosChanged = 1;
			}
		}
		if (bPosChanged) {
			ComputeBBox(m_BBoxNew);
			UpdatePosition(m_pWorld->RepositionEntity(this,1,m_BBoxNew));
		}
		m_nSleepFrames = (m_nSleepFrames&~-m_bAwake) + (m_bAwake^1);
		m_bAwake |= isneg(m_nSleepFrames-4) & isneg(nCollJoints-3);
		m_timeIdle *= isneg(-m_nColliders-m_nPrevColliders-m_submergedFraction) & m_pWorld->m_bCurGroupInvisible;
		i = isneg(0.0001f-m_maxTimeIdle); // forceful deactivation is turned on
		dt = time_interval+(m_timeStepFull-time_interval)*m_pWorld->m_bCurGroupInvisible;
		m_bAwake &= i^1 | isneg((m_timeIdle+=dt*i)-m_maxTimeIdle);
		if (!m_bGrounded)
			m_body.v = m_joints[0].body.v;
		m_body.P = m_body.v*m_body.M;
		m_body.w.zero();
		m_bInGroup = isneg(-m_nDynContacts);

		for(i=0; i<m_nJoints; i++) {
#ifdef _DEBUG
Vec3 v0=m_joints[i].body.v,w0=m_joints[i].body.w;
#endif
			SyncJointWithBody(i,2);
			if (m_iSimTypeCur==0)
				SyncBodyWithJoint(i,2);
#ifdef _DEBUG
float diff = (m_joints[i].body.v-v0).len()+(m_joints[i].body.w-w0).len();
__maxdiff = max(diff,__maxdiff);
if (m_joints[i].iParent>=0)	{
	Vec3 pivot[2],dpivot,dvel; int j = m_joints[i].iParent;
	pivot[0] = m_joints[j].body.pos + m_joints[j].quat*m_joints[i].pivot[0];
	pivot[1] = m_joints[i].body.pos + m_joints[i].quat*m_joints[i].pivot[1];
	dpivot = pivot[0]-pivot[1];
	dvel = m_joints[i].body.v + (m_joints[i].body.w^pivot[1]-m_joints[i].body.pos) -
				 m_joints[j].body.v - (m_joints[j].body.w^pivot[0]-m_joints[j].body.pos);
	float diff1 = (dvel-dpivot*10.0f).len();
	pivot[0].zero();
}
#endif 
			/*for(j=iPersistentContacts=0; j<NMASKBITS && getmask(j)<=m_joints[i].iContacts; j++) 
			if (m_joints[i].iContacts & getmask(j)) {
				pbody = m_pContacts[j].pbody[0]; vrel = pbody->v+(pbody->w ^ m_pContacts[j].pt-pbody->pos);
				pbody = m_pContacts[j].pbody[1]; vrel -= pbody->v+(pbody->w ^ m_pContacts[j].pt-pbody->pos);
				if (vrel*m_pContacts[j].n<m_pWorld->m_vars.minSeparationSpeed && m_pContacts[j].penetration==0)
					iPersistentContacts |= getmask(j);
				
			}
			m_joints[i].iContacts = iPersistentContacts;*/
		}

		if (m_iSimClass-1!=m_bAwake) {
			m_iSimTypeOverride &= m_bAwake;
			m_iSimClass = 1+m_bAwake;
			m_pWorld->RepositionEntity(this,2);
		}

		if (!m_bAwake) {
			for(i=0;i<m_nJoints;i++) {
				m_joints[i].vSleep=m_joints[i].body.v; m_joints[i].wSleep=m_joints[i].body.w;
				m_joints[i].dq.zero(); m_joints[i].body.P.zero(); m_joints[i].body.L.zero();
				m_joints[i].body.v.zero(); m_joints[i].body.w.zero();
			}
			m_body.P.zero(); m_body.L.zero(); m_body.v.zero(); m_body.w.zero();
		}
		UpdateJointDyn();
	}

	return (m_bAwake^1) | isneg(m_timeStepFull-m_timeStepPerformed-0.001f) | m_pWorld->m_bCurGroupInvisible;
}


void CArticulatedEntity::GetMemoryStatistics(ICrySizer *pSizer)
{
	CRigidEntity::GetMemoryStatistics(pSizer);
	if (GetType()==PE_ARTICULATED)
		pSizer->AddObject(this, sizeof(CArticulatedEntity));
	pSizer->AddObject(m_joints, m_nJointsAlloc*sizeof(m_joints[0]));
	pSizer->AddObject(m_infos, m_nPartsAlloc*sizeof(m_infos[0]));
}