csColor GetPixel (float coord_x, float coord_y)
{
// Scale the texture coordinates.
coord_x *= textureScale.x;
coord_y *= textureScale.y;
// Calculate the material coordinates.
float matcoord_x_f = (coord_x * img_w);
float matcoord_y_f = (coord_y * img_h);
int matcoord_x = int (matcoord_x_f);
int matcoord_y = int (matcoord_y_f);
// Bilinearly filter from material.
csColor p00 (GetPixelWrap (img, img_w, img_h,
matcoord_x, matcoord_y));
csColor p01 (GetPixelWrap (img, img_w, img_h,
matcoord_x, matcoord_y+1));
csColor p11 (GetPixelWrap (img, img_w, img_h,
matcoord_x+1, matcoord_y+1));
csColor p10 (GetPixelWrap (img, img_w, img_h,
matcoord_x+1, matcoord_y));
float f1 = matcoord_x_f - matcoord_x;
float f2 = matcoord_y_f - matcoord_y;
return csLerp (csLerp (p00, p10, f1),
csLerp (p01, p11, f1), f2);
}
/**
* @brief
* Calculate the intersection point of a line segment and the two triangles making a the cell of a height map terrain
*/
dFloat BodyTerrain::RayCastCell(int xIndex0, int zIndex0, const Vector3 &p0, const Vector3 &dp, Vector3 &normalOut)
{
dFloat t;
// Clamp x
if (xIndex0 < 0)
xIndex0 = 0;
if (xIndex0 >= static_cast<int>(m_nWidth)-1)
xIndex0 = m_nWidth-2;
// Clamp z
if (zIndex0 < 0)
zIndex0 = 0;
if (zIndex0 >= static_cast<int>(m_nHeight)-1)
zIndex0 = m_nHeight-2;
// Get the 3d point at the corner of the cell
Vector3 p00((xIndex0 + 0)*m_vScale.x, HEIGHFIELD(zIndex0+0, xIndex0+0), (zIndex0 + 0)*m_vScale.z);
Vector3 p10((xIndex0 + 1)*m_vScale.x, HEIGHFIELD(zIndex0+0, xIndex0+1), (zIndex0 + 0)*m_vScale.z);
Vector3 p11((xIndex0 + 1)*m_vScale.x, HEIGHFIELD(zIndex0+1, xIndex0+1), (zIndex0 + 1)*m_vScale.z);
// Clip line again first triangle
Vector3 e0 = p10 - p00;
Vector3 e1 = p11 - p00;
t = RayCastTriangle(p0, dp, p00, e0, e1);
if (t < 1.0f) {
return t;
}
// Clip line against second triangle
Vector3 p01((xIndex0 + 0)*m_vScale.x, HEIGHFIELD(zIndex0+1, xIndex0+0), (zIndex0 + 1)*m_vScale.z);
Vector3 e2 = p01 - p00;
return RayCastTriangle(p0, dp, p00, e1, e2);
}
void Polyhedron_triangulation::extract_CAT()
{
std::for_each(cat_cells.begin(), cat_cells.end(), std::mem_fun_ref(&std::list<CAT_facet>::clear));
int *tetra_vtx_ptr = tetra_mesh.tetrahedronlist;
int *marker = tetra_mesh.pointmarkerlist;
REAL *pnt_tbl = tetra_mesh.pointlist;
std::function<Point_3(int)> cgal_pnt = [=](int idx) { return Point_3(pnt_tbl[idx*3], pnt_tbl[idx*3+1], pnt_tbl[idx*3+2]); };
for (int i = 0; i < tetra_mesh.numberoftetrahedra; i++, tetra_vtx_ptr += 4)
{
std::set<int> group_no;
std::multimap<int, int> group_no_vidx;
typedef std::multimap<int, int>::const_iterator Iter;
for (int j = 0; j < 4; j++)
{
int vid = tetra_vtx_ptr[j];
int gid = marker[vid];
group_no.insert(gid);
group_no_vidx.insert(std::make_pair(gid, vid));
}
if (group_no.size() == 2)
{
std::set<int>::const_iterator gid0 = group_no.begin(), gid1 = group_no.begin();
++gid1;
if (group_no_vidx.count(*gid0) == 1 && group_no_vidx.count(*gid1) == 3)
{
std::pair<Iter, Iter> i0 = group_no_vidx.equal_range(*gid0);
std::pair<Iter, Iter> i1 = group_no_vidx.equal_range(*gid1);
std::list<Point_3> mid_tri;
for (Iter it = i1.first; it != i1.second; ++it)
mid_tri.push_back(CGAL::midpoint(cgal_pnt(i0.first->second), cgal_pnt(it->second)));
if (*gid0 >= 0)
cat_cells[*gid0].push_back(CAT_facet(cgal_pnt(i0.first->second), mid_tri));
if (*gid1 >= 0)
for (Iter it = i1.first; it != i1.second; ++it)
cat_cells[*gid1].push_back(CAT_facet(cgal_pnt(it->second), mid_tri));
}
else if (group_no_vidx.count(*gid0) == 3 && group_no_vidx.count(*gid1) == 1)
{
std::pair<Iter, Iter> i0 = group_no_vidx.equal_range(*gid0);
std::pair<Iter, Iter> i1 = group_no_vidx.equal_range(*gid1);
std::list<Point_3> mid_tri;
for (Iter it = i0.first; it != i0.second; ++it)
mid_tri.push_back(CGAL::midpoint(cgal_pnt(i1.first->second), cgal_pnt(it->second)));
if (*gid1 >= 0)
cat_cells[*gid1].push_back(CAT_facet(cgal_pnt(i1.first->second), mid_tri));
if (*gid0 >= 0)
for (Iter it = i0.first; it != i0.second; ++it)
cat_cells[*gid0].push_back(CAT_facet(cgal_pnt(it->second), mid_tri));
}
else
{
std::pair<Iter, Iter> i0 = group_no_vidx.equal_range(*gid0);
std::pair<Iter, Iter> i1 = group_no_vidx.equal_range(*gid1);
Iter it = i0.first;
Point_3 p00(cgal_pnt(it->second));
++it;
Point_3 p01(cgal_pnt(it->second));
it = i1.first;
Point_3 p10(cgal_pnt(it->second));
++it;
Point_3 p11(cgal_pnt(it->second));
Point_3 midpnt[4] = {CGAL::midpoint(p00, p10), CGAL::midpoint(p00, p11), CGAL::midpoint(p01, p10), CGAL::midpoint(p01, p11)};
std::list<Point_3> mid_pm; // the middle parallelogram
mid_pm.push_back(midpnt[0]);
mid_pm.push_back(midpnt[1]);
if (Vector_3(midpnt[0], midpnt[1])*Vector_3(midpnt[2], midpnt[3]) < 0)
{
mid_pm.push_back(midpnt[2]);
mid_pm.push_back(midpnt[3]);
}
else
{
mid_pm.push_back(midpnt[3]);
mid_pm.push_back(midpnt[2]);
}
if (*gid0 >= 0)
{
cat_cells[*gid0].push_back(CAT_facet(p00, mid_pm));
cat_cells[*gid0].push_back(CAT_facet(p01, mid_pm));
}
if (*gid1 >= 0)
{
cat_cells[*gid1].push_back(CAT_facet(p10, mid_pm));
cat_cells[*gid1].push_back(CAT_facet(p11, mid_pm));
}
}
}
else if (group_no.size() == 3)
{
// the two vertices in the same group
int smgp[2];
// the two vertices in the other two different groups
int dfgp[2];
int gi, gj, gk;
std::vector< std::pair<int, int> > tmpv(group_no_vidx.begin(), group_no_vidx.end());
if (tmpv[0].first == tmpv[1].first)
{
smgp[0] = tmpv[0].second; smgp[1] = tmpv[1].second; gi = tmpv[0].first;
dfgp[0] = tmpv[2].second; dfgp[1] = tmpv[3].second; gj = tmpv[2].first; gk = tmpv[3].first;
}
else if (tmpv[1].first == tmpv[2].first)
{
smgp[0] = tmpv[1].second; smgp[1] = tmpv[2].second; gi = tmpv[1].first;
dfgp[0] = tmpv[0].second; dfgp[1] = tmpv[3].second; gj = tmpv[0].first; gk = tmpv[3].first;
}
else
{
smgp[0] = tmpv[2].second; smgp[1] = tmpv[3].second; gi = tmpv[2].first;
dfgp[0] = tmpv[0].second; dfgp[1] = tmpv[1].second; gj = tmpv[0].first; gk = tmpv[1].first;
}
Point_3 pi[2] = {cgal_pnt(smgp[0]), cgal_pnt(smgp[1])};
Point_3 pj(cgal_pnt(dfgp[0])), pk(cgal_pnt(dfgp[1]));
Point_3 tri_cent[2] = {CGAL::centroid(pi[0], pj, pk), CGAL::centroid(pi[1], pj, pk)};
Point_3 edge_mid[5] = {CGAL::midpoint(pi[0], pj), CGAL::midpoint(pi[0], pk),
CGAL::midpoint(pi[1], pj), CGAL::midpoint(pi[1], pk),
CGAL::midpoint(pj, pk)};
//std::list<Point_3> quad_i0i1j, quad_i0i1k, tri_i0i1jk;
std::array<Point_3, 4> quad_i0i1j = {edge_mid[0], edge_mid[2], tri_cent[1], tri_cent[0]};
std::array<Point_3, 4> quad_i0i1k = {edge_mid[1], edge_mid[3], tri_cent[1], tri_cent[0]};
std::array<Point_3, 3> tri_i0i1jk = {edge_mid[4], tri_cent[1], tri_cent[0]};
if (gi >= 0)
{
cat_cells[gi].push_back(CAT_facet(pi[0], quad_i0i1j.begin(), quad_i0i1j.end()));
cat_cells[gi].push_back(CAT_facet(pi[0], quad_i0i1k.begin(), quad_i0i1k.end()));
cat_cells[gi].push_back(CAT_facet(pi[1], quad_i0i1j.begin(), quad_i0i1j.end()));
cat_cells[gi].push_back(CAT_facet(pi[1], quad_i0i1k.begin(), quad_i0i1k.end()));
}
if (gj >= 0)
{
cat_cells[gj].push_back(CAT_facet(pj, quad_i0i1j.begin(), quad_i0i1j.end()));
cat_cells[gj].push_back(CAT_facet(pj, tri_i0i1jk.begin(), tri_i0i1jk.end()));
}
if (gk >= 0)
{
cat_cells[gk].push_back(CAT_facet(pk, quad_i0i1k.begin(), quad_i0i1k.end()));
cat_cells[gk].push_back(CAT_facet(pk, tri_i0i1jk.begin(), tri_i0i1jk.end()));
}
}
else if (group_no.size() == 4)
{
std::array<Point_3, 4> vs;
std::array<int, 4> groupid;
for (int j = 0; j < 4; j++)
{
vs[j] = cgal_pnt(tetra_vtx_ptr[j]);
groupid[j] = marker[tetra_vtx_ptr[j]];
}
Point_3 tetra_cent = CGAL::centroid(vs.begin(), vs.end(), CGAL::Dimension_tag<0>());
for (int j = 0; j < 4; j++)
{
if (groupid[j] < 0)
continue;
for (int k = 0; k < 4; k++)
{
if (j == k)
continue;
for (int l = 0; l < 4; l++)
{
if (l == j || l == k)
continue;
Point_3 mpnt = CGAL::midpoint(vs[j], vs[k]);
int m;
for (m = 0; m < 4; m++)
if (m != j && m != k && m != l)
break;
Point_3 tri_cent[2] = { CGAL::centroid(vs[j], vs[k], vs[l]), CGAL::centroid(vs[j], vs[k], vs[m]) };
std::list<Point_3> tri;
tri.push_back(mpnt);
tri.push_back(tri_cent[0]);
tri.push_back(tri_cent[1]);
cat_cells[groupid[j]].push_back(CAT_facet(vs[j], tri));
tri.pop_front();
tri.push_back(tetra_cent);
cat_cells[groupid[j]].push_back(CAT_facet(vs[j], tri));
}
}
}
}
}
}
void LLSurfacePatch::calcNormal(const U32 x, const U32 y, const U32 stride)
{
U32 patch_width = mSurfacep->mPVArray.mPatchWidth;
U32 surface_stride = mSurfacep->getGridsPerEdge();
const F32 mpg = mSurfacep->getMetersPerGrid() * stride;
S32 poffsets[2][2][2];
poffsets[0][0][0] = x - stride;
poffsets[0][0][1] = y - stride;
poffsets[0][1][0] = x - stride;
poffsets[0][1][1] = y + stride;
poffsets[1][0][0] = x + stride;
poffsets[1][0][1] = y - stride;
poffsets[1][1][0] = x + stride;
poffsets[1][1][1] = y + stride;
const LLSurfacePatch *ppatches[2][2];
// LLVector3 p1, p2, p3, p4;
ppatches[0][0] = this;
ppatches[0][1] = this;
ppatches[1][0] = this;
ppatches[1][1] = this;
U32 i, j;
for (i = 0; i < 2; i++)
{
for (j = 0; j < 2; j++)
{
if (poffsets[i][j][0] < 0)
{
if (!ppatches[i][j]->getNeighborPatch(WEST))
{
poffsets[i][j][0] = 0;
}
else
{
poffsets[i][j][0] += patch_width;
ppatches[i][j] = ppatches[i][j]->getNeighborPatch(WEST);
}
}
if (poffsets[i][j][1] < 0)
{
if (!ppatches[i][j]->getNeighborPatch(SOUTH))
{
poffsets[i][j][1] = 0;
}
else
{
poffsets[i][j][1] += patch_width;
ppatches[i][j] = ppatches[i][j]->getNeighborPatch(SOUTH);
}
}
if (poffsets[i][j][0] >= (S32)patch_width)
{
if (!ppatches[i][j]->getNeighborPatch(EAST))
{
poffsets[i][j][0] = patch_width - 1;
}
else
{
poffsets[i][j][0] -= patch_width;
ppatches[i][j] = ppatches[i][j]->getNeighborPatch(EAST);
}
}
if (poffsets[i][j][1] >= (S32)patch_width)
{
if (!ppatches[i][j]->getNeighborPatch(NORTH))
{
poffsets[i][j][1] = patch_width - 1;
}
else
{
poffsets[i][j][1] -= patch_width;
ppatches[i][j] = ppatches[i][j]->getNeighborPatch(NORTH);
}
}
}
}
LLVector3 p00(-mpg,-mpg,
*(ppatches[0][0]->mDataZ
+ poffsets[0][0][0]
+ poffsets[0][0][1]*surface_stride));
LLVector3 p01(-mpg,+mpg,
*(ppatches[0][1]->mDataZ
+ poffsets[0][1][0]
+ poffsets[0][1][1]*surface_stride));
LLVector3 p10(+mpg,-mpg,
*(ppatches[1][0]->mDataZ
+ poffsets[1][0][0]
+ poffsets[1][0][1]*surface_stride));
LLVector3 p11(+mpg,+mpg,
*(ppatches[1][1]->mDataZ
+ poffsets[1][1][0]
+ poffsets[1][1][1]*surface_stride));
LLVector3 c1 = p11 - p00;
LLVector3 c2 = p01 - p10;
LLVector3 normal = c1;
normal %= c2;
normal.normVec();
*(mDataNorm + surface_stride * y + x) = normal;
}
BezTreeNode::BezTreeNode(int _lv, float us, float ue, float vs, float ve,
BezTree* _tree, BezTreeNode* p, int* _idx)
: lv(_lv), u_s(us), u_e(ue), v_s(vs), v_e(ve)
{
tree = _tree;
parent = p;
child[0] = child[1] = child[2] = child[3] = NULL;
memcpy(idx, _idx, sizeof(int) * 9);
//const double Height = static_cast<double>(GeoCorrection::gridY) - 1;
// compute bezier control points
Vector2f p00(tree->transformedPoints[idx[0]].at<double>(0, 0), tree->transformedPoints[idx[0]].at<double>(1, 0)),
p02(tree->transformedPoints[idx[2]].at<double>(0, 0), tree->transformedPoints[idx[2]].at<double>(1, 0)),
p01 = estimateControlPoint(p00, p02, cv::Point2f(tree->transformedPoints[idx[1]].at<double>(0, 0), tree->transformedPoints[idx[1]].at<double>(1, 0)), 0.5),
p20(tree->transformedPoints[idx[6]].at<double>(0, 0), tree->transformedPoints[idx[6]].at<double>(1, 0)),
p22(tree->transformedPoints[idx[8]].at<double>(0, 0), tree->transformedPoints[idx[8]].at<double>(1, 0)),
p21 = estimateControlPoint(p20, p22, cv::Point2f(tree->transformedPoints[idx[7]].at<double>(0, 0), tree->transformedPoints[idx[7]].at<double>(1, 0)), 0.5),
p10 = estimateControlPoint(p00, p20, cv::Point2f(tree->transformedPoints[idx[3]].at<double>(0, 0), tree->transformedPoints[idx[3]].at<double>(1, 0)), 0.5),
p12 = estimateControlPoint(p02, p22, cv::Point2f(tree->transformedPoints[idx[5]].at<double>(0, 0), tree->transformedPoints[idx[5]].at<double>(1, 0)), 0.5),
p11 = estimateControlPoint(p10, p12, cv::Point2f(tree->transformedPoints[idx[4]].at<double>(0, 0), tree->transformedPoints[idx[4]].at<double>(1, 0)), 0.5);
Vector2f glProjPoint0(tree->projPoints[idx[0]].x, tree->projPoints[idx[0]].y),
glProjPoint1(tree->projPoints[idx[1]].x, tree->projPoints[idx[1]].y),
glProjPoint2(tree->projPoints[idx[2]].x, tree->projPoints[idx[2]].y),
glProjPoint3(tree->projPoints[idx[3]].x, tree->projPoints[idx[3]].y),
glProjPoint4(tree->projPoints[idx[4]].x, tree->projPoints[idx[4]].y),
glProjPoint5(tree->projPoints[idx[5]].x, tree->projPoints[idx[5]].y),
glProjPoint6(tree->projPoints[idx[6]].x, tree->projPoints[idx[6]].y),
glProjPoint7(tree->projPoints[idx[7]].x, tree->projPoints[idx[7]].y),
glProjPoint8(tree->projPoints[idx[8]].x, tree->projPoints[idx[8]].y);
if (p == nullptr)
{
// actual bezier control points, need to differntiate delta
/*surface = new QuadBezierPatch2f(2.0f*glProjPoint0 - p00,
2.0f*glProjPoint1 - p01,
2.0f*glProjPoint2 - p02,
2.0f*glProjPoint3 - p10,
2.0f*glProjPoint4 - p11,
2.0f*glProjPoint5 - p12,
2.0f*glProjPoint6 - p20,
2.0f*glProjPoint7 - p21,
2.0f*glProjPoint8 - p22
);*/
surface = new QuadBezierPatch2f(glProjPoint0 ,
2.0f*glProjPoint1 - p01,
glProjPoint2,
2.0f*glProjPoint3 - p10,
2.0f*glProjPoint4 - p11,
2.0f*glProjPoint5 - p12,
glProjPoint6,
2.0f*glProjPoint7 - p21,
glProjPoint8);
}
else
{
std::cout << "subdivision above 0th level" << std::endl;
size_t row = sqrt(p->bezPatch.size());
surface = new QuadBezierPatch2f(p->bezPatch[idx[0]],
2.0*glProjPoint1 - p01,
p->bezPatch[idx[2]],
2.0*glProjPoint3 - p10,
2.0*glProjPoint4 - p11,
2.0*glProjPoint5 - p12,
p->bezPatch[idx[6]],
2.0*glProjPoint7 - p21,
p->bezPatch[idx[8]]
);
}
// generate vertices and mesh indices
surface->interpolate(bezPatch, bezPatchIdx, BernsVal, 0, BernsVal.size());
}
void onInitialization( ) {
glViewport(0, 0, screenWidth, screenHeight);
srand(42);
Mat gold;
gold.ka=Color(0.24725, 0.1995, 0.0745);
gold.kd=Color(0.75164, 0.60648, 0.22648);
gold.ks=Color(0.628281, 0.555802, 0.366065);
gold.shine=0.4 * 128;
gold.n=Color(0.17, 0.35, 1.5);
gold.k=Color(3.1, 2.7, 1.9);
gold.reflective=true;
gold.refractive=false;
Mat brown;
brown.ka=Color(0.1,0.1,0.1);
brown.kd=Color(0.58, 0.294, 0);
brown.ks=Color(0,0,0);
brown.shine=0;
brown.reflective=false;
brown.refractive=false;
double gn=1.5;
Mat glass;
glass.ka=Color(0.1,0.1,0.1);
glass.kd=Color(0.2,0.2,0.2);
glass.ks=Color(1,1,1);
glass.shine=120;
glass.reflective=true;
glass.refractive=true;
glass.n=Color(gn,gn,gn);
glass.k=Color(0,0,0);
Mat glass2=glass;
glass2.n=Color(1/gn, 1/gn, 1/gn);
add(new Sphere(glass2, Vector(0,0,0), 0.4));
add(new Torus(gold, 0.25, 0.1));
double z=-0.65;
Vector p00(-2,-2,z);
Vector p10(2,-2,z);
Vector p01(-2,2,z);
Vector p11(2,2,z);
Vector n(0,0,1);
add(new Triangle(brown, p00, p10, p01, n,n,n));
add(new Triangle(brown, p11, p01, p10, n,n,n));
int cube[12][3][3]={
{
{-1, -1, -1},
{1, -1, -1},
{1, -1, 1}
},
{
{-1, -1, 1},
{-1, -1, -1},
{1, -1, 1}
},
{
{-1, 1, -1},
{1, 1, 1},
{1, 1, -1}
},
{
{-1, 1, 1},
{1, 1, 1},
{-1, 1, -1}
},
{
{-1, -1, -1},
{-1, 1, 1},
{-1, 1, -1}
},
{
{-1, 1, 1},
{-1, -1, -1},
{-1, -1, 1}
},
{
{1, -1, -1},
{1, 1, -1},
{1, 1, 1}
},
{
{1, 1, 1},
{1, -1, 1},
{1, -1, -1}
},
{
{-1, 1, -1},
{1, 1, -1},
{-1, -1, -1}
},
{
{1, -1, -1},
{-1, -1, -1},
{1, 1, -1}
},
{
{-1, 1, 1},
{-1, -1, 1},
{1, 1, 1},
},
{
{1, -1, 1},
{1, 1, 1},
{-1, -1, 1}
}};
double a=0.5,b=0.5,c=0.5;
for(int i=0;i<12;i++){
Vector p1(cube[i][0][0]*a, cube[i][0][1]*b, cube[i][0][2]*c);
Vector p2(cube[i][1][0]*a, cube[i][1][1]*b, cube[i][1][2]*c);
Vector p3(cube[i][2][0]*a, cube[i][2][1]*b, cube[i][2][2]*c);
Vector n=((p2-p1)%(p3-p1)).unit();
add(new Triangle(glass, p1, p2, p3, n, n, n));
}
gen(phLim);
render();
tone();
for(int i=0;i<objSize;i++){
delete obj[i];
}
}
void MSNewton::CurvySlider::submit_constraints(const NewtonJoint* joint, dgFloat32 timestep, int thread_index) {
JointData* joint_data = (JointData*)NewtonJointGetUserData(joint);
CurvySliderData* cj_data = (CurvySliderData*)joint_data->cj_data;
// Calculate position of pivot points and Jacobian direction vectors in global space.
dMatrix matrix0, matrix1, matrix2;
MSNewton::Joint::c_calculate_global_matrix(joint_data, matrix0, matrix1, matrix2);
dVector location = matrix2.UntransformVector(matrix0.m_posit);
dVector point, vector, min_pt, max_pt;
dFloat distance, min_len, max_len;
if (!c_calc_curve_data_at_location(cj_data, location, point, vector, distance, min_pt, max_pt, min_len, max_len)) {
cj_data->cur_data_set = false;
return;
}
point = matrix2.TransformVector(point);
vector = matrix2.RotateVector(vector);
min_pt = matrix2.TransformVector(min_pt);
max_pt = matrix2.TransformVector(max_pt);
cj_data->cur_point = point;
cj_data->cur_vector = vector;
cj_data->cur_tangent = (1.0f - dAbs(vector.m_z) < EPSILON) ? Y_AXIS * vector : Z_AXIS * vector;
cj_data->cur_data_set = true;
dFloat last_pos = cj_data->cur_pos;
dFloat last_vel = cj_data->cur_vel;
if (cj_data->loop) {
dFloat diff1 = distance - cj_data->last_dist;
dFloat diff2 = diff1 + (diff1 > 0 ? -cj_data->curve_len : cj_data->curve_len);
if (dAbs(diff1) < dAbs(diff2))
cj_data->cur_pos += diff1;
else
cj_data->cur_pos += diff2;
}
else
cj_data->cur_pos = distance;
cj_data->cur_vel = (cj_data->cur_pos - last_pos) / timestep;
cj_data->cur_accel = (cj_data->cur_vel - last_vel) / timestep;
cj_data->last_dist = distance;
dMatrix matrix3;
Util::matrix_from_pin_dir(point, vector, matrix3);
const dVector& p0 = matrix0.m_posit;
const dVector& p1 = matrix3.m_posit;
dVector p00(p0 + matrix0.m_right.Scale(MIN_JOINT_PIN_LENGTH));
dVector p11(p1 + matrix3.m_right.Scale(MIN_JOINT_PIN_LENGTH));
// Restrict movement on the pivot point along the normal and bi normal of the path.
NewtonUserJointAddLinearRow(joint, &p0[0], &p1[0], &matrix3.m_front[0]);
if (joint_data->ctype == CT_FLEXIBLE)
NewtonUserJointSetRowSpringDamperAcceleration(joint, Joint::LINEAR_STIFF, Joint::LINEAR_DAMP);
else if (joint_data->ctype == CT_ROBUST)
NewtonUserJointSetRowAcceleration(joint, NewtonUserCalculateRowZeroAccelaration(joint));
NewtonUserJointSetRowStiffness(joint, joint_data->stiffness);
NewtonUserJointAddLinearRow(joint, &p0[0], &p1[0], &matrix3.m_up[0]);
if (joint_data->ctype == CT_FLEXIBLE)
NewtonUserJointSetRowSpringDamperAcceleration(joint, Joint::LINEAR_STIFF, Joint::LINEAR_DAMP);
else if (joint_data->ctype == CT_ROBUST)
NewtonUserJointSetRowAcceleration(joint, NewtonUserCalculateRowZeroAccelaration(joint));
NewtonUserJointSetRowStiffness(joint, joint_data->stiffness);
// Align to curve
if (cj_data->align) {
NewtonUserJointAddLinearRow(joint, &p00[0], &p11[0], &matrix3.m_front[0]);
if (joint_data->ctype == CT_ROBUST)
NewtonUserJointSetRowAcceleration(joint, NewtonUserCalculateRowZeroAccelaration(joint));
else
NewtonUserJointSetRowSpringDamperAcceleration(joint, Joint::LINEAR_STIFF, Joint::LINEAR_DAMP);
NewtonUserJointSetRowStiffness(joint, joint_data->stiffness);
NewtonUserJointAddLinearRow(joint, &p00[0], &p11[0], &matrix3.m_up[0]);
if (joint_data->ctype == CT_ROBUST)
NewtonUserJointSetRowAcceleration(joint, NewtonUserCalculateRowZeroAccelaration(joint));
else
NewtonUserJointSetRowSpringDamperAcceleration(joint, Joint::LINEAR_STIFF, Joint::LINEAR_DAMP);
NewtonUserJointSetRowStiffness(joint, joint_data->stiffness);
}
// Add linear friction or limits
dFloat min_posit = matrix3.UntransformVector(min_pt).m_z;
dFloat max_posit = matrix3.UntransformVector(max_pt).m_z;
dFloat cur_posit = matrix3.UntransformVector(p0).m_z;
dFloat margin = EPSILON + 0.01f * dAbs(cj_data->cur_vel);
if (cur_posit < min_posit - margin || (cur_posit < min_posit - Joint::LINEAR_LIMIT_EPSILON && dAbs(min_len) < EPSILON && cj_data->loop == false)) {
NewtonUserJointAddLinearRow(joint, &p0[0], &min_pt[0], &matrix3.m_right[0]);
NewtonUserJointSetRowMinimumFriction(joint, 0.0f);
if (joint_data->ctype == CT_FLEXIBLE)
NewtonUserJointSetRowSpringDamperAcceleration(joint, Joint::LINEAR_STIFF, Joint::LINEAR_DAMP);
else if (joint_data->ctype == CT_ROBUST)
NewtonUserJointSetRowAcceleration(joint, NewtonUserCalculateRowZeroAccelaration(joint));
NewtonUserJointSetRowStiffness(joint, joint_data->stiffness);
}
else if (cur_posit > max_posit + margin || (cur_posit > max_posit + Joint::LINEAR_LIMIT_EPSILON && dAbs(max_len - cj_data->curve_len) < EPSILON && cj_data->loop == false)) {
NewtonUserJointAddLinearRow(joint, &p0[0], &max_pt[0], &matrix3.m_right[0]);
NewtonUserJointSetRowMaximumFriction(joint, 0.0f);
if (joint_data->ctype == CT_FLEXIBLE)
NewtonUserJointSetRowSpringDamperAcceleration(joint, Joint::LINEAR_STIFF, Joint::LINEAR_DAMP);
else if (joint_data->ctype == CT_ROBUST)
NewtonUserJointSetRowAcceleration(joint, NewtonUserCalculateRowZeroAccelaration(joint));
NewtonUserJointSetRowStiffness(joint, joint_data->stiffness);
}
else {
dVector point(matrix3.UntransformVector(matrix0.m_posit));
point.m_z = 0.0f;
point = matrix3.TransformVector(point);
NewtonUserJointAddLinearRow(joint, &point[0], &matrix3.m_posit[0], &matrix3.m_right[0]);
dFloat power = cj_data->linear_friction * cj_data->controller;
NewtonUserJointSetRowMinimumFriction(joint, -power);
NewtonUserJointSetRowMaximumFriction(joint, power);
NewtonUserJointSetRowStiffness(joint, joint_data->stiffness);
}
// Add angular friction or limits
if (cj_data->rotate) {
if (cj_data->align) {
NewtonUserJointAddAngularRow(joint, 0.0f, &matrix3.m_right[0]);
dFloat power = cj_data->angular_friction * cj_data->controller;
NewtonUserJointSetRowMinimumFriction(joint, -power);
NewtonUserJointSetRowMaximumFriction(joint, power);
NewtonUserJointSetRowStiffness(joint, joint_data->stiffness);
}
else {
dFloat cur_cone_angle_cos = matrix0.m_right % cj_data->last_dir;
if (dAbs(cur_cone_angle_cos) < 0.99995f) {
dVector lateral_dir = matrix0.m_right * cj_data->last_dir;
Util::normalize_vector(lateral_dir);
NewtonUserJointAddAngularRow(joint, 0.0f, &lateral_dir[0]);
dFloat power = cj_data->angular_friction * cj_data->controller;
NewtonUserJointSetRowMinimumFriction(joint, -power);
NewtonUserJointSetRowMaximumFriction(joint, power);
NewtonUserJointSetRowStiffness(joint, joint_data->stiffness);
}
}
}
else if (cj_data->align) {
NewtonUserJointAddAngularRow(joint, Joint::c_calculate_angle(matrix0.m_front, matrix3.m_front, matrix3.m_right), &matrix3.m_right[0]);
if (joint_data->ctype == CT_FLEXIBLE)
NewtonUserJointSetRowSpringDamperAcceleration(joint, Joint::ANGULAR_STIFF, Joint::ANGULAR_DAMP);
else if (joint_data->ctype == CT_ROBUST)
NewtonUserJointSetRowAcceleration(joint, NewtonUserCalculateRowZeroAccelaration(joint));
NewtonUserJointSetRowStiffness(joint, joint_data->stiffness);
}
else {
// Get a point along the pin axis at some reasonable large distance from the pivot.
dVector q0(p0 + matrix0.m_right.Scale(MIN_JOINT_PIN_LENGTH));
dVector q1(p1 + matrix1.m_right.Scale(MIN_JOINT_PIN_LENGTH));
// Get the ankle point.
dVector r0(p0 + matrix0.m_front.Scale(MIN_JOINT_PIN_LENGTH));
dVector r1(p1 + matrix1.m_front.Scale(MIN_JOINT_PIN_LENGTH));
// Restrict rotation along all three orthonormal directions
NewtonUserJointAddLinearRow(joint, &q0[0], &q1[0], &matrix0.m_front[0]);
if (joint_data->ctype == CT_FLEXIBLE)
NewtonUserJointSetRowSpringDamperAcceleration(joint, Joint::LINEAR_STIFF, Joint::LINEAR_DAMP);
else if (joint_data->ctype == CT_ROBUST)
NewtonUserJointSetRowAcceleration(joint, NewtonUserCalculateRowZeroAccelaration(joint));
NewtonUserJointSetRowStiffness(joint, joint_data->stiffness);
NewtonUserJointAddLinearRow(joint, &q0[0], &q1[0], &matrix0.m_up[0]);
if (joint_data->ctype == CT_FLEXIBLE)
NewtonUserJointSetRowSpringDamperAcceleration(joint, Joint::LINEAR_STIFF, Joint::LINEAR_DAMP);
else if (joint_data->ctype == CT_ROBUST)
NewtonUserJointSetRowAcceleration(joint, NewtonUserCalculateRowZeroAccelaration(joint));
NewtonUserJointSetRowStiffness(joint, joint_data->stiffness);
NewtonUserJointAddLinearRow(joint, &r0[0], &r1[0], &matrix0.m_up[0]);
if (joint_data->ctype == CT_FLEXIBLE)
NewtonUserJointSetRowSpringDamperAcceleration(joint, Joint::LINEAR_STIFF, Joint::LINEAR_DAMP);
else if (joint_data->ctype == CT_ROBUST)
NewtonUserJointSetRowAcceleration(joint, NewtonUserCalculateRowZeroAccelaration(joint));
NewtonUserJointSetRowStiffness(joint, joint_data->stiffness);
}
cj_data->last_dir = matrix0.m_right;
}
