Exemple #1
0
/** @brief updates the stored jacobian for the current values in the pivots*/
void InvKinematic::ConstructJacobian() {
  // ds_i / dtheta_j = v_j x (s_i - p_j)
  //
  //         (u2*v3 - u3*v2)
  // u x v = (u3*v1 - u1*v3)
  //         (u1*v2 - u2*v1)
  const int end = pivot_count_ - 1;  // don't include the end effector
  double unity_z[3] = {0.0, 0.0, 1.0};
  double v_j[3];
  HMatrix H_0_s = HMatrix(
    pivots_[pivot_count_ - 1]->GetRelativeHMatrixArray());
  HMatrix H_0_pj;
  //  printf(" transposed jacobian:\n");
  for (int j = 0; j < end; ++j) {
    H_0_pj = HMatrix(pivots_[j]->GetRelativeHMatrixArray());
    H_0_pj.RotateVector(unity_z, v_j);
    const double v_diff[3] = {
      H_0_s.GetX() - H_0_pj.GetX(),
      H_0_s.GetY() - H_0_pj.GetY(),
      H_0_s.GetZ() - H_0_pj.GetZ()};
    CrossProduct3(v_j, v_diff, &jacobianT_[j*3]);
    //  printf(" %.4f, %.4f, %.4f\n", jacobianT_[j*3], jacobianT_[j*3+1],
      //  jacobianT_[j*3+2]);
  }
}
Exemple #2
0
void Face3::MakeNormal()
{
	// prepare the components of two vectors from three vertices of face
	double x1 = this->V(0)->x - this->V(1)->x;
	double y1 = this->V(0)->y - this->V(1)->y;
	double z1 = this->V(0)->z - this->V(1)->z;

	double x2 = this->V(0)->x - this->V(2)->x;
	double y2 = this->V(0)->y - this->V(2)->y;
	double z2 = this->V(0)->z - this->V(2)->z;

	// vector product of two vectors is the normal to the face
	Vector3 v1(x1, y1, z1);
	Vector3 v2(x2, y2, z2);
	CrossProduct3(v1, v2, normal);
}
	void GeneratePlane(SoftPhongPrimitiveList& prims) {
		const float size = 500;
		const float z_value = -20;
		const float uv_value = 20;

		static SoftPhongVertex mesh[8] = {
			{ { -size, -size, z_value, 1 } },
			{ { size, -size, z_value, 1 } },
			{ { size, size, z_value, 1 } },
			{ { -size, size, z_value, 1 } }
		};

		auto draw_plane = [&](int a, int b, int c, int d) {
			SoftPhongVertex p1 = mesh[a], p2 = mesh[b], p3 = mesh[c], p4 = mesh[d];

			p1.attribs.uv.set(0, 0);
			p2.attribs.uv.set(0, uv_value);
			p3.attribs.uv.set(uv_value, uv_value);
			p4.attribs.uv.set(uv_value, 0);

			Vector3f norm = CrossProduct3((p3.pos - p2.pos).xyz(), (p1.pos - p2.pos).xyz());
			p1.attribs.normal = norm;
			p2.attribs.normal = norm;
			p3.attribs.normal = norm;
			p4.attribs.normal = norm;

			size_t index = prims.verts_.size();
			prims.verts_.push_back(p1);
			prims.verts_.push_back(p2);
			prims.verts_.push_back(p3);
			prims.indexs_.push_back(index);
			prims.indexs_.push_back(index + 1);
			prims.indexs_.push_back(index + 2);

			index = prims.verts_.size();
			prims.verts_.push_back(p3);
			prims.verts_.push_back(p4);
			prims.verts_.push_back(p1);
			prims.indexs_.push_back(index);
			prims.indexs_.push_back(index + 1);
			prims.indexs_.push_back(index + 2);
		};

		draw_plane(0, 1, 2, 3);
	}