Пример #1
0
dgCollision::dgCollision (dgWorld* const world, dgDeserialize deserialization, void* const userData)
{
	dgInt32 signature[4];
	deserialization (userData, &signature, sizeof (signature));
	deserialization (userData, &m_offset, sizeof (dgMatrix));
	
	m_rtti = 0;
	m_refCount = 1;
	m_signature = dgUnsigned32 (signature[0]);
	m_userDataID = dgUnsigned32 (signature[2]);
	m_allocator = world->GetAllocator();
	m_collsionId = dgCollisionID (signature[1]);
}
Пример #2
0
dgUnsigned32 NewtonUserJoint::JacobianDerivative (dgContraintDescritor & params)
{
   m_rows = 0;
   m_param = &params;
   m_jacobianFnt ((NewtonJoint *)this, params.m_timestep, params.m_threadIndex);
   return dgUnsigned32 (m_rows);
}
Пример #3
0
void dgWorld::InitBody (dgBody* const body, dgCollisionInstance* const collision, const dgMatrix& matrix)
{
	dgAssert (collision);

	m_bodiesUniqueID ++;
	body->m_world = this;

	body->m_spawnnedFromCallback = dgUnsigned32 (m_inUpdate ? true : false);
	body->m_uniqueID = dgInt32 (m_bodiesUniqueID);

	dgBodyMasterList::AddBody(body);

	body->SetCentreOfMass (dgVector (dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (1.0f))); 
	body->SetLinearDamping (dgFloat32 (0.1045f)) ;
	body->SetAngularDamping (dgVector (dgFloat32 (0.1045f), dgFloat32 (0.1045f), dgFloat32 (0.1045f), dgFloat32 (0.0f)));

	body->AttachCollision(collision);
	body->m_bodyGroupId = dgInt32 (m_defualtBodyGroupID);

	dgMatrix inertia (dgGetIdentityMatrix());
	inertia[0][0] = DG_INFINITE_MASS;
	inertia[1][1] = DG_INFINITE_MASS;
	inertia[2][2] = DG_INFINITE_MASS;
	body->SetMassMatrix (DG_INFINITE_MASS * dgFloat32 (2.0f), inertia);
	body->SetMatrix (matrix);
	if (!body->GetCollision()->IsType (dgCollision::dgCollisionNull_RTTI)) {
		m_broadPhase->Add (body);
	}
}
Пример #4
0
dgUnsigned32 dgUpVectorConstraint::JacobianDerivative (dgContraintDescritor& params)
{
	dgMatrix matrix0;
	dgMatrix matrix1;
	CalculateGlobalMatrixAndAngle (matrix0, matrix1);

	dgVector lateralDir (matrix0.m_front * matrix1.m_front);

	dgInt32 ret = 0;
	dgFloat32 mag = lateralDir % lateralDir;
	if (mag > dgFloat32 (1.0e-6f)) {
		mag = dgSqrt (mag);
		lateralDir = lateralDir.Scale3 (dgFloat32 (1.0f) / mag);
		dgFloat32 angle = dgAsin (mag);
		CalculateAngularDerivative (0, params, lateralDir, m_stiffness, angle, &m_jointForce[0]);

		dgVector frontDir (lateralDir * matrix1.m_front);
		CalculateAngularDerivative (1, params, frontDir, m_stiffness, dgFloat32 (0.0f), &m_jointForce[1]);
		ret = 2;
	} else {
		CalculateAngularDerivative (0, params, matrix0.m_up, m_stiffness, 0.0, &m_jointForce[0]);
		CalculateAngularDerivative (1, params, matrix0.m_right, m_stiffness, dgFloat32 (0.0f), &m_jointForce[1]);
		ret = 2;
	}
	return dgUnsigned32 (ret);
}
Пример #5
0
void dgWorld::SetThreadsCount (dgInt32 count)
{
//count = 1;

	m_threadsManager.CreateThreaded (count);
	m_numberOfTheads = dgUnsigned32 (m_threadsManager.GetThreadCount());
	
}
Пример #6
0
dgUnsigned32 dgHingeConstraint::JacobianDerivative (dgContraintDescritor& params)
{
	dgMatrix matrix0;
	dgMatrix matrix1;
	dgVector angle (CalculateGlobalMatrixAndAngle (matrix0, matrix1));

	m_angle = -angle.m_x;

	dgAssert (dgAbsf (1.0f - (matrix0.m_front % matrix0.m_front)) < dgFloat32 (1.0e-5f)); 
	dgAssert (dgAbsf (1.0f - (matrix0.m_up % matrix0.m_up)) < dgFloat32 (1.0e-5f)); 
	dgAssert (dgAbsf (1.0f - (matrix0.m_right % matrix0.m_right)) < dgFloat32 (1.0e-5f)); 

	const dgVector& dir0 = matrix0.m_front;
	const dgVector& dir1 = matrix0.m_up;
	const dgVector& dir2 = matrix0.m_right;

	const dgVector& p0 = matrix0.m_posit;
	const dgVector& p1 = matrix1.m_posit;
	dgVector q0 (p0 + matrix0.m_front.Scale3(MIN_JOINT_PIN_LENGTH));
	dgVector q1 (p1 + matrix1.m_front.Scale3(MIN_JOINT_PIN_LENGTH));

//	dgAssert (((p1 - p0) % (p1 - p0)) < 1.0e-2f);

	dgPointParam pointDataP;
	dgPointParam pointDataQ;
	InitPointParam (pointDataP, m_stiffness, p0, p1);
	InitPointParam (pointDataQ, m_stiffness, q0, q1);

	CalculatePointDerivative (0, params, dir0, pointDataP, &m_jointForce[0]); 
	CalculatePointDerivative (1, params, dir1, pointDataP, &m_jointForce[1]); 
	CalculatePointDerivative (2, params, dir2, pointDataP, &m_jointForce[2]); 
	CalculatePointDerivative (3, params, dir1, pointDataQ, &m_jointForce[3]); 
	CalculatePointDerivative (4, params, dir2, pointDataQ, &m_jointForce[4]); 

	dgInt32 ret = 5;
	if (m_jointAccelFnt) {
		dgJointCallbackParam axisParam;
		axisParam.m_accel = dgFloat32 (0.0f);
		axisParam.m_timestep = params.m_timestep;
		axisParam.m_minFriction = DG_MIN_BOUND;
		axisParam.m_maxFriction = DG_MAX_BOUND;

		if (m_jointAccelFnt (*this, &axisParam)) {
			if ((axisParam.m_minFriction > DG_MIN_BOUND) || (axisParam.m_maxFriction < DG_MAX_BOUND)) {
				params.m_forceBounds[5].m_low = axisParam.m_minFriction;
				params.m_forceBounds[5].m_upper = axisParam.m_maxFriction;
				params.m_forceBounds[5].m_normalIndex = DG_BILATERAL_FRICTION_CONSTRAINT;
			}

			CalculateAngularDerivative (5, params, dir0, m_stiffness, dgFloat32 (0.0f), &m_jointForce[5]);
//			params.m_jointAccel[5] = axisParam.m_accel;
			SetMotorAcceleration (5, axisParam.m_accel, params);
			ret = 6;
		}
	}

	return dgUnsigned32 (ret);
}
Пример #7
0
dgBody* dgWorld::CreateBody(dgCollision* const collision, const dgMatrix& matrix)
{
	dgBody* body;

	_ASSERTE (collision);

	body = new (m_allocator) dgBody();
	_ASSERTE ((sizeof (dgBody) & 0xf) == 0);
	_ASSERTE ((dgUnsigned64 (body) & 0xf) == 0);

	memset (body, 0, sizeof (dgBody));

//	m_bodiesCount ++;
	m_bodiesUniqueID ++;


	body->m_world = this;

	body->m_freeze = false;
	body->m_sleeping = false;
	body->m_autoSleep = true;
	body->m_isInWorld = true;
	body->m_equilibrium = false;
	body->m_continueCollisionMode = false;
	body->m_collideWithLinkedBodies = true;
	body->m_solverInContinueCollision = false;
	body->m_spawnnedFromCallback = dgUnsigned32 (m_inUpdate ? true : false);
	body->m_uniqueID = dgInt32 (m_bodiesUniqueID);


	dgBodyMasterList::AddBody(body);

//	dgBodyActiveList___::AddBody(body);
//	_ASSERTE (body->m_activeNode);


//	body->m_freezeAccel2 = m_freezeAccel2;
//	body->m_freezeAlpha2 = m_freezeAlpha2;
//	body->m_freezeSpeed2 = m_freezeSpeed2;
//	body->m_freezeOmega2 = m_freezeOmega2;

	body->SetCentreOfMass (dgVector (dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (1.0f))); 
	body->SetLinearDamping (dgFloat32 (0.1045f)) ;
	body->SetAngularDamping (dgVector (dgFloat32 (0.1045f), dgFloat32 (0.1045f), dgFloat32 (0.1045f), dgFloat32 (0.0f)));

	body->AttachCollision(collision);
	body->m_bodyGroupId = dgInt32 (m_defualtBodyGroupID);

	body->SetMassMatrix (DG_INFINITE_MASS * dgFloat32 (2.0f), DG_INFINITE_MASS, DG_INFINITE_MASS, DG_INFINITE_MASS);
	dgBroadPhaseCollision::Add (body);

	//body->SetMatrix (dgGetIdentityMatrix());
	body->SetMatrix (matrix);
	body->m_invWorldInertiaMatrix[3][3] = dgFloat32 (1.0f);
	return body;
}
Пример #8
0
dgUnsigned32 dgThreads::GetPerfomanceTicks(dgUnsigned32 threadIndex) const
{

  if (dgInt32(threadIndex) <= m_numOfThreads)
  {
    return dgUnsigned32(m_localData[threadIndex].m_ticks);
  }
  else
  {
    return 0;
  }
}
Пример #9
0
dgUserConstraint::dgUserConstraint(dgWorld* const world, dgBody* const body0, dgBody* const body1, dgInt32 constraintID)
	:dgBilateralConstraint()
{
	m_maxDOF = 6;	
	m_enableCollision = false;
	m_constId = dgUnsigned32 (m_unknownConstraint + constraintID);
	m_body0 = body0;
	m_body1 = body1;
	m_userData = NULL;
	m_destructor = NULL;

	world->AttachConstraint(this, body0, body1);
}
Пример #10
0
dgUnsigned32 dgContact::JacobianDerivative (dgContraintDescritor& params)
{
	dgInt32 frictionIndex = 0;
	if (m_maxDOF) {
		dgInt32 i = 0;
		frictionIndex = GetCount();
		for (dgList<dgContactMaterial>::dgListNode* node = GetFirst(); node; node = node->GetNext()) {
			const dgContactMaterial& contact = node->GetInfo(); 
			JacobianContactDerivative (params, contact, i, frictionIndex);
			i ++;
		}
	}

	return dgUnsigned32 (frictionIndex);
}
Пример #11
0
void dgWorld::InitBody (dgBody* const body, dgCollisionInstance* const collision, const dgMatrix& matrix)
{
	dgAssert (collision);

	m_bodiesUniqueID ++;
	body->m_world = this;

	body->m_spawnnedFromCallback = dgUnsigned32 (m_inUpdate ? true : false);
	body->m_uniqueID = dgInt32 (m_bodiesUniqueID);

	dgBodyMasterList::AddBody(body);

	body->SetCentreOfMass (dgVector (dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (1.0f))); 
	body->SetLinearDamping (dgFloat32 (0.1045f)) ;
	body->SetAngularDamping (dgVector (dgFloat32 (0.1045f), dgFloat32 (0.1045f), dgFloat32 (0.1045f), dgFloat32 (0.0f)));

	body->AttachCollision(collision);
	body->m_bodyGroupId = dgInt32 (m_defualtBodyGroupID);

	body->SetMassMatrix (DG_INFINITE_MASS * dgFloat32 (2.0f), DG_INFINITE_MASS, DG_INFINITE_MASS, DG_INFINITE_MASS);
	body->SetMatrix (matrix);
	m_broadPhase->Add (body);
}
Пример #12
0
dgUnsigned32 dgBallConstraint::JacobianDerivative(dgContraintDescritor& params)
{
  dgInt32 ret;
  dgFloat32 relVelocErr;
  dgFloat32 penetrationErr;
  dgMatrix matrix0;
  dgMatrix matrix1;

  if (m_jointUserCallback)
  {
    m_jointUserCallback(*this, params.m_timestep);
  }

  dgVector angle(CalculateGlobalMatrixAndAngle(matrix0, matrix1));
  m_angles = angle.Scale(-dgFloat32(1.0f));

  const dgVector& dir0 = matrix0.m_front;
  const dgVector& dir1 = matrix0.m_up;
  const dgVector& dir2 = matrix0.m_right;
  const dgVector& p0 = matrix0.m_posit;
  const dgVector& p1 = matrix1.m_posit;

  dgPointParam pointData;
  InitPointParam(pointData, m_stiffness, p0, p1);
  CalculatePointDerivative(0, params, dir0, pointData, &m_jointForce[0]);
  CalculatePointDerivative(1, params, dir1, pointData, &m_jointForce[1]);
  CalculatePointDerivative(2, params, dir2, pointData, &m_jointForce[2]);
  ret = 3;

  if (m_twistLimit)
  {
    if (angle.m_x > m_twistAngle)
    {
      dgVector p0(matrix0.m_posit + matrix0.m_up.Scale(MIN_JOINT_PIN_LENGTH));
      InitPointParam(pointData, m_stiffness, p0, p0);

      const dgVector& dir = matrix0.m_right;
      CalculatePointDerivative(ret, params, dir, pointData, &m_jointForce[ret]);

      dgVector velocError(pointData.m_veloc1 - pointData.m_veloc0);
      relVelocErr = velocError % dir;
      if (relVelocErr > dgFloat32(1.0e-3f))
      {
        relVelocErr *= dgFloat32(1.1f);
      }

      penetrationErr = MIN_JOINT_PIN_LENGTH * (angle.m_x - m_twistAngle);
      _ASSERTE(penetrationErr >= dgFloat32 (0.0f));

      params.m_forceBounds[ret].m_low = dgFloat32(0.0f);
      params.m_forceBounds[ret].m_normalIndex = DG_NORMAL_CONSTRAINT;
      params.m_forceBounds[ret].m_jointForce = &m_jointForce[ret];
//			params.m_jointAccel[ret] = (relVelocErr + penetrationErr) * params.m_invTimestep;
      SetMotorAcceleration(ret,
          (relVelocErr + penetrationErr) * params.m_invTimestep, params);
      ret++;
    }
    else if (angle.m_x < -m_twistAngle)
    {
      dgVector p0(matrix0.m_posit + matrix0.m_up.Scale(MIN_JOINT_PIN_LENGTH));
      InitPointParam(pointData, m_stiffness, p0, p0);
      dgVector dir(matrix0.m_right.Scale(-dgFloat32(1.0f)));
      CalculatePointDerivative(ret, params, dir, pointData, &m_jointForce[ret]);

      dgVector velocError(pointData.m_veloc1 - pointData.m_veloc0);
      relVelocErr = velocError % dir;
      if (relVelocErr > dgFloat32(1.0e-3f))
      {
        relVelocErr *= dgFloat32(1.1f);
      }

      penetrationErr = MIN_JOINT_PIN_LENGTH * (-m_twistAngle - angle.m_x);
      _ASSERTE(penetrationErr >= dgFloat32 (0.0f));

      params.m_forceBounds[ret].m_low = dgFloat32(0.0f);
      params.m_forceBounds[ret].m_normalIndex = DG_NORMAL_CONSTRAINT;
      params.m_forceBounds[ret].m_jointForce = &m_jointForce[ret];
//			params.m_jointAccel[ret] = (relVelocErr + penetrationErr) * params.m_invTimestep;
      SetMotorAcceleration(ret,
          (relVelocErr + penetrationErr) * params.m_invTimestep, params);
      ret++;
    }
  }

  if (m_coneLimit)
  {

    dgFloat32 coneCos;
    coneCos = matrix0.m_front % matrix1.m_front;
    if (coneCos < m_coneAngleCos)
    {
      dgVector p0(
          matrix0.m_posit + matrix0.m_front.Scale(MIN_JOINT_PIN_LENGTH));
      InitPointParam(pointData, m_stiffness, p0, p0);

      dgVector tangentDir(matrix0.m_front * matrix1.m_front);
      tangentDir = tangentDir.Scale(
          dgRsqrt ((tangentDir % tangentDir) + 1.0e-8f));
      CalculatePointDerivative(ret, params, tangentDir, pointData,
          &m_jointForce[ret]);
      ret++;

      dgVector normalDir(tangentDir * matrix0.m_front);

      dgVector velocError(pointData.m_veloc1 - pointData.m_veloc0);
      //restitution = contact.m_restitution;
      relVelocErr = velocError % normalDir;
      if (relVelocErr > dgFloat32(1.0e-3f))
      {
        relVelocErr *= dgFloat32(1.1f);
      }

      penetrationErr = MIN_JOINT_PIN_LENGTH
          * (dgAcos (GetMax (coneCos, dgFloat32(-0.9999f))) - m_coneAngle);
      _ASSERTE(penetrationErr >= dgFloat32 (0.0f));

      CalculatePointDerivative(ret, params, normalDir, pointData,
          &m_jointForce[ret]);
      params.m_forceBounds[ret].m_low = dgFloat32(0.0f);
      params.m_forceBounds[ret].m_normalIndex = DG_NORMAL_CONSTRAINT;
      params.m_forceBounds[ret].m_jointForce = &m_jointForce[ret];
//			params.m_jointAccel[ret] = (relVelocErr + penetrationErr) * params.m_invTimestep;
      SetMotorAcceleration(ret,
          (relVelocErr + penetrationErr) * params.m_invTimestep, params);
      ret++;
    }
  }

  return dgUnsigned32(ret);
}
Пример #13
0
dgInt32 dgPolygonSoupDatabaseBuilder::AddConvexFace(dgInt32 count,
    dgInt32* const pool, dgInt32* const facesArray)
{
  dgPolySoupFilterAllocator polyhedra(m_allocator);

  count = polyhedra.AddFilterFace(dgUnsigned32(count), pool);

  dgEdge* edge = &polyhedra.GetRoot()->GetInfo();
  if (edge->m_incidentFace < 0)
  {
    edge = edge->m_twin;
  }

  dgInt32 isconvex = 1;
  dgInt32 facesCount = 0;

  dgInt32 flag = 1;
  while (flag)
  {
    flag = 0;
    if (count >= 3)
    {
      dgEdge* ptr = edge;

      dgBigVector p0(&m_vertexPoints[ptr->m_incidentVertex].m_x);
      do
      {
        dgBigVector p1(&m_vertexPoints[ptr->m_next->m_incidentVertex].m_x);
        dgBigVector e0(p1 - p0);
        dgFloat64 mag2 = e0 % e0;
        if (mag2 < dgFloat32(1.0e-6f))
        {
          count--;
          flag = 1;
          edge = ptr->m_next;
          ptr->m_prev->m_next = ptr->m_next;
          ptr->m_next->m_prev = ptr->m_prev;
          ptr->m_twin->m_next->m_prev = ptr->m_twin->m_prev;
          ptr->m_twin->m_prev->m_next = ptr->m_twin->m_next;
          break;
        }
        p0 = p1;
        ptr = ptr->m_next;
      } while (ptr != edge);
    }
  }
  if (count >= 3)
  {
    flag = 1;

    while (flag)
    {
      flag = 0;
      if (count >= 3)
      {
        dgEdge* ptr = edge;

        dgBigVector p0(&m_vertexPoints[ptr->m_prev->m_incidentVertex].m_x);
        dgBigVector p1(&m_vertexPoints[ptr->m_incidentVertex].m_x);
        dgBigVector e0(p1 - p0);
        e0 = e0.Scale(dgRsqrt (e0 % e0 + dgFloat32(1.0e-10f)));
        do
        {
          dgBigVector p2(&m_vertexPoints[ptr->m_next->m_incidentVertex].m_x);
          dgBigVector e1(p2 - p1);

          e1 = e1.Scale(dgRsqrt (e1 % e1 + dgFloat32(1.0e-10f)));
          dgFloat64 mag2 = e1 % e0;
          if (mag2 > dgFloat32(0.9999f))
          {
            count--;
            flag = 1;
            edge = ptr->m_next;
            ptr->m_prev->m_next = ptr->m_next;
            ptr->m_next->m_prev = ptr->m_prev;
            ptr->m_twin->m_next->m_prev = ptr->m_twin->m_prev;
            ptr->m_twin->m_prev->m_next = ptr->m_twin->m_next;
            break;
          }

          e0 = e1;
          p1 = p2;
          ptr = ptr->m_next;
        } while (ptr != edge);
      }
    }

    dgBigVector normal(
        polyhedra.FaceNormal(edge, &m_vertexPoints[0].m_x,
            sizeof(dgBigVector)));
    dgFloat64 mag2 = normal % normal;
    if (mag2 < dgFloat32(1.0e-8f))
    {
      return 0;
    }
    normal = normal.Scale(dgRsqrt (mag2));

    if (count >= 3)
    {
      dgEdge* ptr = edge;
      dgBigVector p0(&m_vertexPoints[ptr->m_prev->m_incidentVertex].m_x);
      dgBigVector p1(&m_vertexPoints[ptr->m_incidentVertex].m_x);
      dgBigVector e0(p1 - p0);
      e0 = e0.Scale(dgRsqrt (e0 % e0 + dgFloat32(1.0e-10f)));
      do
      {
        dgBigVector p2(&m_vertexPoints[ptr->m_next->m_incidentVertex].m_x);
        dgBigVector e1(p2 - p1);

        e1 = e1.Scale(dgRsqrt (e1 % e1 + dgFloat32(1.0e-10f)));

        dgBigVector n(e0 * e1);
        dgFloat64 mag2 = n % normal;
        if (mag2 < dgFloat32(1.0e-5f))
        {
          isconvex = 0;
          break;
        }

        e0 = e1;
        p1 = p2;
        ptr = ptr->m_next;
      } while (ptr != edge);
    }
  }

  if (isconvex)
  {
    dgEdge* const first = edge;
    if (count >= 3)
    {
      count = 0;
      dgEdge* ptr = first;
      do
      {
        pool[count] = ptr->m_incidentVertex;
        count++;
        ptr = ptr->m_next;
      } while (ptr != first);
      facesArray[facesCount] = count;
      facesCount = 1;
    }
  }
  else
  {
    dgPolyhedra leftOver(m_allocator);
    dgPolyhedra polyhedra2(m_allocator);
    dgEdge* ptr = edge;
    count = 0;
    do
    {
      pool[count] = ptr->m_incidentVertex;
      count++;
      ptr = ptr->m_next;
    } while (ptr != edge);

    polyhedra2.BeginFace();
    polyhedra2.AddFace(count, pool);
    polyhedra2.EndFace();
    leftOver.BeginFace();
    polyhedra2.ConvexPartition(&m_vertexPoints[0].m_x, sizeof(dgTriplex),
        &leftOver);
    leftOver.EndFace();
    _ASSERTE(leftOver.GetCount() == 0);

    dgInt32 mark = polyhedra2.IncLRU();
    dgInt32 index = 0;
    dgPolyhedra::Iterator iter(polyhedra2);
    for (iter.Begin(); iter; iter++)
    {
      dgEdge* const edge = &(*iter);
      if (edge->m_incidentFace < 0)
      {
        continue;
      }
      if (edge->m_mark == mark)
      {
        continue;
      }

      ptr = edge;
      count = 0;
      do
      {
        ptr->m_mark = mark;
        pool[index] = ptr->m_incidentVertex;
        index++;
        count++;
        ptr = ptr->m_next;
      } while (ptr != edge);

      facesArray[facesCount] = count;
      facesCount++;
    }
  }

  return facesCount;
}
Пример #14
0
void dgBallConstraint::SetConeLimitState(bool state)
{
  m_coneLimit = dgUnsigned32(state);
}
Пример #15
0
void dgBallConstraint::SetLatealLimitState(bool state)
{
  m_lateralLimit = dgUnsigned32(state);
}
dgUnsigned32 dgCorkscrewConstraint::JacobianDerivative (dgContraintDescritor& params)
{
	dgMatrix matrix0;
	dgMatrix matrix1;

	dgVector angle (CalculateGlobalMatrixAndAngle (matrix0, matrix1));

	m_angle = -angle.m_x;
	m_posit = (matrix0.m_posit - matrix1.m_posit) % matrix0.m_front;
	matrix1.m_posit += matrix1.m_front.Scale3 (m_posit);

	dgAssert (dgAbsf (dgFloat32 (1.0f) - (matrix0.m_front % matrix0.m_front)) < dgFloat32 (1.0e-5f)); 
	dgAssert (dgAbsf (dgFloat32 (1.0f) - (matrix0.m_up % matrix0.m_up)) < dgFloat32 (1.0e-5f)); 
	dgAssert (dgAbsf (dgFloat32 (1.0f) - (matrix0.m_right % matrix0.m_right)) < dgFloat32 (1.0e-5f)); 

	const dgVector& dir1 = matrix0.m_up;
	const dgVector& dir2 = matrix0.m_right;

//	const dgVector& p0 = matrix0.m_posit;
//	const dgVector& p1 = matrix1.m_posit;
	dgVector p0 (matrix0.m_posit);
	dgVector p1 (matrix1.m_posit + matrix1.m_front.Scale3 ((p0 - matrix1.m_posit) % matrix1.m_front));

	dgVector q0 (p0 + matrix0.m_front.Scale3(MIN_JOINT_PIN_LENGTH));
	dgVector q1 (p1 + matrix1.m_front.Scale3(MIN_JOINT_PIN_LENGTH));

	dgPointParam pointDataP;
	dgPointParam pointDataQ;
	InitPointParam (pointDataP, m_stiffness, p0, p1);
	InitPointParam (pointDataQ, m_stiffness, q0, q1);

	CalculatePointDerivative (0, params, dir1, pointDataP, &m_jointForce[0]); 
	CalculatePointDerivative (1, params, dir2, pointDataP, &m_jointForce[1]); 
	CalculatePointDerivative (2, params, dir1, pointDataQ, &m_jointForce[2]); 
	CalculatePointDerivative (3, params, dir2, pointDataQ, &m_jointForce[3]); 

	dgInt32 ret = 4;
	if (m_jointAccelFnt) {
		dgUnsigned32 code;
		dgJointCallbackParam axisParam[2];

		// linear acceleration
		axisParam[0].m_accel = dgFloat32 (0.0f);
		axisParam[0].m_timestep = params.m_timestep;
		axisParam[0].m_minFriction = DG_MIN_BOUND;
		axisParam[0].m_maxFriction = DG_MAX_BOUND;

		// angular acceleration
		axisParam[1].m_accel = dgFloat32 (0.0f);
		axisParam[1].m_timestep = params.m_timestep;
		axisParam[1].m_minFriction = DG_MIN_BOUND;
		axisParam[1].m_maxFriction = DG_MAX_BOUND;

		code = m_jointAccelFnt (*this, axisParam);
		if (code & 1) {
			if ((axisParam[0].m_minFriction > DG_MIN_BOUND) || (axisParam[0].m_maxFriction < DG_MAX_BOUND)) {
				params.m_forceBounds[ret].m_low = axisParam[0].m_minFriction;
				params.m_forceBounds[ret].m_upper = axisParam[0].m_maxFriction;
				params.m_forceBounds[ret].m_normalIndex = DG_BILATERAL_FRICTION_CONSTRAINT;
			}

			CalculatePointDerivative (ret, params, matrix0.m_front, pointDataP, &m_jointForce[ret]); 
			//params.m_jointAccel[ret] = axisParam[0].m_accel;
			SetMotorAcceleration (ret, axisParam[0].m_accel, params);
			ret ++;
		}


		if (code & 2) {
			if ((axisParam[1].m_minFriction > DG_MIN_BOUND) || (axisParam[1].m_maxFriction < DG_MAX_BOUND)) {
				params.m_forceBounds[ret].m_low = axisParam[1].m_minFriction;
				params.m_forceBounds[ret].m_upper = axisParam[1].m_maxFriction;
				params.m_forceBounds[ret].m_normalIndex = DG_BILATERAL_FRICTION_CONSTRAINT;
			}

//			dgVector p (p0 +  dir1);
//			dgPointParam pointData;
//			InitPointParam (pointData, m_stiffness, p, p);
//			CalculatePointDerivative (ret, params, dir2, pointData, &m_jointForce[ret]); 
			CalculateAngularDerivative (ret, params, matrix0.m_front, m_stiffness, dgFloat32 (0.0f), &m_jointForce[ret]);
			//params.m_jointAccel[ret] = axisParam[1].m_accel;
			SetMotorAcceleration (ret, axisParam[1].m_accel, params);
			ret ++;
		}
	}

	return dgUnsigned32 (ret);
}
Пример #17
0
void dgBallConstraint::SetTwistLimitState(bool state)
{
  m_twistLimit = dgUnsigned32(state);
}
Пример #18
0
void dgCollidingPairCollector::AddPair (dgBody* const bodyPtr0, dgBody* const bodyPtr1, dgInt32 threadIndex)
{
	if ((bodyPtr0 != m_sentinel) && (bodyPtr1 != m_sentinel)) {
		dgWorld* const world = (dgWorld*) this;
		if (bodyPtr0->GetSleepState() & bodyPtr1->GetSleepState()) {
			dgContact* contact = NULL;
			if (bodyPtr0->m_invMass.m_w != dgFloat32 (0.0f)) {
				for (dgBodyMasterListRow::dgListNode* link = world->FindConstraintLink (bodyPtr0, bodyPtr1); link; link = world->FindConstraintLinkNext (link, bodyPtr1)) {
					dgConstraint* const constraint = link->GetInfo().m_joint;
					if (constraint->GetId() == dgContactConstraintId) {
						contact = (dgContact*)constraint;
						break;
					}
				}
			} else if (bodyPtr1->m_invMass.m_w != dgFloat32 (0.0f)) {
				_ASSERTE (bodyPtr1->m_invMass.m_w != dgFloat32 (0.0f));
				for (dgBodyMasterListRow::dgListNode* link = world->FindConstraintLink (bodyPtr1, bodyPtr0); link; link = world->FindConstraintLinkNext (link, bodyPtr0)) {
					dgConstraint* const constraint = link->GetInfo().m_joint;
					if (constraint->GetId() == dgContactConstraintId) {
						contact = (dgContact*)constraint;
						break;
					} 
				}
			} else {
				return;
			}

			if (contact) {
				_ASSERTE (contact->GetId() == dgContactConstraintId);
				contact->m_broadphaseLru = dgInt32 (world->m_broadPhaseLru);
			}
			
		} else {
			dgBody* tmpbody0 (bodyPtr0);
			dgBody* tmpbody1 (bodyPtr1);
			if (tmpbody0->m_uniqueID > tmpbody1->m_uniqueID) {
				Swap(tmpbody0, tmpbody1);
			}
			dgBody* const body0 (tmpbody0);
			dgBody* const body1 (tmpbody1);

			_ASSERTE (body0->GetWorld());
			_ASSERTE (body1->GetWorld());
			_ASSERTE (body0->GetWorld() == world);
			_ASSERTE (body1->GetWorld() == world);

			dgContact* contact = NULL;
			if (body0->m_invMass.m_w != dgFloat32 (0.0f)) {
				for (dgBodyMasterListRow::dgListNode* link = world->FindConstraintLink (body0, body1); link; link = world->FindConstraintLinkNext (link, body1)) {
					dgConstraint* const constraint = link->GetInfo().m_joint;
					if (constraint->GetId() == dgContactConstraintId) {
						contact = (dgContact*)constraint;
					} else {
						if (!constraint->IsCollidable()) {
							return;
						} 
					}
				}
			} else if (body1->m_invMass.m_w != dgFloat32 (0.0f)) {
				_ASSERTE (body1->m_invMass.m_w != dgFloat32 (0.0f));
				for (dgBodyMasterListRow::dgListNode* link = world->FindConstraintLink (body1, body0); link; link = world->FindConstraintLinkNext (link, body0)) {
					dgConstraint* const constraint = link->GetInfo().m_joint;
					if (constraint->GetId() == dgContactConstraintId) {
						contact = (dgContact*)constraint;
					} else {
						if (!constraint->IsCollidable()) {
							return;
						}
					}
				}
			} else {
				return;
			}

			if (!(body0->m_collideWithLinkedBodies & body1->m_collideWithLinkedBodies)) {
				if (world->AreBodyConnectedByJoints (body0, body1)) {
					return;
				}
			}

		
			_ASSERTE (!contact || contact->GetId() == dgContactConstraintId);

			dgUnsigned32 group0_ID = dgUnsigned32 (body0->m_bodyGroupId);
			dgUnsigned32 group1_ID = dgUnsigned32 (body1->m_bodyGroupId);
			if (group1_ID < group0_ID) {
				Swap (group0_ID, group1_ID);
			}

			dgUnsigned32 key = (group1_ID << 16) + group0_ID;
			const dgBodyMaterialList& materialList = *world;  

			const dgContactMaterial* const material = &materialList.Find (key)->GetInfo();
//			if (material->m_collisionEnable) {
			if (material->m_flags & dgContactMaterial::m_collisionEnable__) {
				dgInt32 processContacts;

				processContacts = 1;
				if (material->m_aabbOverlap) {
					processContacts = material->m_aabbOverlap (*material, *body0, *body1, threadIndex);
				}
				if (processContacts) {
					_ASSERTE (!body0->m_collision->IsType (dgCollision::dgCollisionNull_RTTI));
					_ASSERTE (!body1->m_collision->IsType (dgCollision::dgCollisionNull_RTTI));

					dgThreadPairCache& pairChache = *m_chacheBuffers[threadIndex];
					
					if (pairChache.m_count >= DG_CACHE_PAIR_BUFFER) {
						world->dgGetUserLock();
						FlushChache (&pairChache);
						world->dgReleasedUserLock();
					}

					dgInt32 count = pairChache.m_count;
					pairChache.m_chacheBuffer[count].m_body0 = body0;
					pairChache.m_chacheBuffer[count].m_body1 = body1;
					pairChache.m_chacheBuffer[count].m_material = material; 
					pairChache.m_chacheBuffer[count].m_contact = contact;
					pairChache.m_count = count + 1;
				}
			}
		}
	}
}
dgCollisionInstance::dgCollisionInstance(const dgWorld* const constWorld, dgDeserialize serialize, void* const userData, dgInt32 revisionNumber)
	:m_globalMatrix(dgGetIdentityMatrix())
	,m_localMatrix (dgGetIdentityMatrix())
	,m_aligmentMatrix (dgGetIdentityMatrix())
	,m_scale(dgFloat32 (1.0f), dgFloat32 (1.0f), dgFloat32 (1.0f), dgFloat32 (0.0f))
	,m_invScale(dgFloat32 (1.0f), dgFloat32 (1.0f), dgFloat32 (1.0f), dgFloat32 (0.0f))
	,m_maxScale(dgFloat32 (1.0f), dgFloat32 (1.0f), dgFloat32 (1.0f), dgFloat32 (0.0f))
	,m_material()
	,m_world(constWorld)
	,m_childShape (NULL)
	,m_subCollisionHandle(NULL)
	,m_parent(NULL)
	,m_skinThickness(dgFloat32 (0.0f))
	,m_collisionMode(1)
	,m_refCount(1)
	,m_scaleType(m_unit)
{
	dgInt32 saved;
	dgInt32 signature;
	dgInt32 primitive;
	dgInt32 scaleType;
	
	serialize (userData, &m_globalMatrix, sizeof (m_globalMatrix));
	serialize (userData, &m_localMatrix, sizeof (m_localMatrix));
	serialize (userData, &m_aligmentMatrix, sizeof (m_aligmentMatrix));
	serialize (userData, &m_scale, sizeof (m_scale));
	serialize (userData, &m_invScale, sizeof (m_invScale));
	serialize (userData, &m_maxScale, sizeof (m_maxScale));
	serialize (userData, &m_skinThickness, sizeof (m_skinThickness));
	serialize (userData, &m_material, sizeof (m_material));
	serialize (userData, &m_collisionMode, sizeof (m_collisionMode));
	serialize (userData, &scaleType, sizeof (scaleType));
	serialize (userData, &primitive, sizeof (primitive));
	serialize (userData, &signature, sizeof (signature));
	serialize (userData, &saved, sizeof (saved));

	m_scaleType = dgScaleType(scaleType);

	dgWorld* const world = (dgWorld*) constWorld;
	if (saved) {
		const dgCollision* collision = NULL;
		dgBodyCollisionList::dgTreeNode* node = world->dgBodyCollisionList::Find (dgUnsigned32 (signature));

		if (node) {
			collision = node->GetInfo();
			collision->AddRef();

		} else {

			dgCollisionID primitiveType = dgCollisionID(primitive);

			dgMemoryAllocator* const allocator = world->GetAllocator();
			switch (primitiveType)
			{
				case m_heightField:
				{
					collision = new (allocator) dgCollisionHeightField (world, serialize, userData, revisionNumber);
					break;
				}

				case m_boundingBoxHierachy:
				{
					collision = new (allocator) dgCollisionBVH (world, serialize, userData, revisionNumber);
					break;
				}

				case m_compoundCollision:
				{
					collision = new (allocator) dgCollisionCompound (world, serialize, userData, this, revisionNumber);
					break;
				}

				case m_compoundFracturedCollision:
				{
					collision = new (allocator) dgCollisionCompoundFractured (world, serialize, userData, this, revisionNumber);
					break;
				}

				case m_sceneCollision:
				{
					collision = new (allocator) dgCollisionScene (world, serialize, userData, this, revisionNumber);
					break;
				}


				case m_sphereCollision:
				{
					collision = new (allocator) dgCollisionSphere (world, serialize, userData, revisionNumber);
					node = world->dgBodyCollisionList::Insert (collision, collision->GetSignature());
					collision->AddRef();
					break;
				}

				case m_boxCollision:
				{
					collision = new (allocator) dgCollisionBox (world, serialize, userData, revisionNumber);
					node = world->dgBodyCollisionList::Insert (collision, collision->GetSignature());
					collision->AddRef();
					break;
				}

				case m_coneCollision:
				{
					collision = new (allocator) dgCollisionCone (world, serialize, userData, revisionNumber);
					node = world->dgBodyCollisionList::Insert (collision, collision->GetSignature());
					collision->AddRef();
					break;
				}

				case m_capsuleCollision:
				{
					collision = new (allocator) dgCollisionCapsule (world, serialize, userData, revisionNumber);
					node = world->dgBodyCollisionList::Insert (collision, collision->GetSignature());
					collision->AddRef();
					break;
				}

				case m_cylinderCollision:
				{
					collision = new (allocator) dgCollisionCylinder (world, serialize, userData, revisionNumber);
					node = world->dgBodyCollisionList::Insert (collision, collision->GetSignature());
					collision->AddRef();
					break;
				}

				case m_chamferCylinderCollision:
				{
					collision = new (allocator) dgCollisionChamferCylinder (world, serialize, userData, revisionNumber);
					node = world->dgBodyCollisionList::Insert (collision, collision->GetSignature());
					collision->AddRef();
					break;
				}

				case m_convexHullCollision:
				{
					collision = new (allocator) dgCollisionConvexHull (world, serialize, userData, revisionNumber);
					node = world->dgBodyCollisionList::Insert (collision, collision->GetSignature());
					collision->AddRef();
					break;
				}

				case m_nullCollision:
				{
					collision = new (allocator) dgCollisionNull (world, serialize, userData, revisionNumber);
					node = world->dgBodyCollisionList::Insert (collision, collision->GetSignature());
					collision->AddRef();
					break;
				}

//				case m_deformableMesh:
//				{
//					dgAssert (0);
//					return NULL;
//				}

				default:
				dgAssert (0);
			}
		}
		m_childShape = collision;
	}
	dgDeserializeMarker (serialize, userData);
}
Пример #20
0
//dgUnsigned32 dgWorld::GetPerfomanceTicks (dgInt32 thread, dgUnsigned32 entry) const
dgUnsigned32 dgWorld::GetPerfomanceTicks (dgUnsigned32 entry) const
{
	entry = ClampValue(dgUnsigned32 (entry), dgUnsigned32 (0), dgUnsigned32 (m_counterSize - 1));
	return m_perfomanceCounters[entry];

}
void dgWorldDynamicUpdate::ResolveClusterForces(dgBodyCluster* const cluster, dgInt32 threadID, dgFloat32 timestep) const
{
	if (cluster->m_activeJointCount) {
		SortClusters(cluster, timestep, threadID);
	}

	if (!cluster->m_isContinueCollision) {
		if (cluster->m_activeJointCount) {
			BuildJacobianMatrix (cluster, threadID, timestep);
			CalculateClusterReactionForces(cluster, threadID, timestep, DG_SOLVER_MAX_ERROR);
			//CalculateClusterReactionForces_1(cluster, threadID, timestep, DG_SOLVER_MAX_ERROR);
		} else {
			IntegrateExternalForce(cluster, timestep, threadID);
		}
		IntegrateVelocity (cluster, DG_SOLVER_MAX_ERROR, timestep, threadID); 
	} else {
		// calculate reaction forces and new velocities
		BuildJacobianMatrix (cluster, threadID, timestep);
		IntegrateReactionsForces (cluster, threadID, timestep, DG_SOLVER_MAX_ERROR);

		// see if the island goes to sleep
		bool isAutoSleep = true;
		bool stackSleeping = true;
		dgInt32 sleepCounter = 10000;

		dgWorld* const world = (dgWorld*) this;
		const dgInt32 bodyCount = cluster->m_bodyCount;
		dgBodyInfo* const bodyArrayPtr = (dgBodyInfo*) &world->m_bodiesMemory[0]; 
		dgBodyInfo* const bodyArray = &bodyArrayPtr[cluster->m_bodyStart];

		const dgFloat32 forceDamp = DG_FREEZZING_VELOCITY_DRAG;
		dgFloat32 maxAccel = dgFloat32 (0.0f);
		dgFloat32 maxAlpha = dgFloat32 (0.0f);
		dgFloat32 maxSpeed = dgFloat32 (0.0f);
		dgFloat32 maxOmega = dgFloat32 (0.0f);

		const dgFloat32 speedFreeze = world->m_freezeSpeed2;
		const dgFloat32 accelFreeze = world->m_freezeAccel2;
		const dgVector forceDampVect (forceDamp, forceDamp, forceDamp, dgFloat32 (0.0f));
		for (dgInt32 i = 1; i < bodyCount; i ++) {
			dgDynamicBody* const body = (dgDynamicBody*) bodyArray[i].m_body;
			if (body->IsRTTIType (dgBody::m_dynamicBodyRTTI)) {
				dgAssert (body->m_invMass.m_w);

				const dgFloat32 accel2 = body->m_accel.DotProduct3(body->m_accel);
				const dgFloat32 alpha2 = body->m_alpha.DotProduct3(body->m_alpha);
				const dgFloat32 speed2 = body->m_veloc.DotProduct3(body->m_veloc);
				const dgFloat32 omega2 = body->m_omega.DotProduct3(body->m_omega);

				maxAccel = dgMax (maxAccel, accel2);
				maxAlpha = dgMax (maxAlpha, alpha2);
				maxSpeed = dgMax (maxSpeed, speed2);
				maxOmega = dgMax (maxOmega, omega2);

				bool equilibrium = (accel2 < accelFreeze) && (alpha2 < accelFreeze) && (speed2 < speedFreeze) && (omega2 < speedFreeze);
				if (equilibrium) {
					dgVector veloc (body->m_veloc * forceDampVect);
					dgVector omega = body->m_omega * forceDampVect;
					body->m_veloc = (dgVector (veloc.DotProduct4(veloc)) > m_velocTol) & veloc;
					body->m_omega = (dgVector (omega.DotProduct4(omega)) > m_velocTol) & omega;

				}
				body->m_equilibrium = dgUnsigned32 (equilibrium);
				stackSleeping &= equilibrium;
				isAutoSleep &= body->m_autoSleep;

				sleepCounter = dgMin (sleepCounter, body->m_sleepingCounter);
			}
			// clear accel and angular acceleration
			body->m_accel = dgVector::m_zero;
			body->m_alpha = dgVector::m_zero;
		}

		if (isAutoSleep) {
			if (stackSleeping) {
				// the island went to sleep mode, 
				for (dgInt32 i = 1; i < bodyCount; i ++) {
					dgBody* const body = bodyArray[i].m_body;
					dgAssert (body->IsRTTIType (dgBody::m_dynamicBodyRTTI) || body->IsRTTIType (dgBody::m_kinematicBodyRTTI));
					body->m_accel = dgVector::m_zero;
					body->m_alpha = dgVector::m_zero;
					body->m_veloc = dgVector::m_zero;
					body->m_omega = dgVector::m_zero;
				}
			} else {
				// island is not sleeping but may be resting with small residual velocity for a long time
				// see if we can force to go to sleep
				if ((maxAccel > world->m_sleepTable[DG_SLEEP_ENTRIES - 1].m_maxAccel) ||
					(maxAlpha > world->m_sleepTable[DG_SLEEP_ENTRIES - 1].m_maxAlpha) ||
					(maxSpeed > world->m_sleepTable[DG_SLEEP_ENTRIES - 1].m_maxVeloc) ||
					(maxOmega > world->m_sleepTable[DG_SLEEP_ENTRIES - 1].m_maxOmega)) {
					for (dgInt32 i = 1; i < bodyCount; i ++) {
						dgDynamicBody* const body = (dgDynamicBody*) bodyArray[i].m_body;
						if (body->IsRTTIType (dgBody::m_dynamicBodyRTTI)) {
							body->m_sleepingCounter = 0;
						}
					}
				} else {
					dgInt32 index = 0;
					for (dgInt32 i = 0; i < DG_SLEEP_ENTRIES; i ++) {
						if ((maxAccel <= world->m_sleepTable[i].m_maxAccel) &&
							(maxAlpha <= world->m_sleepTable[i].m_maxAlpha) &&
							(maxSpeed <= world->m_sleepTable[i].m_maxVeloc) &&
							(maxOmega <= world->m_sleepTable[i].m_maxOmega)) {
								index = i;
								break;
						}
					}

					dgInt32 timeScaleSleepCount = dgInt32 (dgFloat32 (60.0f) * sleepCounter * timestep);
					if (timeScaleSleepCount > world->m_sleepTable[index].m_steps) {
						// force island to sleep
						stackSleeping = true;
						for (dgInt32 i = 1; i < bodyCount; i ++) {
							dgBody* const body = bodyArray[i].m_body;
							dgAssert (body->IsRTTIType (dgBody::m_dynamicBodyRTTI) || body->IsRTTIType (dgBody::m_kinematicBodyRTTI));
							body->m_accel = dgVector::m_zero;
							body->m_alpha = dgVector::m_zero;
							body->m_veloc = dgVector::m_zero;
							body->m_omega = dgVector::m_zero;
							body->m_equilibrium = true;
						}
					} else {
						sleepCounter ++;
						for (dgInt32 i = 1; i < bodyCount; i ++) {
							dgDynamicBody* const body = (dgDynamicBody*) bodyArray[i].m_body;
							if (body->IsRTTIType (dgBody::m_dynamicBodyRTTI)) {
								body->m_sleepingCounter = sleepCounter;
							}
						}
					}
				}
			}
		} 


		if (!(isAutoSleep & stackSleeping)) {
			// island is not sleeping, need to integrate island velocity
			const dgUnsigned32 lru = world->GetBroadPhase()->m_lru;
			const dgInt32 jointCount = cluster->m_jointCount;
			dgJointInfo* const constraintArrayPtr = (dgJointInfo*) &world->m_jointsMemory[0];
			dgJointInfo* const constraintArray = &constraintArrayPtr[cluster->m_jointStart];

			dgFloat32 timeRemaining = timestep;
			const dgFloat32 timeTol = dgFloat32 (0.01f) * timestep;
			for (dgInt32 i = 0; (i < DG_MAX_CONTINUE_COLLISON_STEPS) && (timeRemaining > timeTol); i ++) {
				// calculate the closest time to impact 
				dgFloat32 timeToImpact = timeRemaining;
				for (dgInt32 j = 0; (j < jointCount) && (timeToImpact > timeTol); j ++) {
					dgContact* const contact = (dgContact*) constraintArray[j].m_joint;
					if (contact->GetId() == dgConstraint::m_contactConstraint) {
						dgDynamicBody* const body0 = (dgDynamicBody*)contact->m_body0;
						dgDynamicBody* const body1 = (dgDynamicBody*)contact->m_body1;
						if (body0->m_continueCollisionMode | body1->m_continueCollisionMode) {
							dgVector p;
							dgVector q;
							dgVector normal;
							timeToImpact = dgMin (timeToImpact, world->CalculateTimeToImpact (contact, timeToImpact, threadID, p, q, normal, dgFloat32 (-1.0f / 256.0f)));
						}
					}
				}

				if (timeToImpact > timeTol) {
					timeRemaining -= timeToImpact;
					for (dgInt32 j = 1; j < bodyCount; j ++) {
						dgDynamicBody* const body = (dgDynamicBody*) bodyArray[j].m_body;
						if (body->IsRTTIType (dgBody::m_dynamicBodyRTTI)) {
							body->IntegrateVelocity(timeToImpact);
							body->UpdateWorlCollisionMatrix();
						}
					}
				} else {
					if (timeToImpact >= dgFloat32 (-1.0e-5f)) {
						for (dgInt32 j = 1; j < bodyCount; j++) {
							dgDynamicBody* const body = (dgDynamicBody*)bodyArray[j].m_body;
							if (body->IsRTTIType(dgBody::m_dynamicBodyRTTI)) {
								body->IntegrateVelocity(timeToImpact);
								body->UpdateWorlCollisionMatrix();
							}
						}
					}

					CalculateClusterContacts (cluster, timeRemaining, lru, threadID);
					BuildJacobianMatrix (cluster, threadID, 0.0f);
					IntegrateReactionsForces (cluster, threadID, 0.0f, DG_SOLVER_MAX_ERROR);

					bool clusterReceding = true;
					const dgFloat32 step = timestep * dgFloat32 (1.0f / DG_MAX_CONTINUE_COLLISON_STEPS); 
					for (dgInt32 k = 0; (k < DG_MAX_CONTINUE_COLLISON_STEPS) && clusterReceding; k ++) {
						dgFloat32 smallTimeStep = dgMin (step, timeRemaining);
						timeRemaining -= smallTimeStep;
						for (dgInt32 j = 1; j < bodyCount; j ++) {
							dgDynamicBody* const body = (dgDynamicBody*) bodyArray[j].m_body;
							if (body->IsRTTIType (dgBody::m_dynamicBodyRTTI)) {
								body->IntegrateVelocity (smallTimeStep);
								body->UpdateWorlCollisionMatrix();
							}
						}

						clusterReceding = false;
						if (timeRemaining > timeTol) {
							CalculateClusterContacts (cluster, timeRemaining, lru, threadID);

							bool isColliding = false;
							for (dgInt32 j = 0; (j < jointCount) && !isColliding; j ++) {
								dgContact* const contact = (dgContact*) constraintArray[j].m_joint;
								if (contact->GetId() == dgConstraint::m_contactConstraint) {

									const dgBody* const body0 = contact->m_body0;
									const dgBody* const body1 = contact->m_body1;

									const dgVector& veloc0 = body0->m_veloc;
									const dgVector& veloc1 = body1->m_veloc;

									const dgVector& omega0 = body0->m_omega;
									const dgVector& omega1 = body1->m_omega;

									const dgVector& com0 = body0->m_globalCentreOfMass;
									const dgVector& com1 = body1->m_globalCentreOfMass;
									
									for (dgList<dgContactMaterial>::dgListNode* node = contact->GetFirst(); node; node = node->GetNext()) {
										const dgContactMaterial* const contactMaterial = &node->GetInfo();
										dgVector vel0 (veloc0 + omega0.CrossProduct3(contactMaterial->m_point - com0));
										dgVector vel1 (veloc1 + omega1.CrossProduct3(contactMaterial->m_point - com1));
										dgVector vRel (vel0 - vel1);
										dgAssert (contactMaterial->m_normal.m_w == dgFloat32 (0.0f));
										dgFloat32 speed = vRel.DotProduct4(contactMaterial->m_normal).m_w;
										isColliding |= (speed < dgFloat32 (0.0f));
									}
								}
							}
							clusterReceding = !isColliding;
						}
					}
				}
			}

			if (timeRemaining > dgFloat32 (0.0)) {
				for (dgInt32 j = 1; j < bodyCount; j ++) {
					dgDynamicBody* const body = (dgDynamicBody*) bodyArray[j].m_body;
					if (body->IsRTTIType (dgBody::m_dynamicBodyRTTI)) {
						body->IntegrateVelocity(timeRemaining);
						body->UpdateCollisionMatrix (timeRemaining, threadID);
					}
				}
			} else {
				for (dgInt32 j = 1; j < bodyCount; j ++) {
					dgDynamicBody* const body = (dgDynamicBody*) bodyArray[j].m_body;
					if (body->IsRTTIType (dgBody::m_dynamicBodyRTTI)) {
						body->UpdateCollisionMatrix (timestep, threadID);
					}
				}
			}
		}
	}
}
dgUnsigned32 dgUniversalConstraint::JacobianDerivative (dgContraintDescritor& params)
{
	dgInt32 ret;
	dgFloat32 sinAngle;
	dgFloat32 cosAngle;
	dgMatrix matrix0;
	dgMatrix matrix1;

	CalculateGlobalMatrixAndAngle (matrix0, matrix1);

	const dgVector& dir0 = matrix0.m_front;
	const dgVector& dir1 = matrix1.m_up;
	dgVector dir2 (dir0 * dir1);

	dgVector dir3 (dir2 * dir0);
	dir3 = dir3.Scale3 (dgRsqrt (dir3 % dir3));

	const dgVector& p0 = matrix0.m_posit;
	const dgVector& p1 = matrix1.m_posit;

	dgVector q0 (p0 + dir3.Scale3(MIN_JOINT_PIN_LENGTH));
	dgVector q1 (p1 + dir1.Scale3(MIN_JOINT_PIN_LENGTH));

	dgPointParam pointDataP;
	dgPointParam pointDataQ;
	InitPointParam (pointDataP, m_stiffness, p0, p1);
	InitPointParam (pointDataQ, m_stiffness, q0, q1);

	CalculatePointDerivative (0, params, dir0, pointDataP, &m_jointForce[0]); 
	CalculatePointDerivative (1, params, dir1, pointDataP, &m_jointForce[1]); 
	CalculatePointDerivative (2, params, dir2, pointDataP, &m_jointForce[2]); 
	CalculatePointDerivative (3, params, dir0, pointDataQ, &m_jointForce[3]); 
	ret = 4;


//	dgVector sinAngle0 (matrix1.m_up * matrix0.m_up);
//	m_angle0 = dgAsin (ClampValue (sinAngle0 % dir0, -0.9999999f, 0.9999999f));
//	if ((matrix0.m_up % matrix1.m_up) < dgFloat32 (0.0f)) {
//		m_angle0 = (m_angle0 >= dgFloat32 (0.0f)) ? dgPI - m_angle0 : dgPI + m_angle0;
//	}

	sinAngle = (matrix1.m_up * matrix0.m_up) % matrix0.m_front;
	cosAngle = matrix0.m_up % matrix1.m_up;
//	dgAssert (dgAbsf (m_angle0 - dgAtan2 (sinAngle, cosAngle)) < 1.0e-1f);
	m_angle0 = dgAtan2 (sinAngle, cosAngle);

//	dgVector sinAngle1 (matrix0.m_front * matrix1.m_front);
//	m_angle1 = dgAsin (ClampValue (sinAngle1 % dir1, -0.9999999f, 0.9999999f));
//	if ((matrix0.m_front % matrix1.m_front) < dgFloat32 (0.0f)) {
//		m_angle1 = (m_angle1 >= dgFloat32 (0.0f)) ? dgPI - m_angle1 : dgPI + m_angle1;
//	}

	sinAngle = (matrix0.m_front * matrix1.m_front) % matrix1.m_up;
	cosAngle = matrix0.m_front % matrix1.m_front;
//	dgAssert (dgAbsf (m_angle1 - dgAtan2 (sinAngle, cosAngle)) < 1.0e-1f);
	m_angle1 = dgAtan2 (sinAngle, cosAngle);

	if (m_jointAccelFnt) {
		dgUnsigned32 code;
		dgJointCallbackParam axisParam[2];

		// linear acceleration
		axisParam[0].m_accel = dgFloat32 (0.0f);
		axisParam[0].m_timestep = params.m_timestep;
		axisParam[0].m_minFriction = DG_MIN_BOUND;
		axisParam[0].m_maxFriction = DG_MAX_BOUND;

		// angular acceleration
		axisParam[1].m_accel = dgFloat32 (0.0f);
		axisParam[1].m_timestep = params.m_timestep;
		axisParam[1].m_minFriction = DG_MIN_BOUND;
		axisParam[1].m_maxFriction = DG_MAX_BOUND;

		code = m_jointAccelFnt (*this, axisParam);
		if (code & 1) {
			if ((axisParam[0].m_minFriction > DG_MIN_BOUND) || (axisParam[0].m_maxFriction < DG_MAX_BOUND)) {
				params.m_forceBounds[ret].m_low = axisParam[0].m_minFriction;
				params.m_forceBounds[ret].m_upper = axisParam[0].m_maxFriction;
				params.m_forceBounds[ret].m_normalIndex = DG_BILATERAL_FRICTION_CONSTRAINT;
			}

//			CalculatePointDerivative (ret, params, dir0, pointDataP, &m_jointForce[ret]); 
			CalculateAngularDerivative (ret, params, dir0, m_stiffness, dgFloat32 (0.0f), &m_jointForce[ret]);
			//params.m_jointAccel[ret] = axisParam[0].m_accel;
			SetMotorAcceleration (ret, axisParam[0].m_accel, params);
			ret ++;
		}

		if (code & 2) {
			if ((axisParam[1].m_minFriction > DG_MIN_BOUND) || (axisParam[1].m_maxFriction < DG_MAX_BOUND)) {
				params.m_forceBounds[ret].m_low = axisParam[1].m_minFriction;
				params.m_forceBounds[ret].m_upper = axisParam[1].m_maxFriction;
				params.m_forceBounds[ret].m_normalIndex = DG_BILATERAL_FRICTION_CONSTRAINT;
			}
			CalculateAngularDerivative (ret, params, dir1, m_stiffness, dgFloat32 (0.0f), &m_jointForce[ret]);
			//params.m_jointAccel[ret] = axisParam[1].m_accel;
			SetMotorAcceleration (ret, axisParam[1].m_accel, params);
			ret ++;

		}
	}
	return dgUnsigned32 (ret);
}
Пример #23
0
dgInt32 dgPolygonSoupDatabaseBuilder::FilterFace(dgInt32 count,
    dgInt32* const pool)
{
  if (count == 3)
  {
    dgBigVector p0(m_vertexPoints[pool[2]]);
    for (dgInt32 i = 0; i < 3; i++)
    {
      dgBigVector p1(m_vertexPoints[pool[i]]);
      dgBigVector edge(p1 - p0);
      dgFloat64 mag2 = edge % edge;
      if (mag2 < dgFloat32(1.0e-6f))
      {
        count = 0;
      }
      p0 = p1;
    }

    if (count == 3)
    {
      dgBigVector edge0(m_vertexPoints[pool[2]] - m_vertexPoints[pool[0]]);
      dgBigVector edge1(m_vertexPoints[pool[1]] - m_vertexPoints[pool[0]]);
      dgBigVector normal(edge0 * edge1);
      dgFloat64 mag2 = normal % normal;
      if (mag2 < dgFloat32(1.0e-8f))
      {
        count = 0;
      }
    }
  }
  else
  {
    dgPolySoupFilterAllocator polyhedra(m_allocator);

    count = polyhedra.AddFilterFace(dgUnsigned32(count), pool);

    if (!count)
    {
      return 0;
    }

    dgEdge* edge = &polyhedra.GetRoot()->GetInfo();
    if (edge->m_incidentFace < 0)
    {
      edge = edge->m_twin;
    }

    bool flag = true;
    while (flag)
    {
      flag = false;
      if (count >= 3)
      {
        dgEdge* ptr = edge;

        dgBigVector p0(&m_vertexPoints[ptr->m_incidentVertex].m_x);
        do
        {
          dgBigVector p1(&m_vertexPoints[ptr->m_next->m_incidentVertex].m_x);
          dgBigVector e0(p1 - p0);
          dgFloat64 mag2 = e0 % e0;
          if (mag2 < dgFloat32(1.0e-6f))
          {
            count--;
            flag = true;
            edge = ptr->m_next;
            ptr->m_prev->m_next = ptr->m_next;
            ptr->m_next->m_prev = ptr->m_prev;
            ptr->m_twin->m_next->m_prev = ptr->m_twin->m_prev;
            ptr->m_twin->m_prev->m_next = ptr->m_twin->m_next;
            break;
          }
          p0 = p1;
          ptr = ptr->m_next;
        } while (ptr != edge);
      }
    }
    if (count >= 3)
    {
      flag = true;
      dgBigVector normal(
          polyhedra.FaceNormal(edge, &m_vertexPoints[0].m_x,
              sizeof(dgBigVector)));

      _ASSERTE((normal % normal) > dgFloat32 (1.0e-10f));
      normal = normal.Scale(dgRsqrt (normal % normal + dgFloat32 (1.0e-20f)));

      while (flag)
      {
        flag = false;
        if (count >= 3)
        {
          dgEdge* ptr = edge;

          dgBigVector p0(&m_vertexPoints[ptr->m_prev->m_incidentVertex].m_x);
          dgBigVector p1(&m_vertexPoints[ptr->m_incidentVertex].m_x);
          dgBigVector e0(p1 - p0);
          e0 = e0.Scale(dgRsqrt (e0 % e0 + dgFloat32(1.0e-10f)));
          do
          {
            dgBigVector p2(&m_vertexPoints[ptr->m_next->m_incidentVertex].m_x);
            dgBigVector e1(p2 - p1);

            e1 = e1.Scale(dgRsqrt (e1 % e1 + dgFloat32(1.0e-10f)));
            dgFloat64 mag2 = e1 % e0;
            if (mag2 > dgFloat32(0.9999f))
            {
              count--;
              flag = true;
              edge = ptr->m_next;
              ptr->m_prev->m_next = ptr->m_next;
              ptr->m_next->m_prev = ptr->m_prev;
              ptr->m_twin->m_next->m_prev = ptr->m_twin->m_prev;
              ptr->m_twin->m_prev->m_next = ptr->m_twin->m_next;
              break;
            }

            dgBigVector n(e0 * e1);
            mag2 = n % normal;
            if (mag2 < dgFloat32(1.0e-5f))
            {
              count--;
              flag = true;
              edge = ptr->m_next;
              ptr->m_prev->m_next = ptr->m_next;
              ptr->m_next->m_prev = ptr->m_prev;
              ptr->m_twin->m_next->m_prev = ptr->m_twin->m_prev;
              ptr->m_twin->m_prev->m_next = ptr->m_twin->m_next;
              break;
            }

            e0 = e1;
            p1 = p2;
            ptr = ptr->m_next;
          } while (ptr != edge);
        }
      }
    }

    dgEdge* first = edge;
    if (count >= 3)
    {
      dgFloat64 best = dgFloat32(2.0f);
      dgEdge* ptr = edge;

      dgBigVector p0(&m_vertexPoints[ptr->m_incidentVertex].m_x);
      dgBigVector p1(&m_vertexPoints[ptr->m_next->m_incidentVertex].m_x);
      dgBigVector e0(p1 - p0);
      e0 = e0.Scale(dgRsqrt (e0 % e0 + dgFloat32(1.0e-10f)));
      do
      {
        dgBigVector p2(
            &m_vertexPoints[ptr->m_next->m_next->m_incidentVertex].m_x);
        dgBigVector e1(p2 - p1);

        e1 = e1.Scale(dgRsqrt (e1 % e1 + dgFloat32(1.0e-10f)));
        dgFloat64 mag2 = fabs(e1 % e0);
        if (mag2 < best)
        {
          best = mag2;
          first = ptr;
        }

        e0 = e1;
        p1 = p2;
        ptr = ptr->m_next;
      } while (ptr != edge);

      count = 0;
      ptr = first;
      do
      {
        pool[count] = ptr->m_incidentVertex;
        count++;
        ptr = ptr->m_next;
      } while (ptr != first);
    }

#ifdef _DEBUG
    if (count >= 3)
    {
      dgInt32 j0 = count - 2;
      dgInt32 j1 = count - 1;
      dgBigVector normal(
          polyhedra.FaceNormal(edge, &m_vertexPoints[0].m_x,
              sizeof(dgBigVector)));
      for (dgInt32 j2 = 0; j2 < count; j2++)
      {
        dgBigVector p0(&m_vertexPoints[pool[j0]].m_x);
        dgBigVector p1(&m_vertexPoints[pool[j1]].m_x);
        dgBigVector p2(&m_vertexPoints[pool[j2]].m_x);
        dgBigVector e0((p0 - p1));
        dgBigVector e1((p2 - p1));

        dgBigVector n(e1 * e0);
        _ASSERTE((n % normal) > dgFloat32 (0.0f));
        j0 = j1;
        j1 = j2;
      }
    }
#endif
  }

  return (count >= 3) ? count : 0;
}
Пример #24
0
void dgWorld::SetFrictionMode (dgInt32 mode)
{
	m_frictionMode = dgUnsigned32 (mode);
}
Пример #25
0
void dgWorld::SetSolverMode (dgInt32 mode)
{
	m_solverMode = dgUnsigned32 (GetMax (0, mode));
}
Пример #26
0
dgUnsigned32 dgCollision::Quantize(dgFloat32 value)
{
	return dgUnsigned32 (value * 1024.0f);
}