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;
}
}
}
