inline float
interpolateAngle(float a, float b, float alpha)
{
float ca = clampAngle(a);
float cb = clampAngle(b);
if (fabs(ca - cb) > 180.f)
{
if (ca < cb)
ca += 360.f;
else
cb += 360.f;
}
return clampAngle(interpolate(ca, cb, alpha));
}
// virtual
bool ConeRollerConstraint::clamp(glm::quat& rotation) const {
bool applied = false;
glm::vec3 rotatedAxis = rotation * _coneAxis;
glm::vec3 perpAxis = glm::cross(rotatedAxis, _coneAxis);
float perpAxisLength = glm::length(perpAxis);
if (perpAxisLength > EPSILON) {
perpAxis /= perpAxisLength;
// enforce the cone
float angle = acosf(glm::dot(rotatedAxis, _coneAxis));
if (angle > _coneAngle) {
rotation = glm::angleAxis(angle - _coneAngle, perpAxis) * rotation;
rotatedAxis = rotation * _coneAxis;
applied = true;
}
} else {
// the rotation is 100% roll
// there is no obvious perp axis so we must pick one
perpAxis = rotatedAxis;
// find the first non-zero element:
float iValue = 0.0f;
int i = 0;
for (i = 0; i < 3; ++i) {
if (fabsf(perpAxis[i]) > EPSILON) {
iValue = perpAxis[i];
break;
}
}
assert(i != 3);
// swap or negate the next element
int j = (i + 1) % 3;
float jValue = perpAxis[j];
if (fabsf(jValue - iValue) > EPSILON) {
perpAxis[i] = jValue;
perpAxis[j] = iValue;
} else {
perpAxis[i] = -iValue;
}
perpAxis = glm::cross(perpAxis, rotatedAxis);
perpAxisLength = glm::length(perpAxis);
assert(perpAxisLength > EPSILON);
perpAxis /= perpAxisLength;
}
// measure the roll
// NOTE: perpAxis is perpendicular to both _coneAxis and rotatedConeAxis, so we can
// rotate it again and we'll end up with an something that has only been rolled.
glm::vec3 rolledPerpAxis = rotation * perpAxis;
float sign = glm::dot(rotatedAxis, glm::cross(perpAxis, rolledPerpAxis)) > 0.0f ? 1.0f : -1.0f;
float roll = sign * angleBetween(rolledPerpAxis, perpAxis);
if (roll < _minRoll || roll > _maxRoll) {
float clampedRoll = clampAngle(roll, _minRoll, _maxRoll);
rotation = glm::normalize(glm::angleAxis(clampedRoll - roll, rotatedAxis) * rotation);
applied = true;
}
return applied;
}
// virtual
bool HingeConstraint::clamp(glm::quat& rotation) const {
glm::vec3 forward = rotation * _forwardAxis;
forward -= glm::dot(forward, _rotationAxis) * _rotationAxis;
float length = glm::length(forward);
if (length < EPSILON) {
// infinite number of solutions ==> choose the middle of the contrained range
rotation = glm::angleAxis(0.5f * (_minAngle + _maxAngle), _rotationAxis);
return true;
}
forward /= length;
float sign = (glm::dot(glm::cross(_forwardAxis, forward), _rotationAxis) > 0.0f ? 1.0f : -1.0f);
//float angle = sign * acos(glm::dot(forward, _forwardAxis) / length);
float angle = sign * acosf(glm::dot(forward, _forwardAxis));
glm::quat newRotation = glm::angleAxis(clampAngle(angle, _minAngle, _maxAngle), _rotationAxis);
if (fabsf(1.0f - glm::dot(newRotation, rotation)) > EPSILON * EPSILON) {
rotation = newRotation;
return true;
}
return false;
}
void Sprite::setRotation(float rotation) {
this->rotation = clampAngle(rotation);
}
QPolygonF curvedArrow( QPointF po, QPointF pm, QPointF pd, qreal startWidth, qreal width, qreal headSize, QgsArrowSymbolLayer::HeadType headType, qreal offset )
{
qreal circleRadius;
QPointF circleCenter;
if ( ! pointsToCircle( po, pm, pd, circleCenter, circleRadius ) || circleRadius > 10000.0 )
{
// aligned points => draw a straight arrow
return straightArrow( po, pd, startWidth, width, headSize, headType, offset );
}
// angles of each point
qreal angle_o = clampAngle( atan2( circleCenter.y() - po.y(), po.x() - circleCenter.x() ) );
qreal angle_m = clampAngle( atan2( circleCenter.y() - pm.y(), pm.x() - circleCenter.x() ) );
qreal angle_d = clampAngle( atan2( circleCenter.y() - pd.y(), pd.x() - circleCenter.x() ) );
// arc direction : 1 = counter-clockwise, -1 = clockwise
int direction = clampAngle( angle_m - angle_o ) < clampAngle( angle_m - angle_d ) ? 1 : -1;
qreal deltaAngle = angle_d - angle_o;
if ( direction * deltaAngle < 0.0 )
deltaAngle = deltaAngle + direction * 2 * M_PI;
qreal length = euclidian_distance( po, pd );
// for close points and deltaAngle < 180, draw a straight line
if ( fabs( deltaAngle ) < M_PI && ((( headType == QgsArrowSymbolLayer::HeadSingle ) && ( length < headSize ) ) ||
(( headType == QgsArrowSymbolLayer::HeadReversed ) && ( length < headSize ) ) ||
(( headType == QgsArrowSymbolLayer::HeadDouble ) && ( length < 2*headSize ) ) ) )
{
return straightArrow( po, pd, startWidth, width, headSize, headType, offset );
}
// ajust coordinates to include offset
circleRadius += offset;
po = circlePoint( circleCenter, circleRadius, angle_o );
pm = circlePoint( circleCenter, circleRadius, angle_m );
pd = circlePoint( circleCenter, circleRadius, angle_d );
qreal headAngle = direction * atan( headSize / circleRadius );
QPainterPath path;
if ( headType == QgsArrowSymbolLayer::HeadDouble )
{
// the first head
path.moveTo( po );
path.lineTo( circlePoint( circleCenter, circleRadius + direction * headSize, angle_o + headAngle ) );
pathArcTo( path, circleCenter, circleRadius + direction * width / 2, angle_o + headAngle, angle_d - headAngle, direction );
// the second head
path.lineTo( circlePoint( circleCenter, circleRadius + direction * headSize, angle_d - headAngle ) );
path.lineTo( pd );
path.lineTo( circlePoint( circleCenter, circleRadius - direction * headSize, angle_d - headAngle ) );
pathArcTo( path, circleCenter, circleRadius - direction * width / 2, angle_d - headAngle, angle_o + headAngle, -direction );
// the end of the first head
path.lineTo( circlePoint( circleCenter, circleRadius - direction * headSize, angle_o + headAngle ) );
path.lineTo( po );
}
else if ( headType == QgsArrowSymbolLayer::HeadSingle )
{
path.moveTo( circlePoint( circleCenter, circleRadius + direction * startWidth / 2, angle_o ) );
spiralArcTo( path, circleCenter, angle_o, circleRadius + direction * startWidth / 2, angle_d - headAngle, circleRadius + direction * width / 2, direction );
// the arrow head
path.lineTo( circlePoint( circleCenter, circleRadius + direction * headSize, angle_d - headAngle ) );
path.lineTo( pd );
path.lineTo( circlePoint( circleCenter, circleRadius - direction * headSize, angle_d - headAngle ) );
spiralArcTo( path, circleCenter, angle_d - headAngle, circleRadius - direction * width / 2, angle_o, circleRadius - direction * startWidth / 2, -direction );
path.lineTo( circlePoint( circleCenter, circleRadius + direction * startWidth / 2, angle_o ) );
}
else if ( headType == QgsArrowSymbolLayer::HeadReversed )
{
path.moveTo( circlePoint( circleCenter, circleRadius + direction * width / 2, angle_o + headAngle ) );
spiralArcTo( path, circleCenter, angle_o + headAngle, circleRadius + direction * width / 2, angle_d, circleRadius + direction * startWidth / 2, direction );
path.lineTo( circlePoint( circleCenter, circleRadius - direction * startWidth / 2, angle_d ) );
spiralArcTo( path, circleCenter, angle_d, circleRadius - direction * startWidth / 2, angle_o + headAngle, circleRadius - direction * width / 2, - direction );
path.lineTo( circlePoint( circleCenter, circleRadius - direction * headSize, angle_o + headAngle ) );
path.lineTo( po );
path.lineTo( circlePoint( circleCenter, circleRadius + direction * headSize, angle_o + headAngle ) );
path.lineTo( circlePoint( circleCenter, circleRadius + direction * width / 2, angle_o + headAngle ) );
}
return path.toSubpathPolygons().at( 0 );
}
