bool segmentCircleIntersect(xy_t p1, xy_t p2, xy_t circle, v_t radius, xy_t *p)
{
xy_t seg = {p2.x - p1.x, p2.y - p1.y};
xy_t cir = {circle.x - p1.x, circle.y - p1.y};
v_t proj = vectorProjectScalar(cir, seg);
xy_t closest;
if(proj < 0){
closest = p1;
}else if(proj > sqrt(seg.x * seg.x + seg.y * seg.y)){
closest = p2;
}else{
xy_t projv = vectorProject(cir, seg);
closest = (xy_t){p1.x + projv.x, p1.y + projv.y};
}
v_t dx = circle.x - closest.x;
v_t dy = circle.y - closest.y;
v_t dist = sqrt(dx * dx + dy * dy);
if(dist < radius){
p->x = closest.x;
p->y = closest.y;
return true;
}else{
return false;
}
}
float MeshShape::calculateMomentOfInertia(float *rotationvector) {
if (vectorDot(rotationvector, rotationvector) < EPSILON)
return 0;
int i;
float j = 0;
for (i = 0; i < mesh->vertexcount; i++) {
float proj[3];
vectorProject(proj, mesh->vertices[i].position, rotationvector);
vectorSub(proj, mesh->vertices[i].position, proj);
// float r = vectorLength(proj);
float r2 = vectorDot(proj, proj);
j += r2;
}
return j / i;
}
v_t distanceToSegment(xy_t p, xy_t p1, xy_t p2)
{
xy_t seg = {p2.x - p1.x, p2.y - p1.y};
xy_t cir = {p.x - p1.x, p.y - p1.y};
v_t proj = vectorProjectScalar(cir, seg);
xy_t closest;
if(proj < 0){
closest = p1;
}else if(proj > sqrt(seg.x * seg.x + seg.y * seg.y)){
closest = p2;
}else{
xy_t projv = vectorProject(cir, seg);
closest = (xy_t){p1.x + projv.x, p1.y + projv.y};
}
v_t dx = p.x - closest.x;
v_t dy = p.y - closest.y;
return sqrt(dx * dx + dy * dy);
}
void onMouse(int e, int x, int y, int, void *){
switch (e)
{
case cv::EVENT_LBUTTONDOWN:
start.x = x;
start.y = y;
prev = start;
mousedn = true;
rmousedn = false;
break;
case cv::EVENT_LBUTTONUP:
mousedn = false;
rmousedn = false;
break;
case cv::EVENT_MOUSEMOVE:
if(mousedn){
cv::Point curr(x,y);
angle_y += (curr.x - prev.x)/180.*CV_PI;
angle_x += (curr.y - prev.y)/180.*CV_PI;
prev = curr;
}
else if(rmousedn){
if(!alljoints){
if(cjoint != -1){
cv::Vec3f ray = lerpPoint(x,y,boundingBoxLerp,lc);
cv::Vec3f newPos = vectorProject(mat_to_vec3(trans * wcSkeletons[frame].points.col(cjoint)), ray);
cv::Mat(trans.inv() * vec3_to_mat4(newPos)).copyTo(wcSkeletons[frame].points.col(cjoint));
calculateSkeletonOffsetPoints(vidRecord, wcSkeletons, cylinderBody);
}
}else{
if(cjoint != -1){
cv::Vec3f ray = lerpPoint(x,y,boundingBoxLerp,lc);
cv::Vec3f newPosProj = vectorProject(mat_to_vec3(trans * wcSkeletons[frame].points.col(cjoint)), ray);
cv::Mat newPos = trans.inv() * vec3_to_mat4(newPosProj);
cv::Mat oldPos = wcSkeletons[frame].points.col(cjoint);
cv::Mat diff = newPos - oldPos;
newPos.copyTo(wcSkeletons[frame].points.col(cjoint));
for(int joint = 0; joint < NUMJOINTS; ++joint){
if(joint != cjoint){
cv::Mat newPosCopyTransform = wcSkeletons[frame].points.col(joint) + diff;
newPosCopyTransform.copyTo(wcSkeletons[frame].points.col(joint));
}
}
calculateSkeletonOffsetPoints(vidRecord, wcSkeletons, cylinderBody);
}
}
}
break;
case cv::EVENT_RBUTTONDOWN:
{
mousedn = false;
rmousedn = true;
cv::Point curr(x,y);
float cdist=10000;
cjoint=-1;
for(int joint=0;joint<NUMJOINTS;++joint)
{
cv::Vec3f jv = mat_to_vec3(trans * wcSkeletons[frame].points.col(joint));
cv::Vec2f jv2 = mat4_to_vec2(getCameraMatrixScene() * vec3_to_mat4(jv));
cv::Point pj(jv2(0), jv2(1));
float dist = cv::norm(curr-pj);
if(dist < cdist && dist < 10){
cdist = dist;
cjoint = joint;
}
}
lastjoint = cjoint;
break;
}
case cv::EVENT_RBUTTONUP:
{
mousedn = false;
rmousedn = false;
cjoint = -1;
}
default:
break;
}
}
bool checkSphereMeshCollision(float *sphereposition, float r, Mesh *mesh,
float *normal, float *contactpoint) {
float linenormal[3];
float pointnormal[3];
float maxdist = 0;
bool planecollision = false;
bool linecollision = false;
bool pointcollision = false;
int i, j;
for (i = 0; i < mesh->polygoncount; i++) {
class Polygon *polygon = &mesh->polygons[i];
float dist = distanceFromPlane(sphereposition, polygon->planenormal,
polygon->planedistance);
if (dist < r && dist > -r) {
bool directcollision = true;
for (j = 0; j < polygon->vertexcount; j++) {
float *p1 = polygon->vertices[j]->position;
float *p2 = polygon->vertices[(j + 1) % polygon->vertexcount]->position;
float *p3 = polygon->vertices[(j + 2) % polygon->vertexcount]->position;
float v1[3], v2[3];
vectorSub(v1, p2, p1);
// Collision for polygon surface
vectorSub(v2, p3, p2);
float t1[3];
vectorProject(t1, v2, v1);
float norm[3];
vectorSub(norm, v2, t1);
vectorNormalize(norm);
// Collision for polygon edges
float newpoint[3];
vectorSub(newpoint, sphereposition, p1);
float dist2 = vectorDot(newpoint, norm);
if (dist2 < 0) {
directcollision = false;
float projloc = vectorDot(newpoint, v1) / vectorDot(v1, v1);
if (projloc >= 0 && projloc <= 1) {
float proj[3];
vectorScale(proj, v1, projloc);
float projorth[3];
vectorSub(projorth, newpoint, proj);
float l2 = vectorDot(projorth, projorth);
if (l2 < r * r) {
vectorNormalize(linenormal, projorth);
if (dist < 0)
vectorScale(linenormal, -1);
linecollision = true;
}
}
}
// Collision for polygon vertices
float pointdiff[3];
vectorSub(pointdiff, sphereposition, p1);
float l3 = vectorDot(pointdiff, pointdiff);
if (l3 < r * r) {
vectorScale(pointnormal, pointdiff, 1.0 / sqrt(l3));
if (dist < 0)
vectorScale(pointnormal, -1);
pointcollision = true;
}
}
if (directcollision) {
if (dist > maxdist || !planecollision) {
vectorCopy(normal, polygon->planenormal);
maxdist = dist;
planecollision = true;
}
}
}
}
if (planecollision) {
vectorScale(contactpoint, normal, -r);
vectorAdd(contactpoint, sphereposition);
} else if (linecollision) {
vectorScale(contactpoint, linenormal, -r);
vectorAdd(contactpoint, sphereposition);
vectorCopy(normal, linenormal);
} else if (pointcollision) {
vectorScale(contactpoint, pointnormal, -r);
vectorAdd(contactpoint, sphereposition);
vectorCopy(normal, pointnormal);
} else {
return false;
}
return true;
}