// Submitted by LegoCyclon static int seg2seg(const cpShape* shape1, const cpShape* shape2, cpContact* con) { cpSegmentShape* seg1 = (cpSegmentShape *)shape1; cpSegmentShape* seg2 = (cpSegmentShape *)shape2; cpVect v1 = cpvsub(seg1->tb, seg1->ta); cpVect v2 = cpvsub(seg2->tb, seg2->ta); cpFloat v1lsq = cpvlengthsq(v1); cpFloat v2lsq = cpvlengthsq(v2); // project seg2 onto seg1 cpVect p1a = cpvproject(cpvsub(seg2->ta, seg1->ta), v1); cpVect p1b = cpvproject(cpvsub(seg2->tb, seg1->ta), v1); // project seg1 onto seg2 cpVect p2a = cpvproject(cpvsub(seg1->ta, seg2->ta), v2); cpVect p2b = cpvproject(cpvsub(seg1->tb, seg2->ta), v2); // clamp projections to segment endcaps if (cpvdot(p1a, v1) < 0.0f) p1a = cpvzero; else if (cpvdot(p1a, v1) > 0.0f && cpvlengthsq(p1a) > v1lsq) p1a = v1; if (cpvdot(p1b, v1) < 0.0f) p1b = cpvzero; else if (cpvdot(p1b, v1) > 0.0f && cpvlengthsq(p1b) > v1lsq) p1b = v1; if (cpvdot(p2a, v2) < 0.0f) p2a = cpvzero; else if (cpvdot(p2a, v2) > 0.0f && cpvlengthsq(p2a) > v2lsq) p2a = v2; if (cpvdot(p2b, v2) < 0.0f) p2b = cpvzero; else if (cpvdot(p2b, v2) > 0.0f && cpvlengthsq(p2b) > v2lsq) p2b = v2; p1a = cpvadd(p1a, seg1->ta); p1b = cpvadd(p1b, seg1->ta); p2a = cpvadd(p2a, seg2->ta); p2b = cpvadd(p2b, seg2->ta); int num = 0; if (!circle2circleQuery(p1a, p2a, seg1->r, seg2->r, nextContactPoint(con, &num))) --num; if (!circle2circleQuery(p1b, p2b, seg1->r, seg2->r, nextContactPoint(con, &num))) --num; if (!circle2circleQuery(p1a, p2b, seg1->r, seg2->r, nextContactPoint(con, &num))) --num; if (!circle2circleQuery(p1b, p2a, seg1->r, seg2->r, nextContactPoint(con, &num))) --num; return num; }
static inline cpVect grooveConstrain(cpGrooveJoint *joint, cpVect j, cpFloat dt){ cpVect n = joint->grv_tn; cpVect jClamp = (joint->clamp*cpvcross(j, n) > 0.0f) ? j : cpvproject(j, n); return cpvclamp(jClamp, joint->constraint.maxForce*dt); }
static inline cpVect grooveConstrain(cpGrooveJoint *joint, cpVect j){ cpVect n = joint->grv_tn; cpVect jClamp = (joint->clamp*cpvcross(j, n) > 0.0f) ? j : cpvproject(j, n); return cpvclamp(jClamp, joint->jMaxLen); }
cpVect * bmx_cpvect_project(cpVect * vec, cpVect * vec1) { return bmx_cpvect_new(cpvproject(*vec, *vec1)); }