void btRigidBody::debugLocalProperties(btIDebugDraw* glDebugDrawer){ btTransform chassisTrans = getCenterOfMassTransform(); btVector3 sideVector ( chassisTrans.getBasis()[0][0], chassisTrans.getBasis()[1][0], chassisTrans.getBasis()[2][0]); btVector3 fwdVector ( chassisTrans.getBasis()[0][2], chassisTrans.getBasis()[1][2], chassisTrans.getBasis()[2][2]); if(idDebug == 1){ for(int i = 0; i < 4;i++){ btVector3 angLinearVelocity = debugAngularVelocity[i].cross(debugRelVector[i]); btVector3 relativePosition = debugRelVector[i] + getCenterOfMassPosition(); btVector3 toRelativePosition = relativePosition+angLinearVelocity*2; //glDebugDrawer->drawLine(relativePosition,toRelativePosition,btVector3(0.2,0.7,0.5)); //angLinearVelocity = debugAngularVelocity.cross(4.9*-sideVector); relativePosition = getCenterOfMassPosition()-sideVector*6; toRelativePosition = relativePosition+angLinearVelocity*1000; //glDebugDrawer->drawLine(relativePosition,toRelativePosition,btVector3(0.2,0.7,0.5)); relativePosition = getCenterOfMassPosition()+fwdVector*4+btVector3(0,3,0); toRelativePosition = relativePosition+debugLinearVelocity*1000; //glDebugDrawer->drawLine(relativePosition,toRelativePosition,btVector3(0.2,0.7,0.5)); } } }
Entity get_side_entity_for_element_side_pair(BulkData &bulkData, const SideSetEntry &facet) { const Entity * sides = bulkData.begin(facet.element, bulkData.mesh_meta_data().side_rank()); ConnectivityOrdinal const * ordinals = bulkData.begin_ordinals(facet.element, bulkData.mesh_meta_data().side_rank()); std::vector<Entity> sideVector(sides, sides+bulkData.num_sides(facet.element)); return get_side_entity_from_ordinal(sideVector, ordinals, facet.side); }
bool GeometryUtilities::placeRectAtLine(const QRectF &rect, const QLineF &line, double lineOffset, double distance, const QLineF &intersectionLine, QPointF *placement, Side *horizontalAlignedSide) { QMT_CHECK(placement); QList<Candidate> candidates; candidates << Candidate(QVector2D(rect.topRight() - rect.topLeft()), QPointF(0.0, 0.0), SideTop) << Candidate(QVector2D(rect.topLeft() - rect.topRight()), rect.topRight() - rect.topLeft(), SideTop) << Candidate(QVector2D(rect.bottomLeft() - rect.topLeft()), QPointF(0.0, 0.0), SideLeft) << Candidate(QVector2D(rect.topLeft() - rect.bottomLeft()), rect.bottomLeft() - rect.topLeft(), SideLeft) << Candidate(QVector2D(rect.bottomRight() - rect.bottomLeft()), rect.bottomLeft() - rect.topLeft(), SideBottom) << Candidate(QVector2D(rect.bottomLeft() - rect.bottomRight()), rect.bottomRight() - rect.topLeft(), SideBottom) << Candidate(QVector2D(rect.bottomRight() - rect.topRight()), rect.topRight() - rect.topLeft(), SideRight) << Candidate(QVector2D(rect.topRight() - rect.bottomRight()), rect.bottomRight() - rect.topLeft(), SideRight); QVector<QVector2D> rectEdgeVectors; rectEdgeVectors << QVector2D(rect.topLeft() - rect.topLeft()) << QVector2D(rect.topRight() - rect.topLeft()) << QVector2D(rect.bottomLeft() - rect.topLeft()) << QVector2D(rect.bottomRight() -rect.topLeft()); QVector2D directionVector(line.p2() - line.p1()); directionVector.normalize(); QVector2D sideVector(directionVector.y(), -directionVector.x()); QVector2D intersectionVector(intersectionLine.p2() - intersectionLine.p1()); intersectionVector.normalize(); QVector2D outsideVector = QVector2D(intersectionVector.y(), -intersectionVector.x()); double p = QVector2D::dotProduct(directionVector, outsideVector); if (p < 0.0) outsideVector = outsideVector * -1.0; double smallestA = -1.0; QPointF rectTranslation; Side side = SideUnspecified; int bestSign = 0; foreach (const Candidate &candidate, candidates) { // solve equation a * directionVector + candidate.first = b * intersectionVector to find smallest a double r = directionVector.x() * intersectionVector.y() - directionVector.y() * intersectionVector.x(); if (r <= -1e-5 || r >= 1e-5) { double a = (candidate.first.y() * intersectionVector.x() - candidate.first.x() * intersectionVector.y()) / r; if (a >= 0.0 && (smallestA < 0.0 || a < smallestA)) { // verify that all rectangle edges lay outside of shape (by checking for positiv projection to intersection) bool ok = true; int sign = 0; QVector2D rectOriginVector = directionVector * a - QVector2D(candidate.second); foreach (const QVector2D &rectEdgeVector, rectEdgeVectors) { QVector2D edgeVector = rectOriginVector + rectEdgeVector; double aa = QVector2D::dotProduct(outsideVector, edgeVector); if (aa < 0.0) { ok = false; break; } int s = sgn(QVector2D::dotProduct(sideVector, edgeVector)); if (s) { if (sign) { if (s != sign) { ok = false; break; } } else { sign = s; } } }