Ejemplo n.º 1
0
void IKTree::solve(int frame)
{
  double x, y, z;
  reset(frame);

  IKEffectorList effList;
  
  for (int i=0; i<20; i++) {
    effList.num = 0;
    solveJoint(frame, 0, effList);
  }

  for (int i=0; i<numBones-1; i++) {
    BVHNode *n = bone[i].node;
    toEuler(bone[i].lRot, n->channelOrder, x, y, z);
    for (int j=0; j<3; j++) {  // rotate each axis in order
      switch (n->channelType[j]) {
      case BVH_XROT: n->ikRot[j] = x - n->frame[frame][j]; break;
      case BVH_YROT: n->ikRot[j] = y - n->frame[frame][j]; break;
      case BVH_ZROT: n->ikRot[j] = z - n->frame[frame][j]; break;
      }
    }
  }

  display = 1;
  updateBones(0);
  display = 0;
}
Ejemplo n.º 2
0
void PropertyGrid::showCoreProperties(Lumix::Entity entity)
{
	char name[256];
	const char* tmp = m_editor.getUniverse()->getEntityName(entity);
	
	ImGui::LabelText("ID", "%d", entity.index);
	
	Lumix::copyString(name, tmp);
	if (ImGui::InputText("Name", name, sizeof(name))) m_editor.setEntityName(entity, name);

	auto pos = m_editor.getUniverse()->getPosition(entity);
	if (ImGui::DragFloat3("Position", &pos.x)) m_editor.setEntitiesPositions(&entity, &pos, 1);

	auto rot = m_editor.getUniverse()->getRotation(entity);
	auto euler = rot.toEuler();
	euler.x = Lumix::Math::radiansToDegrees(fmodf(euler.x, Lumix::Math::PI));
	euler.y = Lumix::Math::radiansToDegrees(fmodf(euler.y, Lumix::Math::PI));
	euler.z = Lumix::Math::radiansToDegrees(fmodf(euler.z, Lumix::Math::PI));
	if (ImGui::DragFloat3("Rotation", &euler.x))
	{
		euler.x = Lumix::Math::degreesToRadians(fmodf(euler.x, 180));
		euler.y = Lumix::Math::degreesToRadians(fmodf(euler.y, 180));
		euler.z = Lumix::Math::degreesToRadians(fmodf(euler.z, 180));
		rot.fromEuler(euler);
		m_editor.setEntitiesRotations(&entity, &rot, 1);
	}

	float scale = m_editor.getUniverse()->getScale(entity);
	if (ImGui::DragFloat("Scale", &scale, 0.1f))
	{
		m_editor.setEntitiesScales(&entity, &scale, 1);
	}
}
Ejemplo n.º 3
0
 Vector6d toVectorET(const Isometry3d& t) {
   Vector6d v;
   v.block<3,1>(3,0)=toEuler(extractRotation(t));
   v.block<3,1>(0,0) = t.translation();
   return v;
 }
Ejemplo n.º 4
0
void IKTree::solveJoint(int frame, int i, IKEffectorList &effList)
{
  double x, y, z;
  double ang = 0;

  MT_Quaternion q;
  MT_Quaternion totalPosRot = MT_Quaternion(0,0,0,0);
  MT_Quaternion totalDirRot = MT_Quaternion(0,0,0,0);
  MT_Vector3 axis(0,0,0);
  BVHNode *n;
  int numPosRot = 0, numDirRot = 0;

  if (bone[i].numChildren == 0) {     // reached end site
    if (bone[i].node->ikOn) {
      effList.index[effList.num++] = i;
    }
    return;
  }

  for (int j=0; j<bone[i].numChildren; j++) {
    IKEffectorList el;
    el.num = 0;
    solveJoint(frame, bone[i].child[j], el);
    for (int k=0; k<el.num; k++) {
      effList.index[effList.num++] = el.index[k];
    }
  }

  updateBones(i);

  for (int j=0; j<effList.num; j++) {
    int effIndex = effList.index[j];
    n = bone[effIndex].node;
    MT_Vector3 effGoalPos(n->ikGoalPos[0],
			  n->ikGoalPos[1],
			  n->ikGoalPos[2]);
    const MT_Vector3 pC =
      (bone[effIndex].pos - bone[i].pos).safe_normalized();
    const MT_Vector3 pD =
      (effGoalPos - bone[i].pos).safe_normalized();
    MT_Vector3 rotAxis = pC.cross(pD);
    if (rotAxis.length2() > MT_EPSILON) {
      totalPosRot += MT_Quaternion(rotAxis, bone[i].weight * acos(pC.dot(pD)));
      numPosRot++;
    }

    const MT_Vector3 uC =
      (bone[effIndex].pos - bone[effIndex-1].pos).safe_normalized();
    const MT_Vector3 uD =
      (MT_Vector3(n->ikGoalDir[0], n->ikGoalDir[1], n->ikGoalDir[2])).safe_normalized();
    rotAxis = uC.cross(uD);
    if (rotAxis.length2() > MT_EPSILON) {
      double weight = 0.0;
      if (i == effIndex-1) weight = 0.5;
      totalDirRot += MT_Quaternion(rotAxis, weight * acos(uC.dot(uD)));
      numDirRot++;
    }
  }

  if ((numPosRot + numDirRot) > MT_EPSILON) {
    n = bone[i].node;
    n->ikOn = true;
    // average the quaternions from all effectors
    if (numPosRot)
      totalPosRot /= numPosRot;
    else
      totalPosRot = identity;
    if (numDirRot)
      totalDirRot /= numDirRot;
    else
      totalDirRot = identity;
    MT_Quaternion targetRot = 0.9 * totalPosRot + 0.1 * totalDirRot;
    targetRot = targetRot * bone[i].lRot;
    toEuler(targetRot, n->channelOrder, x, y, z);
    if (jointLimits) {
      bone[i].lRot = identity;
      for (int k=0; k<n->numChannels; k++) {  // clamp each axis in order
        switch (n->channelType[k]) {
          case BVH_XROT: ang = x; axis = xAxis; break;
          case BVH_YROT: ang = y; axis = yAxis; break;
          case BVH_ZROT: ang = z; axis = zAxis; break;
          default: break;
        }
        // null axis leads to crash in q.setRotation(), so check first
        if(axis.length())
        {
          if (ang < n->channelMin[k]) ang = n->channelMin[k];
          else if (ang > n->channelMax[k]) ang = n->channelMax[k];
          q.setRotation(axis, ang * M_PI / 180);
          bone[i].lRot = q * bone[i].lRot;
        }
      }
    }
    else
      bone[i].lRot = targetRot;
  }
}