/** @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]);
}
}
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);
}
