void Camera::LookAt(glm::vec3 const & eye, glm::vec3 const & center)
{
m_scale = glm::vec3(1.0f);
m_position = eye;
glm::vec3 vdir = center-eye;
float rotY = (vdir.x == 0.0f && vdir.z == 0.0f) ? 0.0f : AngleBetween(glm::vec2(0.0f,-1.0f),glm::vec2(vdir.x, vdir.z)); vdir = glm::vec3(glm::rotateY(glm::vec4(vdir,1.0f),-rotY));
float rotX = (vdir.x == 0.0f && vdir.y == 0.0f) ? 0.0f : AngleBetween(glm::vec2(0.0f, 1.0f),glm::vec2(vdir.y, vdir.z));
float rotZ = 0.0f;
m_rotation = -glm::vec3(rotX,rotY,rotZ);
UpdateModelMatrix();
if (GetUpDir().y < 0) m_rotation += glm::vec3(180.0f,0,0);
}
Quaternion Quaternion::RotateTowards(Quaternion from, Quaternion to, float maxDegreesDelta)
{
float angle = AngleBetween(from, to);
if (angle == 0.0f)
{
return to;
}
float t = System::Math::Min(1.0f, maxDegreesDelta / angle);
return SlerpUnclamped(from, to, t);
}
double SignedPlaneDistance(const vgl_plane_3d<double> &Plane, const vgl_point_3d<double> &Point)
{
double d = vgl_distance(Plane, Point);
vgl_point_3d<double> ClosestPoint = vgl_closest_point(Plane, Point);
vgl_vector_3d<double> V = Point - ClosestPoint;
if(AngleBetween(Plane.normal(), V) < M_PI/2.0)
return d;
else
return -d;
}
/// Determine the angles in the list of lines in allLines
/// and sorts the lines of the same angle into the same vector
void StairDetection::SortLinesByAngle(std::vector<cv::Vec4i> &allLines, std::vector<std::vector<int>> &angles)
{
for (size_t i = 0; i < allLines.size(); i++) {
cv::Vec4i l = allLines.at(i);
int angle = AngleBetween(l[0], l[1], l[2], l[3]);
angle = GroupAngles(angle);
if (angle == -1)
continue;
angles[angle].push_back(i);
}
}
/// Using the found best fit line that represents the stairs,
/// find all lines that intersect with the fit line
/// all lines that intersect belong to the stairs
void StairDetection::GetStairPoints(std::vector<cv::Vec4i> &allLines, std::vector<cv::Point> &stairMidLine, int &stairsAngle, std::vector<cv::Point> &stairPoints, std::vector<cv::Point> &stairMidPoints)
{
cv::Point pt1 = stairMidLine[0];
cv::Point pt2 = stairMidLine[1];
for (cv::Vec4i vec : allLines) {
cv::Point l1(vec[0], vec[1]);
cv::Point l2(vec[2], vec[3]);
cv::Point r;
int theta = AngleBetween(l1.x, l1.y, l2.x, l2.y);
// if the line is within +- 10 degs, and it intersects the midline,
// then it belongs to the set of lines that represents the stairs.
if (abs(stairsAngle - theta) <= 10) {
if (intersection(l1, l2, pt1, pt2, r)) {
stairPoints.push_back(l1);
stairPoints.push_back(l2);
stairMidPoints.push_back(r);
}
}
}
}
/**
* GetSkyXYZRadiance:
* this function compute sky radiance according to a view parameters `theta' and `phi'and sunSky values
* parameters:
* sunSky, sontains sun and sky parameters
* theta, is sun's theta
* phi, is sun's phi
* color_out, is computed color that shows sky radiance in XYZ color format
* */
void GetSkyXYZRadiance(struct SunSky* sunsky, float theta, float phi, float color_out[3])
{
float gamma;
float x,y,Y,X,Z;
float hfade=1, nfade=1;
if (theta>(0.5f*(float)M_PI)) {
hfade = 1.0f-(theta*(float)M_1_PI-0.5f)*2.0f;
hfade = hfade*hfade*(3.0f-2.0f*hfade);
theta = 0.5*M_PI;
}
if (sunsky->theta>(0.5f*(float)M_PI)) {
if (theta<=0.5f*(float)M_PI) {
nfade = 1.0f-(0.5f-theta*(float)M_1_PI)*2.0f;
nfade *= 1.0f-(sunsky->theta*(float)M_1_PI-0.5f)*2.0f;
nfade = nfade*nfade*(3.0f-2.0f*nfade);
}
}
gamma = AngleBetween(theta, phi, sunsky->theta, sunsky->phi);
// Compute xyY values
x = PerezFunction(sunsky, sunsky->perez_x, theta, gamma, sunsky->zenith_x);
y = PerezFunction(sunsky, sunsky->perez_y, theta, gamma, sunsky->zenith_y);
Y = 6.666666667e-5f * nfade * hfade * PerezFunction(sunsky, sunsky->perez_Y, theta, gamma, sunsky->zenith_Y);
if(sunsky->sky_exposure!=0.0f)
Y = 1.0 - exp(Y*sunsky->sky_exposure);
X = (x / y) * Y;
Z = ((1 - x - y) / y) * Y;
color_out[0] = X;
color_out[1] = Y;
color_out[2] = Z;
}
static TPoint
ComputePupil(int num, TPoint mouse, const TRectangle* screen)
{
float cx, cy;
float dist;
float angle;
float dx, dy;
float cosa, sina;
TPoint ret;
cx = EYE_X(num); dx = mouse.x - cx;
cy = EYE_Y(num); dy = mouse.y - cy;
if (dx == 0 && dy == 0);
else {
angle = atan2 ((float) dy, (float) dx);
cosa = cos (angle);
sina = sin (angle);
dist = BALL_DIST;
if (screen)
{
/* use distance mapping */
float x0, y0, x1, y1;
float a[4];
x0 = screen->x - cx;
y0 = screen->y - cy;
x1 = x0 + screen->width;
y1 = y0 + screen->height;
a[0] = atan2(y0, x0);
a[1] = atan2(y1, x0);
a[2] = atan2(y1, x1);
a[3] = atan2(y0, x1);
if (AngleBetween(angle, a[0], a[1]))
{
/* left */
dist *= dx / x0;
}
else if (AngleBetween(angle, a[1], a[2]))
{
/* bottom */
dist *= dy / y1;
}
else if (AngleBetween(angle, a[2], a[3]))
{
/* right */
dist *= dx / x1;
}
else if (AngleBetween(angle, a[3], a[0]))
{
/* top */
dist *= dy / y0;
}
if (dist > BALL_DIST)
dist = BALL_DIST;
}
if (dist > hypot ((double) dx, (double) dy)) {
cx += dx;
cy += dy;
} else {
cx += dist * cosa;
cy += dist * sina;
}
}
ret.x = cx;
ret.y = -cy;
return ret;
}
void EdGizmo::OnMouseMove( const EdSceneViewport& viewport, const SMouseMoveEvent& args )
{
AHitProxy* pHitProxy = viewport.objAtCursor;
const bool bHighlightObjects = true;
if( bHighlightObjects )
{
if( pHitProxy != nil )
{
APlaceable* pPlaceable = pHitProxy->IsPlaceable();
m_hightlighted = pPlaceable;
HGizmoAxis* pHGizmoAxis = SafeCast<HGizmoAxis>( pHitProxy );
if( pHGizmoAxis != nil )
{
m_highlightedAxes = pHGizmoAxis->axis;
}
else
{
m_highlightedAxes = EGizmoAxis::GizmoAxis_None;
}
}
else
{
m_hightlighted = nil;
}
}
if( viewport.IsDraggingMouse() && m_selected != nil )
{
// gizmo pick point, in screen space
Vec2D pickPosNDC;
PointToNDC( viewport, viewport.dragStartPosition.x(), viewport.dragStartPosition.y(), pickPosNDC );
//DBGOUT("pickPosNDC: %f, %f\n",pickPosNDC.x,pickPosNDC.y);
Vec2D currPosNDC;
PointToNDC( viewport, args.mouseX, args.mouseY, currPosNDC );
//DBGOUT("currPosNDC: %f, %f\n",currPosNDC.x,currPosNDC.y);
// we need to convert screen space delta to world space movement
Vec2D deltaNDC = currPosNDC - pickPosNDC;
//dbgout << "delta=" << deltaNDC << dbgout.NewLine();
switch( m_currentMode )
{
case EGizmoMode::Gizmo_Translate :
if( m_currentAxis == GizmoAxis_None )
{
return;
}
if( m_currentAxis == GizmoAxis_All )
{
L_TranslateAll:
const rxView& eye = viewport.GetView();
Ray3D pickRay = GetEyeRay( viewport, currPosNDC );
FLOAT prevDist = (m_oldState.translation - eye.origin).LengthSqr();
if( prevDist < VECTOR_EPSILON )
{
return;
}
prevDist = mxSqrt(prevDist);
Vec3D newEntityPos = eye.origin + pickRay.direction * prevDist;
m_selected->SetOrigin( newEntityPos );
}
else
{
Vec3D gizmoPickPos = F_Get_Translation_Gizmo_Pick_Point_In_World_Space( viewport, pickPosNDC, m_selected, m_currentAxis );
//Vec3D centerOfGizmo = m_selected->GetOrigin();
Vec3D pointOnGizmo = F_Get_Translation_Gizmo_Pick_Point_In_World_Space( viewport, currPosNDC, m_selected, m_currentAxis );
Vec3D translationDelta = pointOnGizmo - gizmoPickPos;
const FLOAT MAX_TRANSLATION_DIST = 100.0f;
translationDelta.Clamp(Vec3D(-MAX_TRANSLATION_DIST),Vec3D(MAX_TRANSLATION_DIST));
Vec3D newEntityPos = m_oldState.translation + translationDelta;
m_selected->SetOrigin( newEntityPos );
}
break;
case EGizmoMode::Gizmo_Scale :
if( m_currentAxis == GizmoAxis_All )
{
goto L_TranslateAll;
}
else
{
//dbgout << "Scaling\n";
FLOAT mag = deltaNDC.LengthFast();
FLOAT newEntityScale = m_oldState.scaleFactor + mag * signf(deltaNDC.x);
newEntityScale = maxf(newEntityScale,0.01f);
m_selected->SetScale( newEntityScale );
}
break;
case EGizmoMode::Gizmo_Rotate :
//if( m_currentAxis == GizmoAxis_None )
//{
// return false;
//}
if( m_currentAxis == GizmoAxis_All )
{
goto L_TranslateAll;
}
else
{
//dbgout << "Rotating\n";
mxUNDONE;
static FLOAT ROT_ARC_RADIUS = 1.0f;
HOT_FLOAT(ROT_ARC_RADIUS);
// starting point of rotation arc
Vec3D vDownPt = ConvertScreenPointToVector(
viewport,
viewport.dragStartPosition.x(), viewport.dragStartPosition.y(),
ROT_ARC_RADIUS
);
// current point of rotation arc
Vec3D vCurrPt = ConvertScreenPointToVector(
viewport,
args.mouseX, args.mouseY,
ROT_ARC_RADIUS
);
#if 0
Quat qRot = QuatFromBallPoints( vDownPt, vCurrPt );
qRot.Normalize();
#else
Vec3D axis = Cross( vDownPt, vCurrPt );
axis.Normalize();
F4 angle = AngleBetween( vDownPt, vCurrPt );
Quat qRot( axis, angle );
qRot.Normalize();
#endif
Quat q = qRot * m_oldState.orientation;
q.Normalize();
m_selected->SetOrientation(q);
}
break;
mxNO_SWITCH_DEFAULT;
}
}
}
Export int PathFind_Vector(FPoint *start, FPoint *end, FLine *obstructions, int obstructioncount, FPoint *pathbuffer, int pathbuffersize, Image* drawbuffer, drawcbk* drawcallback) {
if (!start) return Failure;
if (!end) return Failure;
if (!obstructions) return Failure;
//if (!pathbuffer) return Failure;
//if (pathbuffersize < 2) return Failure;
if (obstructioncount < 1) {
pathbuffer[0] = *start;
pathbuffer[1] = *end;
return Trivial_Success;
}
treeNode *root = new treeNode(*start);
treeNode *tail = new treeNode(*end);
treeNode *current, *newnode;
root->pushLeft(tail);
FLine currentLine;
FPoint where, newpoint;
FPoint vector, newvector;
float vector_length, vector_angle;
int leaf = 0, closest_obstruction;
float closest_obstruction_distance;
stateStack stack;
stack.push_front(buildState(root, Null));
drawbuffer->clear();
drawTree(drawbuffer, root);
drawcallback();
while (stack.size() > 0) {
leaf = stack.front().leaf;
switch (leaf) {
case 0:
current = stack.front().node->left;
stack.front().leaf++;
break;
case 1:
current = stack.front().node->right;
stack.front().leaf++;
break;
case 2:
stack.pop_front();
continue;
break;
}
if (current) {
currentLine.Start = current->up->point;
currentLine.End = current->point;
closest_obstruction = -1;
closest_obstruction_distance = 999999;
for (int o = 0; o < obstructioncount; o++) {
bool skip = false;
if (currentLine.intersect(obstructions[o], where)) {
for (stateStack::iterator iter = stack.begin(); iter != stack.end(); iter++) {
if (iter->obstruction == &(obstructions[o])) {
skip = true;
break;
}
}
if (!skip) {
vector = FLine(current->up->point, where).vector();
if (vector.length() < closest_obstruction_distance) {
closest_obstruction = o;
closest_obstruction_distance = vector.length();
}
}
}
}
if (closest_obstruction > -1) {
if (stack.front().node->depth < max_tree_depth) {
vector = FLine(obstructions[closest_obstruction].Start, obstructions[closest_obstruction].End).vector();
vector_length = vector.length();
vector_angle = AngleBetween(obstructions[closest_obstruction].Start, obstructions[closest_obstruction].End);
vector.X = sin(vector_angle * Radian);
vector.Y = -cos(vector_angle * Radian);
newvector = vector;
newvector *= (-1);
newpoint = obstructions[closest_obstruction].Start;
newpoint += FPoint(newvector.X, newvector.Y);
newnode = new treeNode(newpoint);
newnode->pushLeft(new treeNode(*end));
current->up->pushLeft(newnode);
newvector = vector;
newvector *= (vector_length + 1);
newpoint = obstructions[closest_obstruction].Start;
newpoint += FPoint(newvector.X, newvector.Y);
newnode = new treeNode(newpoint);
newnode->pushLeft(new treeNode(*end));
current->up->pushRight(newnode);
if (leaf == 0) {
if (current->up->left)
stack.push_front(buildState(current->up->left, &(obstructions[closest_obstruction])));
} else {
if (current->up->right)
stack.push_front(buildState(current->up->right, &(obstructions[closest_obstruction])));
}
}
}
}
drawbuffer->clear();
drawTree(drawbuffer, root);
if (current)
drawbuffer->setPixelAA(current->point.X, current->point.Y, Pixel(0,0,255,255));
drawcallback();
}
return Success;
}
