dgInt32 dgCollisionCone::CalculatePlaneIntersectionSimd ( const dgVector& normal, const dgVector& origin, dgVector contactsOut[]) const { #ifdef DG_BUILD_SIMD_CODE dgInt32 i; dgInt32 count; dgFloat32 y; dgFloat32 z; dgFloat32 cosAng; dgFloat32 sinAng; dgFloat32 magInv; simd_type tmp0; simd_type mag2; if (dgAbsf (normal.m_x) < dgFloat32 (0.999f)) { // magInv = dgRsqrt (normal.m_y * normal.m_y + normal.m_z * normal.m_z); // cosAng = normal.m_y * magInv; // sinAng = normal.m_z * magInv; // dgMatrix matrix (dgGetIdentityMatrix ()); // matrix[1][1] = cosAng; // matrix[1][2] = sinAng; // matrix[2][1] = -sinAng; // matrix[2][2] = cosAng; // dgVector normal2 (matrix.UnrotateVector (normal)); // dgVector origin2 (matrix.UnrotateVector (origin)); // count = dgCollisionConvex::CalculatePlaneIntersection (normal1, origin1, contactsOut); // matrix.TransformTriplex (contactsOut, sizeof (dgVector), contactsOut, sizeof (dgVector), count); y = normal.m_y * normal.m_y + normal.m_z * normal.m_z; mag2 = simd_load_s (y); tmp0 = simd_rsqrt_s(mag2); simd_store_s (simd_mul_s (simd_mul_s(*(simd_type*)&m_nrh0p5, tmp0), simd_mul_sub_s (*(simd_type*)&m_nrh3p0, simd_mul_s (mag2, tmp0), tmp0)), &magInv); cosAng = normal.m_y * magInv; sinAng = normal.m_z * magInv; _ASSERTE (dgAbsf (normal.m_z * cosAng - normal.m_y * sinAng) < dgFloat32 (1.0e-4f)); // dgVector normal1 (normal.m_x, normal.m_y * cosAng + normal.m_z * sinAng, // normal.m_z * cosAng - normal.m_y * sinAng, dgFloat32 (0.0f)); dgVector normal1 (normal.m_x, normal.m_y * cosAng + normal.m_z * sinAng, dgFloat32 (0.0f), dgFloat32 (0.0f)); dgVector origin1 (origin.m_x, origin.m_y * cosAng + origin.m_z * sinAng, origin.m_z * cosAng - origin.m_y * sinAng, dgFloat32 (0.0f)); count = dgCollisionConvex::CalculatePlaneIntersectionSimd (normal1, origin1, contactsOut); for (i = 0; i < count; i ++) { y = contactsOut[i].m_y; z = contactsOut[i].m_z; contactsOut[i].m_y = y * cosAng - z * sinAng; contactsOut[i].m_z = z * cosAng + y * sinAng; } } else { count = dgCollisionConvex::CalculatePlaneIntersectionSimd (normal, origin, contactsOut); } return count; #else return 0; #endif }
dgInt32 dgCollisionCone::CalculatePlaneIntersection (const dgVector& normal, const dgVector& origin, dgVector* const contactsOut) const { dgInt32 count = 0; if (normal.m_x > dgFloat32(0.99f)) { contactsOut[0] = dgVector(m_height, dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f)); count = 1; } else if (normal.m_x < dgFloat32(-0.995f)) { if (normal.m_x < dgFloat32(-0.9995f)) { dgMatrix matrix(normal); matrix.m_posit.m_x = origin.m_x; dgVector scale(m_radius); const int n = sizeof (m_unitCircle) / sizeof (m_unitCircle[0]); for (dgInt32 i = 0; i < n; i++) { contactsOut[i] = matrix.TransformVector(m_unitCircle[i].CompProduct4(scale)) & dgVector::m_triplexMask; } count = RectifyConvexSlice(n, normal, contactsOut); } else { dgFloat32 magInv = dgRsqrt(normal.m_y * normal.m_y + normal.m_z * normal.m_z); dgFloat32 cosAng = normal.m_y * magInv; dgFloat32 sinAng = normal.m_z * magInv; dgAssert(dgAbsf(normal.m_z * cosAng - normal.m_y * sinAng) < dgFloat32(1.0e-4f)); dgVector normal1(normal.m_x, normal.m_y * cosAng + normal.m_z * sinAng, dgFloat32(0.0f), dgFloat32(0.0f)); dgVector origin1(origin.m_x, origin.m_y * cosAng + origin.m_z * sinAng, origin.m_z * cosAng - origin.m_y * sinAng, dgFloat32(0.0f)); count = dgCollisionConvex::CalculatePlaneIntersection(normal1, origin1, contactsOut); if (count > 6) { dgInt32 dy = 2 * 6; dgInt32 dx = 2 * count; dgInt32 acc = dy - count; dgInt32 index = 0; for (dgInt32 i = 0; i < count; i++) { if (acc > 0) { contactsOut[index] = contactsOut[i]; index++; acc -= dx; } acc += dy; } count = index; } for (dgInt32 i = 0; i < count; i++) { dgFloat32 y = contactsOut[i].m_y; dgFloat32 z = contactsOut[i].m_z; contactsOut[i].m_y = y * cosAng - z * sinAng; contactsOut[i].m_z = z * cosAng + y * sinAng; } } } else { dgFloat32 magInv = dgRsqrt(normal.m_y * normal.m_y + normal.m_z * normal.m_z); dgFloat32 cosAng = normal.m_y * magInv; dgFloat32 sinAng = normal.m_z * magInv; dgAssert(dgAbsf(normal.m_z * cosAng - normal.m_y * sinAng) < dgFloat32(1.0e-4f)); dgVector normal1(normal.m_x, normal.m_y * cosAng + normal.m_z * sinAng, dgFloat32(0.0f), dgFloat32(0.0f)); dgVector origin1(origin.m_x, origin.m_y * cosAng + origin.m_z * sinAng, origin.m_z * cosAng - origin.m_y * sinAng, dgFloat32(0.0f)); count = 0; int i0 = 2; dgVector test0((m_profile[i0] - origin1).DotProduct4(normal1)); for (int i = 0; (i < 3) && (count < 2); i++) { dgVector test1((m_profile[i] - origin1).DotProduct4(normal1)); dgVector acrossPlane(test0.CompProduct4(test1)); if (acrossPlane.m_x < 0.0f) { dgVector step(m_profile[i] - m_profile[i0]); contactsOut[count] = m_profile[i0] - step.Scale4(test0.m_x / (step.DotProduct4(normal1).m_x)); count++; } i0 = i; test0 = test1; } for (dgInt32 i = 0; i < count; i++) { dgFloat32 y = contactsOut[i].m_y; dgFloat32 z = contactsOut[i].m_z; contactsOut[i].m_y = y * cosAng - z * sinAng; contactsOut[i].m_z = z * cosAng + y * sinAng; } } return count; }
void GLWidget::project_quad_texture() { const int sx = width(), sy = height(); Point pt[4]; static Vec3f corners[] = { Vec3f(0, 0, 0), Vec3f(sx-1, 0, 0), Vec3f(0, sy-1, 0), Vec3f(sx-1, sy-1, 0) }; for (int i = 0; i < 4; i++) { pt[i] = project(Vec3f(corners[i].x - sx/2, corners[i].y - sy/2, 0)); pt[i].x += sx/2; pt[i].y += sy/2; } Vec3f normal1(0, 0, 1); Vec3f normal2; { Vec3f foo[3]; for (int i = 0; i < 3; i++) foo[i] = project2(corners[i]); normal2 = normal(foo[0], foo[1], foo[2]); } double dir = normal1.x * normal2.x + normal1.y * normal2.y + normal1.z * normal2.z; QImage& tex = dir < 0 ? back : front; int ow = tex.width(), oh = tex.height(); Vec2f p2[4]; for (int i = 0; i < 4; i++) p2[i] = Vec2f(pt[i].x, pt[i].y); QImage texture(QSize(sx, sy), QImage::Format_RGB888); QColor bgColor = palette().color(QPalette::Current, QPalette::Window); texture.fill(bgColor); const Vec2f projected[2][3] = { { p2[0], p2[1], p2[2] }, { p2[3], p2[1], p2[2] } }; const Vec2f origs[2][3] = { { Vec2f(0, 0), Vec2f(ow-1, 0), Vec2f(0, oh-1) }, { Vec2f(ow-1, oh-1), Vec2f(ow-1, 0), Vec2f(0, oh-1) } }; const Triangle triangles[2] = { Triangle(projected[0][0], projected[0][1], projected[0][2]), Triangle(projected[1][0], projected[1][1], projected[1][2]) }; int orig_pitch = tex.bytesPerLine(); int dest_pitch = texture.bytesPerLine(); const unsigned char* orig = tex.bits(); unsigned char* dest = texture.bits(); int orig_depth = tex.depth() / 8; int dest_depth = texture.depth() / 8; /* image breakage? */ if (orig_depth < 3) return; for (int y = 0; y < sy; y++) for (int x = 0; x < sx; x++) { Vec2f pos; pos.x = x; pos.y = y; for (int i = 0; i < 2; i++) { Vec2f coords; if (triangles[i].barycentric_coords(pos, coords)) { int px = origs[i][0].x + coords.x * (origs[i][2].x - origs[i][0].x) + coords.y * (origs[i][1].x - origs[i][0].x); int py = origs[i][0].y + coords.x * (origs[i][2].y - origs[i][0].y) + coords.y * (origs[i][1].y - origs[i][0].y); int r = orig[py * orig_pitch + px * orig_depth + 2]; int g = orig[py * orig_pitch + px * orig_depth + 1]; int b = orig[py * orig_pitch + px * orig_depth + 0]; dest[y * dest_pitch + x * dest_depth + 0] = r; dest[y * dest_pitch + x * dest_depth + 1] = g; dest[y * dest_pitch + x * dest_depth + 2] = b; break; } } } this->texture = texture; }
dgInt32 dgCollisionChamferCylinder::CalculatePlaneIntersection (const dgVector& normal, const dgVector& origin, dgVector* const contactsOut) const { dgInt32 count; if (dgAbsf (normal.m_x) < dgFloat32 (0.999f)) { dgFloat32 magInv = dgRsqrt (normal.m_y * normal.m_y + normal.m_z * normal.m_z); dgFloat32 cosAng = normal.m_y * magInv; dgFloat32 sinAng = normal.m_z * magInv; _ASSERTE (dgAbsf (normal.m_z * cosAng - normal.m_y * sinAng) < dgFloat32 (1.0e-4f)); dgVector normal1 (normal.m_x, normal.m_y * cosAng + normal.m_z * sinAng, dgFloat32 (0.0f), dgFloat32 (0.0f)); dgVector origin1 (origin.m_x, origin.m_y * cosAng + origin.m_z * sinAng, origin.m_z * cosAng - origin.m_y * sinAng, dgFloat32 (0.0f)); dgPlane plane (normal1, - (normal1 % origin1)); count = 0; dgVector maxDir ((normal1.m_x > dgFloat32 (0.0f)) ? m_silhuette[0].m_x : -m_silhuette[0].m_x, (normal1.m_y > dgFloat32 (0.0f)) ? m_silhuette[0].m_y : -m_silhuette[0].m_y, dgFloat32 (0.0f), dgFloat32 (0.0f)); dgFloat32 test0 = plane.Evalue (maxDir); dgFloat32 test1 = plane.Evalue (maxDir.Scale (dgFloat32 (-1.0f))); if ((test0 * test1) > dgFloat32 (0.0f)) { test0 = plane.m_w + plane.m_y * m_radius; if (dgAbsf (test0) < m_height) { contactsOut[count] = normal1.Scale (-test0); contactsOut[count].m_y += m_radius; count ++; } else { test0 = plane.m_w - plane.m_y * m_radius; if (dgAbsf (test0) < m_height) { contactsOut[count] = normal1.Scale (-test0); contactsOut[count].m_y -= m_radius; count ++; } } } else { dgVector dp (m_silhuette[1] - m_silhuette[0]); dgFloat32 den = normal1 % dp; _ASSERTE (dgAbsf (den) > dgFloat32 (0.0f)); test0 = -plane.Evalue (m_silhuette[0]) / den; if ((test0 <= dgFloat32 (1.0)) && (test0 >= dgFloat32 (0.0f))) { contactsOut[count] = m_silhuette[0] + dp.Scale (test0); count ++; } if (count < 2) { test0 = plane.m_w - plane.m_y * m_radius; if (dgAbsf (test0) < m_height) { dgFloat32 r = -m_radius; dgFloat32 d = plane.m_w + r * plane.m_y; dgFloat32 a = plane.m_x * plane.m_x + plane.m_y * plane.m_y; dgFloat32 b = dgFloat32 (2.0f) * plane.m_y * d; dgFloat32 c = d * d - m_height * m_height * plane.m_x * plane.m_x; dgFloat32 desc = b * b - dgFloat32 (4.0f) * a * c; if (desc > dgFloat32 (0.0f)) { _ASSERTE (dgAbsf (a) > dgFloat32 (0.0f)); desc = dgSqrt (desc); a = - dgFloat32 (0.5f) * b / a; dgFloat32 y0 = a + desc; dgFloat32 y1 = a - desc; if (y0 > dgFloat32 (0.0f)) { y0 = y1; } _ASSERTE (y0 < dgFloat32 (0.0f)); _ASSERTE (dgAbsf (plane.m_x) > dgFloat32 (0.0f)); dgFloat32 x = - (plane.m_y * y0 + d) / plane.m_x; contactsOut[count] = dgVector (x, y0 + r, dgFloat32 (0.0f), dgFloat32 (0.0f)); count ++; } } } if (count < 2) { dgVector dp (m_silhuette[3] - m_silhuette[2]); den = normal1 % dp; _ASSERTE (dgAbsf (den) > dgFloat32 (0.0f)); test0 = - plane.Evalue (m_silhuette[2]) / den; if ((test0 <= dgFloat32 (1.0)) && (test0 >= dgFloat32 (0.0f))) { contactsOut[count] = m_silhuette[2] + dp.Scale (test0); count ++; } } if (count < 2) { test0 = plane.m_w + plane.m_y * m_radius; if (dgAbsf (test0) < m_height) { dgFloat32 r = m_radius; dgFloat32 d = plane.m_w + r * plane.m_y; dgFloat32 a = plane.m_x * plane.m_x + plane.m_y * plane.m_y; dgFloat32 b = dgFloat32 (2.0f) * plane.m_y * d; dgFloat32 c = d * d - m_height * m_height * plane.m_x * plane.m_x; dgFloat32 desc = b * b - dgFloat32 (4.0f) * a * c; if (desc > dgFloat32 (0.0f)) { _ASSERTE (dgAbsf (a) > dgFloat32 (0.0f)); desc = dgSqrt (desc); a = - dgFloat32 (0.5f) * b / a; dgFloat32 y0 = a + desc; dgFloat32 y1 = a - desc; if (y0 < dgFloat32 (0.0f)) { y0 = y1; } _ASSERTE (y0 > dgFloat32 (0.0f)); _ASSERTE (dgAbsf (plane.m_x) > dgFloat32 (0.0f)); dgFloat32 x = - (plane.m_y * y0 + d) / plane.m_x; contactsOut[count] = dgVector (x, y0 + r, dgFloat32 (0.0f), dgFloat32 (0.0f)); count ++; } } } } for (dgInt32 i = 0; i < count; i ++) { dgFloat32 y = contactsOut[i].m_y; dgFloat32 z = contactsOut[i].m_z; contactsOut[i].m_y = y * cosAng - z * sinAng; contactsOut[i].m_z = z * cosAng + y * sinAng; } } else { count = dgCollisionConvex::CalculatePlaneIntersection (normal, origin, contactsOut); } return count; }
BaseIF* makePlate(const Real& height, const Real& thick, const Real& radius, const int& doHoles, const Real& holeRadius, const Real& holeSpace) { RealVect zero(D_DECL(0.0,0.0,0.0)); RealVect xAxis(D_DECL(1.0,0.0,0.0)); bool inside = true; // Create the plate without holes Vector<BaseIF*> pieces; RealVect normal1(D_DECL(1.0,0.0,0.0)); RealVect point1(D_DECL(height,0.0,0.0)); PlaneIF plane1(normal1,point1,inside); pieces.push_back(&plane1); RealVect normal2(D_DECL(-1.0,0.0,0.0)); RealVect point2(D_DECL(height+thick,0.0,0.0)); PlaneIF plane2(normal2,point2,inside); pieces.push_back(&plane2); TiltedCylinderIF middle(radius,xAxis,zero,inside); pieces.push_back(&middle); IntersectionIF plate(pieces); // Make the drills Vector<BaseIF*> drillBits; // Compute how many drills are needed in each direciton - 2*num+1 - // conservatively int num = (int)((radius - holeRadius) / holeSpace + 1.0); if (doHoles != 0) { for (int i = -num; i <= num; i++) { for (int j = -num; j <= num; j++) { RealVect center(D_DECL(0.0,i*holeSpace,j*holeSpace)); TiltedCylinderIF* drill = new TiltedCylinderIF(holeRadius,xAxis,center,inside); drillBits.push_back(drill); } } } UnionIF drills(drillBits); ComplementIF notDrills(drills,true); // Drill the plate IntersectionIF* holyPlate = new IntersectionIF(plate,notDrills); return holyPlate; }
void GLWidget::project_quad_texture() { const int sx = width(), sy = height(); Point pt[4]; static Vec3f corners[] = { Vec3f(0, 0, 0), Vec3f(sx-1, 0, 0), Vec3f(0, sy-1, 0), Vec3f(sx-1, sy-1, 0) }; for (int i = 0; i < 4; i++) { pt[i] = project(Vec3f(corners[i].x - sx/2, corners[i].y - sy/2, 0)); pt[i].x += sx/2; pt[i].y += sy/2; } Vec3f normal1(0, 0, 1); Vec3f normal2; { Vec3f foo[3]; for (int i = 0; i < 3; i++) foo[i] = project2(corners[i]); normal2 = normal(foo[0], foo[1], foo[2]); } double dir = normal1.x * normal2.x + normal1.y * normal2.y + normal1.z * normal2.z; QImage& tex = dir < 0 ? back : front; int ow = tex.width(), oh = tex.height(); Vec2f p2[4]; for (int i = 0; i < 4; i++) p2[i] = Vec2f(pt[i].x, pt[i].y); QImage texture(QSize(sx, sy), QImage::Format_RGB888); texture.fill(Qt::black); const Vec2f projected[2][3] = { { p2[0], p2[1], p2[2] }, { p2[3], p2[1], p2[2] } }; const Vec2f origs[2][3] = { { Vec2f(0, 0), Vec2f(ow-1, 0), Vec2f(0, oh-1) }, { Vec2f(ow-1, oh-1), Vec2f(ow-1, 0), Vec2f(0, oh-1) } }; const Triangle triangles[2] = { Triangle(projected[0][0], projected[0][1], projected[0][2]), Triangle(projected[1][0], projected[1][1], projected[1][2]) }; int orig_pitch = tex.bytesPerLine(); int dest_pitch = texture.bytesPerLine(); const unsigned char* orig = tex.bits(); unsigned char* dest = texture.bits(); int orig_depth = tex.depth() / 8; int dest_depth = texture.depth() / 8; /* image breakage? */ if (orig_depth < 3) return; for (int y = 0; y < sy; y++) for (int x = 0; x < sx; x++) { Vec2f pos; pos.x = x; pos.y = y; for (int i = 0; i < 2; i++) { Vec2f coords; if (triangles[i].barycentric_coords(pos, coords)) { double qx = origs[i][0].x + coords.x * (origs[i][2].x - origs[i][0].x) + coords.y * (origs[i][1].x - origs[i][0].x); double qy = origs[i][0].y + coords.x * (origs[i][2].y - origs[i][0].y) + coords.y * (origs[i][1].y - origs[i][0].y); int qx1 = std::min<int>(ow - 1, std::max<int>(0, qx - 0.5)); int qy1 = std::min<int>(oh - 1, std::max<int>(0, qy - 0.5)); int qx2 = std::min<int>(ow - 1, std::max<int>(0, qx + 0.5)); int qy2 = std::min<int>(oh - 1, std::max<int>(0, qy + 0.5)); double dx1 = qx1 - qx; double dy1 = qy1 - qy; double dx2 = qx2 - qx; double dy2 = qy2 - qy; double d1 = 2 - (dx1 * dx1 + dy1 * dy1); double d2 = 2 - (dx2 * dx2 + dy2 * dy2); double d3 = 2 - (dx2 * dx2 + dy1 * dy1); double d4 = 2 - (dx1 * dx1 + dy2 * dy2); double inv_norm = 1. / (d1 + d2 + d3 + d4); d1 *= inv_norm; d2 *= inv_norm; d3 *= inv_norm; d4 *= inv_norm; double r = d1 * (double) orig[qy1 * orig_pitch + qx1 * orig_depth + 2] + d2 * (double) orig[qy2 * orig_pitch + qx2 * orig_depth + 2] + d3 * (double) orig[qy1 * orig_pitch + qx2 * orig_depth + 2] + d4 * (double) orig[qy2 * orig_pitch + qx1 * orig_depth + 2]; double g = d1 * (double) orig[qy1 * orig_pitch + qx1 * orig_depth + 1] + d2 * (double) orig[qy2 * orig_pitch + qx2 * orig_depth + 1] + d3 * (double) orig[qy1 * orig_pitch + qx2 * orig_depth + 1] + d4 * (double) orig[qy2 * orig_pitch + qx1 * orig_depth + 1]; double b = d1 * (double) orig[qy1 * orig_pitch + qx1 * orig_depth + 0] + d2 * (double) orig[qy2 * orig_pitch + qx2 * orig_depth + 0] + d3 * (double) orig[qy1 * orig_pitch + qx2 * orig_depth + 0] + d4 * (double) orig[qy2 * orig_pitch + qx1 * orig_depth + 0]; dest[y * dest_pitch + x * dest_depth + 0] = std::max<int>(0, std::min<int>(255, r)); dest[y * dest_pitch + x * dest_depth + 1] = std::max<int>(0, std::min<int>(255, g)); dest[y * dest_pitch + x * dest_depth + 2] = std::max<int>(0, std::min<int>(255, b)); break; } } } pixmap = QPixmap::fromImage(texture); }
bool dgCollisionConvexHull::RemoveCoplanarEdge (dgPolyhedra& polyhedra, const dgBigVector* const hullVertexArray) const { bool removeEdge = false; // remove coplanar edges dgInt32 mark = polyhedra.IncLRU(); dgPolyhedra::Iterator iter (polyhedra); for (iter.Begin(); iter; ) { dgEdge* edge0 = &(*iter); iter ++; if (edge0->m_incidentFace != -1) { if (edge0->m_mark < mark) { edge0->m_mark = mark; edge0->m_twin->m_mark = mark; dgBigVector normal0 (FaceNormal (edge0, &hullVertexArray[0])); dgBigVector normal1 (FaceNormal (edge0->m_twin, &hullVertexArray[0])); dgFloat64 test = normal0.DotProduct(normal1).GetScalar(); if (test > dgFloat64 (0.99995f)) { if ((edge0->m_twin->m_next->m_twin->m_next != edge0) && (edge0->m_next->m_twin->m_next != edge0->m_twin)) { #define DG_MAX_EDGE_ANGLE dgFloat32 (1.0e-3f) if (edge0->m_twin == &(*iter)) { if (iter) { iter ++; } } dgBigVector e1 (hullVertexArray[edge0->m_twin->m_next->m_next->m_incidentVertex] - hullVertexArray[edge0->m_incidentVertex]); dgBigVector e0 (hullVertexArray[edge0->m_incidentVertex] - hullVertexArray[edge0->m_prev->m_incidentVertex]); dgAssert(e0.m_w == dgFloat64(0.0f)); dgAssert(e1.m_w == dgFloat64(0.0f)); dgAssert (e0.DotProduct(e0).GetScalar() >= dgFloat64 (0.0f)); dgAssert (e1.DotProduct(e1).GetScalar() >= dgFloat64 (0.0f)); e0 = e0.Scale (dgFloat64 (1.0f) / sqrt (e0.DotProduct(e0).GetScalar())); e1 = e1.Scale (dgFloat64 (1.0f) / sqrt (e1.DotProduct(e1).GetScalar())); dgBigVector n1 (e0.CrossProduct(e1)); dgFloat64 projection = n1.DotProduct(normal0).GetScalar(); if (projection >= DG_MAX_EDGE_ANGLE) { dgBigVector e11 (hullVertexArray[edge0->m_next->m_next->m_incidentVertex] - hullVertexArray[edge0->m_twin->m_incidentVertex]); dgBigVector e00 (hullVertexArray[edge0->m_twin->m_incidentVertex] - hullVertexArray[edge0->m_twin->m_prev->m_incidentVertex]); dgAssert (e00.m_w == dgFloat64 (0.0f)); dgAssert (e11.m_w == dgFloat64 (0.0f)); dgAssert (e00.DotProduct(e00).GetScalar() >= dgFloat64 (0.0f)); dgAssert (e11.DotProduct(e11).GetScalar() >= dgFloat64 (0.0f)); e00 = e00.Scale(dgFloat64(1.0f) / sqrt(e00.DotProduct(e00).GetScalar())); e11 = e11.Scale(dgFloat64(1.0f) / sqrt(e11.DotProduct(e11).GetScalar())); dgBigVector n11 (e00.CrossProduct(e11)); projection = n11.DotProduct(normal0).GetScalar(); if (projection >= DG_MAX_EDGE_ANGLE) { dgAssert (&(*iter) != edge0); dgAssert (&(*iter) != edge0->m_twin); polyhedra.DeleteEdge(edge0); removeEdge = true; } } } else { dgEdge* next = edge0->m_next; dgEdge* prev = edge0->m_prev; polyhedra.DeleteEdge(edge0); for (edge0 = next; edge0->m_prev->m_twin == edge0; edge0 = next) { next = edge0->m_next; polyhedra.DeleteEdge(edge0); } for (edge0 = prev; edge0->m_next->m_twin == edge0; edge0 = prev) { prev = edge0->m_prev; polyhedra.DeleteEdge(edge0); } iter.Begin(); removeEdge = true; } } } } } return removeEdge; }
void dSceneRender::DrawCone(int segments, dFloat radius, dFloat heigh) { dVector q1 (-heigh / 2.0f, radius, 0.0f, 0.0f); dMatrix rotation (dPitchMatrix(2.0f * 3.1614f/segments)); dVector cap[1024]; dAssert (segments < sizeof (cap)/sizeof (cap[0])); cap[segments] = q1; for (int i = 0; i < segments; i ++) { cap[i] = q1; q1 = rotation.RotateVector(q1); } dVector normal1 (-1.0f, 0.0f, 0.0f, 0.0f); BeginTriangle(); for (int i = 2; i < segments; i ++) { SubmitNormal(normal1); DrawTriangle(cap[0], cap[i], cap[i - 1]); } dVector p0 ( heigh / 2.0f, 0.0f, 0.0f, 0.0f); for (int i = 0; i < segments; i ++) { dVector p1 (cap[i]); dVector p2 (cap[i + 1]); dVector normal ((p1 - p0) * (p2 - p0)); normal = normal.Scale (1.0f / dSqrt(normal % normal)); SubmitNormal(normal); DrawTriangle(p0, p1, p2); } End(); /* { dVector p1 (-heigh / 2.0f, radius, 0.0f, 0.0f); dMatrix rotation (dPitchMatrix(2.0f * 3.1614f/segments)); dVector cap[1024]; dAssert (segments < sizeof (cap)/sizeof (cap[0])); cap[segments] = p1; for (int i = 0; i < segments; i ++) { cap[i] = p1; p1 = rotation.RotateVector(p1); } dVector normal1 (-1.0f, 0.0f, 0.0f, 0.0f); BeginLine(); DrawLine(cap[0], cap[0] + normal1); dVector p0 ( heigh / 2.0f, 0.0f, 0.0f, 0.0f); for (int i = 0; i < segments; i ++) { dVector p1 (cap[i]); dVector p2 (cap[i + 1]); dVector normal ((p1 - p0) * (p2 - p0)); normal = normal.Scale (1.0f / dSqrt(normal % normal)); DrawLine(p0, p0 + normal); } End(); } */ }
void ShapeParabolicRectangle::generatePrimitives(SoAction *action) { SoPrimitiveVertex pv; SoState *state = action->getState(); SbBool useTexFunc = ( SoTextureCoordinateElement::getType(state) == SoTextureCoordinateElement::FUNCTION ); const SoTextureCoordinateElement* tce = 0; if ( useTexFunc ) tce = SoTextureCoordinateElement::getInstance(state); SbVec3f point; const int rows = 12; // Number of points per row const int columns = 12; // Number of points per column const int totalPoints = (rows)*(columns); // Total points in the grid float vertex[totalPoints][6]; int h = 0; double ui = 0; double vj = 0; for (int i = 0; i < rows; ++i ) { ui =( 1.0 /(double)(rows-1) ) * i; for ( int j = 0 ; j < columns ; ++j ) { vj = ( 1.0 /(double)(columns-1) ) * j; Point3D point = GetPoint3D(ui, vj); NormalVector normal; if( activeSide.getValue() == 0 ) normal = -GetNormal(ui, vj); else normal = GetNormal(ui, vj); vertex[h][0] = point.x; vertex[h][1] = point.y; vertex[h][2] = point.z; vertex[h][3] = normal.x; vertex[h][4] = normal.y; vertex[h][5] = normal.z; pv.setPoint( vertex[h][0], vertex[h][1], vertex[h][2] ); h++; //Increase h to the next point. } } float u = 1; float v = 1; beginShape(action, QUADS ); for( int irow = 0; irow < (rows-1); ++irow ) { for( int icolumn = 0; icolumn < (columns-1); ++icolumn ) { int index0 = irow*columns + icolumn; SbVec3f point0( vertex[index0][0], vertex[index0][1], vertex[index0][2] ); SbVec3f normal0(vertex[index0][3], vertex[index0][4], vertex[index0][5] ); SbVec4f texCoord0 = useTexFunc ? tce->get(point0, normal0): SbVec4f( u,v, 0.0, 1.0 ); pv.setPoint(point0); pv.setNormal(normal0); pv.setTextureCoords(texCoord0); shapeVertex(&pv); int index1 = index0 + 1; SbVec3f point1( vertex[index1][0], vertex[index1][1], vertex[index1][2] ); SbVec3f normal1(vertex[index1][3], vertex[index1][4], vertex[index1][5] ); SbVec4f texCoord1 = useTexFunc ? tce->get(point1, normal1): SbVec4f( u,v, 0.0, 1.0 ); pv.setPoint(point1); pv.setNormal(normal1); pv.setTextureCoords(texCoord1); shapeVertex(&pv); int index3 = index0 + columns; int index2 = index3 + 1; SbVec3f point2( vertex[index2][0], vertex[index2][1], vertex[index2][2] ); SbVec3f normal2(vertex[index2][3], vertex[index2][4], vertex[index2][5] ); SbVec4f texCoord2 = useTexFunc ? tce->get(point2, normal2): SbVec4f( u,v, 0.0, 1.0 ); pv.setPoint(point2); pv.setNormal(normal2); pv.setTextureCoords(texCoord2); shapeVertex(&pv); SbVec3f point3( vertex[index3][0], vertex[index3][1], vertex[index3][2] ); SbVec3f normal3(vertex[index3][3], vertex[index3][4], vertex[index3][5] ); SbVec4f texCoord3 = useTexFunc ? tce->get(point3, normal3): SbVec4f( u,v, 0.0, 1.0 ); pv.setPoint(point3); pv.setNormal(normal3); pv.setTextureCoords(texCoord3); shapeVertex(&pv); } } endShape(); }
void QmaxButton::createArrowBackground(QPainter &painter) { // printf("Arrow BackGround2\n"); QRect scaledRect; scaledRect = matrix.mapRect(QRect(0, 0, this->logicalSize.width(), this->logicalSize.height())); QImage image(scaledRect.width(), scaledRect.height(), QImage::Format_ARGB32_Premultiplied); image.fill(QColor(0, 0, 0, 0).rgba()); //QPainter painter(image); painter.setRenderHint(QPainter::SmoothPixmapTransform); painter.setRenderHint(QPainter::Antialiasing); painter.setPen(Qt::NoPen); painter.drawImage(0, 0, image); if(Colors::useEightBitPalette) { painter.setPen(QColor(120,120,120)); if(this->m_nPressed) painter.setBrush(QColor(60,60,60)); else if(this->m_nHighlight) painter.setBrush(QColor(100,100,100)); else painter.setBrush(QColor(80,80,80)); } else { QLinearGradient outlinebrush(0,0,0,scaledRect.height()); QLinearGradient brush(0,0,0,scaledRect.height()); brush.setSpread(QLinearGradient::PadSpread); QColor highlight(255,255,255,128); QColor shadow(0,0,0,70); QColor sunken(220,220,220,30); QColor normal1(88,88,89,255); QColor normal2(88,88,89,255); QColor normal3(0,0,200,10); QColor normal4(255,255,250,255); if(m_nType==3 || m_nType == 4) { normal1 = QColor(100,180,189,55); normal2 = QColor(100,180,189,255); } if(m_nPressed) { outlinebrush.setColorAt(0.0f,shadow); outlinebrush.setColorAt(1.0f,highlight); brush.setColorAt(1.0f,sunken); painter.setPen(Qt::NoPen); } else { outlinebrush.setColorAt(0.75f,shadow); outlinebrush.setColorAt(0.0f,highlight); brush.setColorAt(0.0f,normal2); if(m_nHighlight) brush.setColorAt(1.0f,normal1); painter.setPen(QPen(outlinebrush,1)); } if(this->isEnabled()==false ) { outlinebrush.setColorAt(1.0f,shadow); outlinebrush.setColorAt(0.0f,highlight); brush.setColorAt(0.0f,normal3); painter.setPen(QPen(outlinebrush,2)); } if(m_nStatus ) { outlinebrush.setColorAt(1.0f,shadow); outlinebrush.setColorAt(0.0f,highlight); brush.setColorAt(0.0f,normal2); painter.setPen(QPen(outlinebrush,1)); } painter.setBrush(brush); } // painter.drawRect(0, 0, scaledRect.width(), scaledRect.height()); float xOff = scaledRect.width() / 2; float yOff = scaledRect.height() / 2; float sizex = 5.0f * matrix.m11(); float sizey = 3.5f * matrix.m22(); if (m_nType == 3) sizey *= -1; QPainterPath path; path.moveTo(xOff, yOff + (5 * sizey)); path.lineTo(xOff - (4 * sizex), yOff - (3 * sizey)); path.lineTo(xOff + (4 * sizex), yOff - (3 * sizey)); path.lineTo(xOff, yOff + (5 * sizey)); painter.drawPath(path); }
/// Normal (Gaussian) distribution probability distribution function. double normal_pdf(double x, double mean, double standard_deviation) { normal_dist normal1(mean, standard_deviation); return boost::math::pdf(normal1, x); }
void QmaxButton::createButton(QPainter &painter) { QRect scaledRect; scaledRect = matrix.mapRect(QRect(0,0,this->logicalSize.width(),this->logicalSize.height())); QImage bg(this->m_strImage); painter.setRenderHint(QPainter::SmoothPixmapTransform); painter.setRenderHint(QPainter::Antialiasing); painter.setPen(Qt::NoPen); QLinearGradient brush1(0,0,0,scaledRect.height()); painter.drawImage(-2, -2, bg); if(Colors::useEightBitPalette) { painter.setPen(QColor(120,120,120)); if(this->m_nPressed) painter.setBrush(QColor(60,60,60)); else if(this->m_nHighlight) painter.setBrush(QColor(100,100,100)); else painter.setBrush(QColor(80,80,80)); } else { QLinearGradient outlinebrush(0,0,0,scaledRect.height()); QLinearGradient brush(0,0,0,scaledRect.height()); brush.setSpread(QLinearGradient::PadSpread); QColor highlight(255,255,255,128); QColor shadow(0,0,0,70); QColor sunken(220,220,220,30); QColor normal1(255,255,245,60); QColor normal2(255,255,235,10); QColor normal3(200,200,200,10); QColor normal4(255,255,250,255); if(m_nType && m_nType != 5 ) { normal1 = QColor(200,170,160,50); normal2 = QColor(50,10,0,50); } if(m_nPressed) { outlinebrush.setColorAt(0.0f,shadow); outlinebrush.setColorAt(1.0f,highlight); brush.setColorAt(1.0f,sunken); painter.setPen(Qt::NoPen); } else { outlinebrush.setColorAt(1.0f,shadow); outlinebrush.setColorAt(0.0f,highlight); brush.setColorAt(0.0f,normal1); if(m_nHighlight) brush.setColorAt(1.0f,normal2); painter.setPen(QPen(outlinebrush,1)); } if(this->isEnabled()==false ) { outlinebrush.setColorAt(1.0f,shadow); outlinebrush.setColorAt(0.0f,highlight); brush.setColorAt(0.0f,normal3); painter.setPen(QPen(outlinebrush,1)); } if(m_nStatus ) { outlinebrush.setColorAt(1.0f,shadow); outlinebrush.setColorAt(0.0f,highlight); brush.setColorAt(0.0f,normal4); painter.setPen(QPen(outlinebrush,1)); } painter.setBrush(brush); } if(m_nType == 1) painter.drawRect(0,0,scaledRect.width(),scaledRect.height()); else if(m_nType == 0) painter.drawRoundedRect(0,0,scaledRect.width(),scaledRect.height(),40.0,40.0,Qt::RelativeSize); else if(m_nType == 5) painter.drawEllipse(0,0,scaledRect.width(),scaledRect.height()); QFont font( "DejaVu Sans" ); font.setPointSize( 12 ); painter.setFont( font ); brush1.setColorAt(1.0f,QColor(255,255,255,255)); if(this->isEnabled()==false) { brush1.setColorAt(1.0f,QColor(200,200,200,100)); } painter.setPen(QPen(brush1,1)); painter.setBrush(brush1); QFontMetrics fMetrics = painter.fontMetrics(); QSize sz = fMetrics.size( Qt::TextWordWrap, m_strText ); QRectF txtRect( scaledRect.center(), sz ); int xPoint = (scaledRect.width()/2)- ((m_strText.count()/2)*10); int yPoint = scaledRect.height()/2; painter.drawText(xPoint,yPoint,m_strText); }
void Collision::modify(Particle& particle,float deltaTime) const { size_t index = particle.getIndex(); float radius1 = particle.getParamCurrentValue(PARAM_SIZE) * scale * 0.5f; float m1 = particle.getParamCurrentValue(PARAM_MASS); Group& group = *particle.getGroup(); // Tests collisions with all the particles that are stored before in the pool for (size_t i = 0; i < index; ++i) { Particle& particle2 = group.getParticle(i); float radius2 = particle2.getParamCurrentValue(PARAM_SIZE) * scale * 0.5f; float sqrRadius = radius1 + radius2; sqrRadius *= sqrRadius; // Gets the normal of the collision plane vec3 normal = particle.position(); normal -= particle2.position(); // float sqrDist = normal.getSqrNorm(); float sqrDist = glm::length2(normal); if (sqrDist < sqrRadius) // particles are intersecting each other { vec3 delta = particle.velocity(); delta -= particle2.velocity(); if (dotProduct(normal,delta) < 0.0f) // particles are moving towards each other { float oldSqrDist = getSqrDist(particle.oldPosition(),particle2.oldPosition()); if (oldSqrDist > sqrDist) { // Disables the move from this frame particle.position() = particle.oldPosition(); particle2.position() = particle2.oldPosition(); normal = particle.position(); normal -= particle2.position(); if (dotProduct(normal,delta) >= 0.0f) continue; } // normal.normalize(); normal = glm::normalize(normal); // Gets the normal components of the velocities vec3 normal1(normal); vec3 normal2(normal); normal1 *= dotProduct(normal,particle.velocity()); normal2 *= dotProduct(normal,particle2.velocity()); // Resolves collision float m2 = particle2.getParamCurrentValue(PARAM_MASS); if (oldSqrDist < sqrRadius && sqrDist < sqrRadius) { // Tweak to separate particles that intersects at both t - deltaTime and t // In that case the collision is no more considered as punctual if (dotProduct(normal,normal1) < 0.0f) { particle.velocity() -= normal1; particle2.velocity() += normal1; } if (dotProduct(normal,normal2) > 0.0f) { particle2.velocity() -= normal2; particle.velocity() += normal2; } } else { // Else classic collision equations are applied // Tangent components of the velocities are left untouched particle.velocity() -= (1.0f + (elasticity * m2 - m1) / (m1 + m2)) * normal1; particle2.velocity() -= (1.0f + (elasticity * m1 - m2) / (m1 + m2)) * normal2; normal1 *= ((1.0f + elasticity) * m1) / (m1 + m2); normal2 *= ((1.0f + elasticity) * m2) / (m1 + m2); particle.velocity() += normal2; particle2.velocity() += normal1; } } } } }
dgFloat32 FastRayTest::PolygonIntersectSimd (const dgVector& normal, const dgFloat32* const polygon, dgInt32 strideInBytes, const dgInt32* const indexArray, dgInt32 indexCount) const { _ASSERTE (m_p0.m_w == m_p1.m_w); simd_128 normal1 ((simd_128&)normal & simd_128 (-1, -1, -1, 0)); simd_128 dist (((simd_128&)normal1).DotProduct((simd_128&)m_diff)); if ((dist < simd_128(m_dirError)).GetSignMask() & 1) { dgInt32 stride = dgInt32 (strideInBytes / sizeof (dgFloat32)); simd_128 v0 (&polygon[indexArray[indexCount - 1] * stride]); simd_128 p0v0 (v0 - (simd_128&)m_p0); simd_128 tOut = normal1.DotProduct(p0v0); simd_128 zero (dgFloat32 (0.0f)); // this only work for convex polygons and for single side faces // walk the polygon around the edges and calculate the volume simd_128 test ((tOut < zero) & (tOut > dist)); if (test.GetSignMask()) { dgInt32 i3 = indexCount - 1; dgInt32 i2 = indexCount - 2; dgInt32 i1 = indexCount - 3; dgInt32 i0 = (indexCount > 3) ? indexCount - 4 : 2; for (dgInt32 i4 = 0; i4 < indexCount; i4 += 4) { simd_128 v0 (&polygon[indexArray[i0] * stride]); simd_128 v1 (&polygon[indexArray[i1] * stride]); simd_128 v2 (&polygon[indexArray[i2] * stride]); simd_128 v3 (&polygon[indexArray[i3] * stride]); simd_128 v4 (&polygon[indexArray[i4] * stride]); simd_128 p0v0 (v0 - (simd_128&)m_p0); simd_128 p0v1 (v1 - (simd_128&)m_p0); simd_128 p0v2 (v2 - (simd_128&)m_p0); simd_128 p0v3 (v3 - (simd_128&)m_p0); simd_128 p0v4 (v4 - (simd_128&)m_p0); simd_128 p0v0_x; simd_128 p0v0_y; simd_128 p0v0_z; simd_128 p0v1_x; simd_128 p0v1_y; simd_128 p0v1_z; Transpose4x4Simd_128 (p0v0_x, p0v0_y, p0v0_z, test, p0v0, p0v1, p0v2, p0v3); Transpose4x4Simd_128 (p0v1_x, p0v1_y, p0v1_z, test, p0v1, p0v2, p0v3, p0v4); simd_128 volume = (m_ray_yyyy * p0v1_z - m_ray_zzzz * p0v1_y) * p0v0_x + (m_ray_zzzz * p0v1_x - m_ray_xxxx * p0v1_z) * p0v0_y + (m_ray_xxxx * p0v1_y - m_ray_yyyy * p0v1_x) * p0v0_z; // if a least one volume is negative it mean the line cross the polygon outside this edge and do not hit the face if ((volume < (simd_128&)m_tolerance).GetSignMask()) { return 1.2f; } i3 = i4 + 3; i2 = i4 + 2; i1 = i4 + 1; i0 = i4 + 0; } //the line is to the left of all the polygon edges, //then the intersection is the point we the line intersect the plane of the polygon tOut = tOut / dist; dgFloat32 ret; tOut.StoreScalar(&ret); _ASSERTE (ret >= dgFloat32 (0.0f)); _ASSERTE (ret <= dgFloat32 (1.0f)); return ret; } } return dgFloat32 (1.2f); }
void ToolButton::paintEvent( QPaintEvent * _pe ) { const bool active = isDown() || isChecked(); QPainter painter(this); painter.setRenderHint(QPainter::SmoothPixmapTransform); painter.setRenderHint(QPainter::Antialiasing); painter.setPen(Qt::NoPen); QLinearGradient outlinebrush(0, 0, 0, height()); QLinearGradient brush(0, 0, 0, height()); brush.setSpread(QLinearGradient::PadSpread); QColor highlight(255, 255, 255, 70); QColor shadow(0, 0, 0, 70); QColor sunken(220, 220, 220, 30); QColor normal1(255, 255, 245, 60); QColor normal2(255, 255, 235, 10); if( active ) { outlinebrush.setColorAt(0.0f, shadow); outlinebrush.setColorAt(1.0f, highlight); brush.setColorAt(0.0f, sunken); painter.setPen(Qt::NoPen); } else { outlinebrush.setColorAt(1.0f, shadow); outlinebrush.setColorAt(0.0f, highlight); brush.setColorAt(0.0f, normal1); if( m_mouseOver == false ) { brush.setColorAt(1.0f, normal2); } painter.setPen(QPen(outlinebrush, 1)); } painter.setBrush(brush); painter.drawRoundedRect( 0, 0, width(), height(), 5, 5 ); const int dd = active ? 1 : 0; QPoint pt = QPoint( ( width() - m_img.width() ) / 2 + dd, 3 + dd ); if( s_iconOnlyMode ) { pt.setY( ( height() - m_img.height() ) / 2 - 1 + dd ); } painter.drawImage( pt, m_img ); if( s_iconOnlyMode == false ) { const QString l = ( active && m_altLabel.isEmpty() == FALSE ) ? m_altLabel : m_label; const int w = painter.fontMetrics().width( l ); painter.setPen( Qt::black ); painter.drawText( ( width() - w ) / 2 +1+dd, height() - 4+dd, l ); painter.setPen( Qt::white ); painter.drawText( ( width() - w ) / 2 +dd, height() - 5+dd, l ); } }
QImage Button::createBackground() const { QImage image(rect().width(), rect().height(), QImage::Format_ARGB32_Premultiplied); image.fill(QColor(0, 0, 0, 0).rgba()); QPainter painter(&image); painter.setRenderHint(QPainter::SmoothPixmapTransform); painter.setRenderHint(QPainter::Antialiasing); painter.setPen(Qt::NoPen); QLinearGradient outlinebrush(0, 0, 0, rect().height()); QLinearGradient brush(0, 0, 0, rect().height()); brush.setSpread(QLinearGradient::PadSpread); QColor highlight(255, 255, 255, 70); QColor shadow(0, 0, 0, 70); QColor sunken(220, 220, 220, 30); QColor normal1(255, 255, 245, 60); QColor normal2(255, 255, 235, 10); if (dark) { normal1 = QColor(200, 170, 160, 50); normal2 = QColor(50, 10, 0, 50); } if (!isEnabled() || isDown()) { outlinebrush.setColorAt(0.0f, shadow); outlinebrush.setColorAt(1.0f, highlight); brush.setColorAt(0.0f, sunken); painter.setPen(Qt::NoPen); } else { outlinebrush.setColorAt(1.0f, shadow); outlinebrush.setColorAt(0.0f, highlight); brush.setColorAt(0.0f, normal1); if (!hover) brush.setColorAt(1.0f, normal2); painter.setPen(QPen(outlinebrush, 1)); } painter.setBrush(brush); if (round) painter.drawRoundedRect(rect(), 15, rect().height()); else painter.drawRect(0, 0, rect().width(), rect().height()); if (text() == "##Left##" || text() == "##Right##") { qreal xCenter = rect().width() * 0.5; qreal yCenter = rect().height() * 0.5; qreal xSize = 12; qreal ySize = 6; if (text() == "##Right##") xSize *= -1; QPainterPath path; path.moveTo(xCenter - xSize, yCenter); path.lineTo(xCenter + xSize, yCenter - ySize); path.lineTo(xCenter + xSize, yCenter + ySize); path.lineTo(xCenter - xSize, yCenter); painter.drawPath(path); } return image; }