void ewol::compositing::Drawing::lineTo(const vec3& _dest) {
resetCount();
internalSetColor(m_color);
//EWOL_VERBOSE("DrawLine : " << m_position << " to " << _dest);
if (m_position.x() == _dest.x() && m_position.y() == _dest.y() && m_position.z() == _dest.z()) {
//EWOL_WARNING("Try to draw a line width 0");
return;
}
//teta = tan-1(oposer/adjacent)
float teta = 0;
if (m_position.x() <= _dest.x()) {
teta = atan((_dest.y()-m_position.y())/(_dest.x()-m_position.x()));
} else {
teta = M_PI + atan((_dest.y()-m_position.y())/(_dest.x()-m_position.x()));
}
if (teta < 0) {
teta += 2*M_PI;
} else if (teta > 2*M_PI) {
teta -= 2*M_PI;
}
//EWOL_DEBUG("teta = " << (teta*180/(M_PI)) << " deg." );
float offsety = sin(teta-M_PI/2) * (m_thickness/2);
float offsetx = cos(teta-M_PI/2) * (m_thickness/2);
setPoint(vec3(m_position.x() - offsetx, m_position.y() - offsety, m_position.z()) );
setPoint(vec3(m_position.x() + offsetx, m_position.y() + offsety, m_position.z()) );
setPoint(vec3(_dest.x() + offsetx, _dest.y() + offsety, m_position.z()) );
setPoint(vec3(_dest.x() + offsetx, _dest.y() + offsety, _dest.z()) );
setPoint(vec3(_dest.x() - offsetx, _dest.y() - offsety, _dest.z()) );
setPoint(vec3(m_position.x() - offsetx, m_position.y() - offsety, _dest.z()) );
// update the system position :
m_position = _dest;
}
void DBaseBatchPlanner::writeRawToFile( int i, int j, const std::vector<RawScanPoint> &rawData,
vec3 loc, vec3 dir, vec3 up)
{
QString filename = QString(mScanDirectory) + mObject->getName() + QString("_raw");
QString n;
n.setNum(i);
filename = filename + n;
n.setNum(j);
filename = filename + QString("_") + n + QString(".txt");
FILE *f = fopen(filename.latin1(),"w");
if (!f) {
DBGAF(mLogStream,"Failed to open file " << filename.latin1());
fprintf(stderr,"Failed to open scan file\n");
return;
}
fprintf(f,"%f %f %f\n",loc.x(), loc.y(), loc.z());
fprintf(f,"%f %f %f\n",dir.x(), dir.y(), dir.z());
fprintf(f,"%f %f %f\n",up.x(), up.y(), up.z());
fprintf(f,"%d\n",(int)rawData.size());
for (int k=0; k<(int)rawData.size(); k++) {
fprintf(f,"%f %f ",rawData[k].hAngle, rawData[k].vAngle);
fprintf(f,"%f %f %f ",rawData[k].dx, rawData[k].dy, rawData[k].dz);
fprintf(f,"%f\n",rawData[k].distance);
}
writeSolutionsToFile(f);
fclose(f);
}
static void lookAt(const vec3<Type>& eye, const vec3<Type>& center, const vec3<Type>& up)
{
const vec3<Type> forward = (center - eye).normalized();
const vec3<Type> side = forward.cross(up).normalized();
const vec3<Type> upVector = side.cross(forward);
matrix4<Type> m;
m.m_data[0][0] = side.x();
m.m_data[1][0] = side.y();
m.m_data[2][0] = side.z();
m.m_data[3][0] = -side.dot(eye);
m.m_data[0][1] = upVector.x();
m.m_data[1][1] = upVector.y();
m.m_data[2][1] = upVector.z();
m.m_data[3][1] = -upVector.dot(eye);
m.m_data[0][2] = -forward.x();
m.m_data[1][2] = -forward.y();
m.m_data[2][2] = -forward.z();
m.m_data[3][2] = -forward.dot(eye);
m.m_data[0][3] = 0;
m.m_data[1][3] = 0;
m.m_data[2][3] = 0;
m.m_data[3][3] = 1;
return m;
}
/** \brief Expand the boundary.
*
* \param other Other boundary.
*/
void expand(const math::rect3<T> &other)
{
if(other.x1() < m_pnt_min.x())
{
m_pnt_min.x() = other.x1();
}
if(other.x2() < m_pnt_max.x())
{
m_pnt_max.x() = other.x2();
}
if(other.y1() < m_pnt_min.y())
{
m_pnt_min.y() = other.y1();
}
if(other.y2() < m_pnt_max.y())
{
m_pnt_max.y() = other.y2();
}
if(other.z1() < m_pnt_min.z())
{
m_pnt_min.z() = other.z1();
}
if(other.z2() < m_pnt_max.z())
{
m_pnt_max.z() = other.z2();
}
}
/** \brief Expand the boundary.
*
* Uses some manual checking to decrease comparison count instead of pure
* min/max.
*
* \param pnt Point to expand with.
*/
void expand(const math::vec3<T> &pnt)
{
if(pnt.x() > m_pnt_max.x())
{
m_pnt_max.x() = pnt.x();
}
else if(pnt.x() < m_pnt_min.x())
{
m_pnt_min.x() = pnt.x();
}
if(pnt.y() > m_pnt_max.y())
{
m_pnt_max.y() = pnt.y();
}
else if(pnt.y() < m_pnt_min.y())
{
m_pnt_min.y() = pnt.y();
}
if(pnt.z() > m_pnt_max.z())
{
m_pnt_max.z() = pnt.z();
}
else if(pnt.z() < m_pnt_min.z())
{
m_pnt_min.z() = pnt.z();
}
}
/*!
Creates a 6x6 matrix, \a transformMat, that transforms a wrench expressed
in one coordinate system to wrench expressed in another. \a T is the
transform, and \a p is the new torque origin expressed within the new
coordinate system.
*/
void buildForceTransform(transf &T,vec3 &p,double *transformMat)
{
static int j,k;
static double R[9];
static double crossMat[9];
static double Rcross[9];
static vec3 radius;
R[0] = T.affine().element(0,0);
R[1] = T.affine().element(0,1);
R[2] = T.affine().element(0,2);
R[3] = T.affine().element(1,0);
R[4] = T.affine().element(1,1);
R[5] = T.affine().element(1,2);
R[6] = T.affine().element(2,0);
R[7] = T.affine().element(2,1);
R[8] = T.affine().element(2,2);
/*
R[0] = T.affine().element(0,0);
R[1] = T.affine().element(1,0);
R[2] = T.affine().element(2,0);
R[3] = T.affine().element(0,1);
R[4] = T.affine().element(1,1);
R[5] = T.affine().element(2,1);
R[6] = T.affine().element(0,2);
R[7] = T.affine().element(1,2);
R[8] = T.affine().element(2,2);
*/
for (j=0;j<9;j++)
if (fabs(R[j]) < MACHINE_ZERO) R[j] = 0.0;
else if (R[j] > 1.0 - MACHINE_ZERO) R[j] = 1.0;
else if (R[j] < -1.0 + MACHINE_ZERO) R[j] = -1.0;
radius = T.translation() - p;
crossMat[0]=0.0; crossMat[3]=-radius.z();crossMat[6]=radius.y();
crossMat[1]=radius.z();crossMat[4]=0.0; crossMat[7]=-radius.x();
crossMat[2]=-radius.y();crossMat[5]= radius.x();crossMat[8]=0.0;
//original graspit
//dgemm("N","N",3,3,3,1.0,R,3,crossMat,3,0.0,Rcross,3);
// mtc: new version, I believe this is the correct one
dgemm("N","N",3,3,3,1.0,crossMat,3,R,3,0.0,Rcross,3);
fillMatrixBlock(R,3,0,0,2,2,transformMat,6);
for (j=3;j<6;j++)
for (k=0;k<3;k++)
transformMat[6*j+k] = 0.0;
fillMatrixBlock(Rcross,3,3,0,5,2,transformMat,6);
fillMatrixBlock(R,3,3,3,5,5,transformMat,6);
}
void System::minimumImageCriterion(vec3 &pos)
{
if (pos.x() > m_systemSize.x() * 0.5) pos[0] = pos[0] - m_systemSize.x();
else if (pos.x() <= -m_systemSize.x() * 0.5) pos[0] = pos[0] + m_systemSize.x();
if (pos.y() > m_systemSize.y() * 0.5) pos[1] = pos[1] - m_systemSize.y();
else if (pos.y() <= -m_systemSize.y() * 0.5) pos[1] = pos[1] + m_systemSize.y();
if (pos.z() > m_systemSize.z() * 0.5) pos[2] = pos[2] - m_systemSize.z();
else if (pos.z() <= -m_systemSize.z() * 0.5) pos[2] = pos[2] + m_systemSize.z();
}
/** \brief Check whether this rect is completely outside another.
*
* Note that rectangles touching on one side are not considered
* intersecting.
*
* \param other Other rectangle.
* \return True if no overlap of any kind, false otherwise.
*/
bool isOutside(const math::rect3<T> &other) const
{
return ((m_pnt_max.x() <= other.m_pnt_min.x()) ||
(m_pnt_min.x() >= other.m_pnt_max.x()) ||
(m_pnt_max.y() <= other.m_pnt_min.y()) ||
(m_pnt_min.y() >= other.m_pnt_max.y()) ||
(m_pnt_max.z() <= other.m_pnt_min.z()) ||
(m_pnt_min.z() >= other.m_pnt_max.z()));
}
RGBColor convertColor( vec3& clr){
int r,g,b;
clr.setX( std::max( std::min( clr.x(), 1.0), 0.0 ) );
clr.setY( std::max( std::min( clr.y(), 1.0), 0.0 ) );
clr.setZ( std::max( std::min( clr.z(), 1.0), 0.0 ) );
r=(int)(255*clr.x());
g=(int)(255*clr.y());
b=(int)(255*clr.z());
return RGBColor(r, g, b);
}
void TouchscreenButton::DebugRender(const vec3& position,
const vec3& texture_size,
fplbase::Renderer& renderer) const {
#if defined(DEBUG_RENDER_BOUNDS)
if (debug_shader_ && draw_bounds_) {
const vec2 window_size = vec2(renderer.window_size());
static const float kButtonZDepth = 0.0f;
static const fplbase::Attribute kFormat[] = {fplbase::kPosition3f,
fplbase::kEND
};
static const unsigned short kIndices[] = {0, 1, 1, 3, 2, 3, 2, 0};
const vec3 bottom_left = position - (texture_size / 2.0f);
const vec3 top_right = position + (texture_size / 2.0f);
// vertex format is [x, y, z]
float vertices[] = {
bottom_left.x(), bottom_left.y(), bottom_left.z(), top_right.x(),
bottom_left.y(), bottom_left.z(), bottom_left.x(), top_right.y(),
top_right.z(), top_right.x(), top_right.y(), top_right.z(),
};
renderer.set_color(vec4(1.0f, 0.0f, 1.0f, 1.0f));
debug_shader_->Set(renderer);
fplbase::Mesh::RenderArray(fplbase::Mesh::kLines, 8,
kFormat, sizeof(float) * 3,
reinterpret_cast<const char*>(vertices),
kIndices);
renderer.set_color(vec4(1.0f, 1.0f, 0.0f, 1.0f));
debug_shader_->Set(renderer);
static unsigned short indicesButtonDef[] = {1, 0, 1, 2, 2, 3, 3, 0};
float verticesButtonDef[] = {
button_def()->top_left()->x() * window_size.x(),
button_def()->top_left()->y() * window_size.y(),
kButtonZDepth,
button_def()->top_left()->x() * window_size.x(),
button_def()->bottom_right()->y() * window_size.y(),
kButtonZDepth,
button_def()->bottom_right()->x() * window_size.x(),
button_def()->bottom_right()->y() * window_size.y(),
kButtonZDepth,
button_def()->bottom_right()->x() * window_size.x(),
button_def()->top_left()->y() * window_size.y(),
kButtonZDepth,
};
fplbase::Mesh::RenderArray(fplbase::Mesh::kLines, 8, kFormat,
sizeof(float) * 3,
reinterpret_cast<const char*>(verticesButtonDef),
indicesButtonDef);
}
#else
(void)position;
(void)texture_size;
(void)renderer;
#endif // DEBUG_RENDER_BOUNDS
}
void boxSize(const position &p, vec3 &min, vec3 &max,
const vec3 &x, const vec3 &y, const vec3 &z, double tolerance)
{
vec3 d = p - position::ORIGIN;
double dx = d % x;
double dy = d % y;
double dz = d % z;
if ( dx + tolerance > max.x() ) max.x() = dx + tolerance;
if ( dy + tolerance > max.y() ) max.y() = dy + tolerance;
if ( dz + tolerance > max.z() ) max.z() = dz + tolerance;
if ( dx - tolerance < min.x() ) min.x() = dx - tolerance;
if ( dy - tolerance < min.y() ) min.y() = dy - tolerance;
if ( dz - tolerance < min.z() ) min.z() = dz - tolerance;
}
// Area of a triangle in 3d.
float area(vec3 &a, vec3 &b, vec3 &c) {
float xA = a.x(), yA = a.y(), zA = a.z();
float xB = b.x(), yB = b.y(), zB = b.z();
float xC = c.x(), yC = c.y(), zC = c.z();
float det0 = det3(xA, xB, xC,
yA, yB, yC,
1 , 1, 1);
float det1 = det3(yA, yB, yC,
zA, zB, zC,
1 , 1, 1);
float det2 = det3(zA, zB, zC,
xA, xB, xC,
1 , 1, 1);
return 0.5f * sqrtf( det0 * det0 + det1 * det1 + det2 * det2 );
}
virtual color value(float u, float v, vec3 const &P) const
{
vec3::type d = sin(scale * P.x()) * cos(scale * P.y()) * sin(scale * P.z());
if (d < 0)
return odd->value(u, v, P);
return even->value(u, v, P);
}
/** Updates the AABB to contain the given point. */
void addPoint(const vec3& p)
{
if (isNull())
{
mMax = p;
mMin = p;
return;
}
if ( mMax.x() < p.x() ) mMax.x() = p.x();
if ( mMax.y() < p.y() ) mMax.y() = p.y();
if ( mMax.z() < p.z() ) mMax.z() = p.z();
if ( mMin.x() > p.x() ) mMin.x() = p.x();
if ( mMin.y() > p.y() ) mMin.y() = p.y();
if ( mMin.z() > p.z() ) mMin.z() = p.z();
}
void HuboPlus::computeGroundReaction(const real m,
const vec3& comPos,
const vec3& comAccel,
const Transform3& footXform,
vec3* force,
vec3* torque) const {
assert(comAccel.z() == 0);
// COM in frame of foot
vec3 cp = footXform.transformInv(comPos);
// COM accel in frame of foot
vec3 ca = footXform.rotInv() * comAccel;
ca[2] += g; // tack on gravity to the Z component
// F = ma
vec3 f = m*ca;
if (force) { *force = f; }
if (torque) {
// moment arm for torque from foot
vec3 r = -cp;
*torque = vec3::cross(r, f);
}
}
static void getUV(float& u, float& v, const vec3& p)
{
float phi = asin(p.y());
float theta = atan(p.x() / p.z());
u = (theta + _MATH_PI1_2) / _MATH_PI;
v = (phi + _MATH_PI1_2) / _MATH_PI;
}
double ArizonaTest::binarySearch(const vec3 &direction, PQP_Model &volume, double startValue, double endValue)
{
//we assume that startValue is inside the model and endValue is outside the model
//we find the value in between that is exactly on the model
double currentValue = startValue;
double currentInterval = (endValue - startValue)/2.0;
PQP_REAL pt[3];
PQP_REAL R[3][3]={{1.0,0.0,0.0},{0.0,1.0,0.0},{0.0,0.0,1.0}};
PQP_REAL T[3]={0.0,0.0,0.0};
double closest_dist = 1.0e9, thresh = 0.0, distance;
PQP_REAL closest_pt[3], closest_normal[3];
bool success = false;
while (1) {
pt[0] = currentValue * direction.x();
pt[1] = currentValue * direction.y();
pt[2] = currentValue * direction.z();
distance = GetShortestDist(pt, &volume, R, T, closest_dist, closest_pt, closest_normal, thresh);
DBGP("disturbance: " << pt[0] << " " << pt[1] << " " << pt[2]);
DBGP("Closest point: " << closest_pt[0] << " " << closest_pt[1] << " " << closest_pt[2] << "; Distance is: " << distance);
DBGP("Current interval: " << currentInterval);
if(distance < EPSILON){
success = true;
break;
}
if(isOutside(&volume, pt)){
DBGP("outside" << std::endl);
if (currentValue <= startValue) {
DBGP("Error: startValue is outside of model!");
break;
}
currentValue -= currentInterval;
} else {
DBGP("inside" << std::endl);
if (currentValue >= endValue) {
DBGP("Error: endValue is inside model!");
break;
}
currentValue += currentInterval;
}
currentInterval /= 2;
if ( fabs(currentInterval) < 1.0e-10 ) {
DBGP("Max loops exceeded!");
break;
}
}
if (distance > 10e-5) {
DBGA("Binary search failed! distance is " << distance);
} else {
DBGP("Binary search success!");
}
return currentValue;
}
vec3 gamma_correct(vec3 const &v)
{
vec3::type p = 1.0f/2.2f;
return vec3(
powf(v.x(), p),
powf(v.y(), p),
powf(v.z(), p)
);
}
vec4 to_color(vec3 const& v)
{
return vec4(
(v.x() + 1.0f) / 2.0f,
(v.y() + 1.0f) / 2.0f,
(v.z() + 1.0f) / 2.0f,
1.0f
);
}
void AndroidCardboardController::UpdateOrientation() {
facing_.Update();
up_.Update();
#ifdef ANDROID_HMD
// Cardboard uses a different coordinate space than we use, so we have to
// remap the axes and swap the handedness before we can use the
// vectors as our facing/up vectors:
const fplbase::HeadMountedDisplayInput& head_mounted_display_input =
input_system_->head_mounted_display_input();
const vec3 hmd_forward = head_mounted_display_input.forward();
const vec3 forward = vec3(hmd_forward.x(), -hmd_forward.z(), hmd_forward.y());
const vec3 hmd_up = head_mounted_display_input.up();
const vec3 up = vec3(hmd_up.x(), -hmd_up.z(), hmd_up.y());
facing_.SetValue(forward);
up_.SetValue(up);
#endif // ANDROID_HMD
}
void quaternion::set_axis_angle(vec3 const& axis,float const angle)
{
float const sin_phi=std::sin(angle/2.0f);
float const cos_phi=std::cos(angle/2.0f);
vec3 const n_axis = cpe::normalized(axis);
x() = n_axis.x()*sin_phi;
y() = n_axis.y()*sin_phi;
z() = n_axis.z()*sin_phi;
w() = cos_phi;
}
void Mesh::RenderAAQuadAlongX(const vec3 &bottom_left, const vec3 &top_right,
const vec2 &tex_bottom_left,
const vec2 &tex_top_right) {
static const Attribute format[] = {kPosition3f, kTexCoord2f, kEND};
static const unsigned short indices[] = {0, 1, 2, 1, 3, 2};
// clang-format off
// vertex format is [x, y, z] [u, v]:
const float vertices[] = {
bottom_left.x(), bottom_left.y(), bottom_left.z(),
tex_bottom_left.x(), tex_bottom_left.y(),
bottom_left.x(), top_right.y(), top_right.z(),
tex_bottom_left.x(), tex_top_right.y(),
top_right.x(), bottom_left.y(), bottom_left.z(),
tex_top_right.x(), tex_bottom_left.y(),
top_right.x(), top_right.y(), top_right.z(),
tex_top_right.x(), tex_top_right.y()};
// clang-format on
Mesh::RenderArray(kTriangles, 6, format, sizeof(float) * 5,
reinterpret_cast<const char *>(vertices), indices);
}
void ewol::compositing::Image::setClipping(const vec3& _pos, vec3 _posEnd) {
// note the internal system all time request to have a bounding all time in the same order
if (_pos.x() <= _posEnd.x()) {
m_clippingPosStart.setX(_pos.x());
m_clippingPosStop.setX(_posEnd.x());
} else {
m_clippingPosStart.setX(_posEnd.x());
m_clippingPosStop.setX(_pos.x());
}
if (_pos.y() <= _posEnd.y()) {
m_clippingPosStart.setY(_pos.y());
m_clippingPosStop.setY(_posEnd.y());
} else {
m_clippingPosStart.setY(_posEnd.y());
m_clippingPosStop.setY(_pos.y());
}
if (_pos.z() <= _posEnd.z()) {
m_clippingPosStart.setZ(_pos.z());
m_clippingPosStop.setZ(_posEnd.z());
} else {
m_clippingPosStart.setZ(_posEnd.z());
m_clippingPosStop.setZ(_pos.z());
}
m_clippingEnable = true;
}
void mesh_basic::transform_apply_auto_scale_and_center()
{
vec3 corner_min,corner_max;
compute_mesh_aabb_extremities(corner_min,corner_max);
vec3 const center=(corner_min+corner_max)/2;
vec3 const d=corner_max-corner_min;
transform_apply_translation(-center);
float const s=std::max(std::max(d.x(),d.y()),d.z());
if(s>1e-6f)
transform_apply_scale(1.0f/s);
}
//!
// Parse vec3 values from an OBJ file.
//
// @param source the source line to parse
// @param v the vec3 to populate
//
static void
obj_get_values(const string& source, vec3& v)
{
// Skip the definition type...
string::size_type endPos = source.find(" ");
string::size_type startPos(0);
if (endPos == string::npos)
{
Log::error("Bad element '%s'\n", source.c_str());
return;
}
// Find the first value...
startPos = endPos + 1;
endPos = source.find(" ", startPos);
if (endPos == string::npos)
{
Log::error("Bad element '%s'\n", source.c_str());
return;
}
string::size_type numChars(endPos - startPos);
string xs(source, startPos, numChars);
float x = Util::fromString<float>(xs);
// Then the second value...
startPos = endPos + 1;
endPos = source.find(" ", startPos);
if (endPos == string::npos)
{
Log::error("Bad element '%s'\n", source.c_str());
return;
}
numChars = endPos - startPos;
string ys(source, startPos, numChars);
float y = Util::fromString<float>(ys);
// And the third value (there might be a fourth, but we don't care)...
startPos = endPos + 1;
endPos = source.find(" ", startPos);
if (endPos == string::npos)
{
numChars = endPos;
}
else
{
numChars = endPos - startPos;
}
string zs(source, startPos, endPos - startPos);
float z = Util::fromString<float>(zs);
v.x(x);
v.y(y);
v.z(z);
}
void Mesh::RenderAAQuadAlongXNinePatch(const vec3 &bottom_left,
const vec3 &top_right,
const vec2i &texture_size,
const vec4 &patch_info) {
static const Attribute format[] = {kPosition3f, kTexCoord2f, kEND};
static const unsigned short indices[] = {
0, 2, 1, 1, 2, 3, 2, 4, 3, 3, 4, 5, 4, 6, 5, 5, 6, 7,
1, 3, 8, 8, 3, 9, 3, 5, 9, 9, 5, 10, 5, 7, 10, 10, 7, 11,
8, 9, 12, 12, 9, 13, 9, 10, 13, 13, 10, 14, 10, 11, 14, 14, 11, 15,
};
auto max = vec2::Max(bottom_left.xy(), top_right.xy());
auto min = vec2::Min(bottom_left.xy(), top_right.xy());
auto p0 = vec2(texture_size) * patch_info.xy() + min;
auto p1 = max - vec2(texture_size) * (mathfu::kOnes2f - patch_info.zw());
// Check if the 9 patch edges are not overwrapping.
// In that case, adjust 9 patch geometry locations not to overwrap.
if (p0.x() > p1.x()) {
p0.x() = p1.x() = (min.x() + max.x()) / 2;
}
if (p0.y() > p1.y()) {
p0.y() = p1.y() = (min.y() + max.y()) / 2;
}
// vertex format is [x, y, z] [u, v]:
float z = bottom_left.z();
// clang-format off
const float vertices[] = {
min.x(), min.y(), z, 0.0f, 0.0f,
p0.x(), min.y(), z, patch_info.x(), 0.0f,
min.x(), p0.y(), z, 0.0f, patch_info.y(),
p0.x(), p0.y(), z, patch_info.x(), patch_info.y(),
min.x(), p1.y(), z, 0.0, patch_info.w(),
p0.x(), p1.y(), z, patch_info.x(), patch_info.w(),
min.x(), max.y(), z, 0.0, 1.0,
p0.x(), max.y(), z, patch_info.x(), 1.0,
p1.x(), min.y(), z, patch_info.z(), 0.0f,
p1.x(), p0.y(), z, patch_info.z(), patch_info.y(),
p1.x(), p1.y(), z, patch_info.z(), patch_info.w(),
p1.x(), max.y(), z, patch_info.z(), 1.0f,
max.x(), min.y(), z, 1.0f, 0.0f,
max.x(), p0.y(), z, 1.0f, patch_info.y(),
max.x(), p1.y(), z, 1.0f, patch_info.w(),
max.x(), max.y(), z, 1.0f, 1.0f,
};
// clang-format on
Mesh::RenderArray(kTriangles, 6 * 9, format, sizeof(float) * 5,
reinterpret_cast<const char *>(vertices), indices);
}
/*! Given two line segments, P1-P2 and P3-P4, returns the line segment
Pa-Pb that is the shortest route between them. Calculates also the
values of \a mua and \a mub where
Pa = P1 + mua (P2 - P1)
Pb = P3 + mub (P4 - P3)
Returns FALSE if no solution exists.
adapted from http://astronomy.swin.edu.au/~pbourke/geometry/lineline3d/
*/
int LineLineIntersect(vec3 p1,vec3 p2,vec3 p3,vec3 p4,vec3 *pa,vec3 *pb,double *mua, double *mub)
{
vec3 p13,p43,p21;
double d1343,d4321,d1321,d4343,d2121;
double numer,denom;
double EPS = 1.0e-5;
p13.x() = p1.x() - p3.x();
p13.y() = p1.y() - p3.y();
p13.z() = p1.z() - p3.z();
p43.x() = p4.x() - p3.x();
p43.y() = p4.y() - p3.y();
p43.z() = p4.z() - p3.z();
if ( fabs(p43.x()) < EPS && fabs(p43.y()) < EPS && fabs(p43.z()) < EPS )
return false;
p21.x() = p2.x() - p1.x();
p21.y() = p2.y() - p1.y();
p21.z() = p2.z() - p1.z();
if ( fabs(p21.x()) < EPS && fabs(p21.y()) < EPS && fabs(p21.z()) < EPS )
return false;
d1343 = p13.x() * p43.x() + p13.y() * p43.y() + p13.z() * p43.z();
d4321 = p43.x() * p21.x() + p43.y() * p21.y() + p43.z() * p21.z();
d1321 = p13.x() * p21.x() + p13.y() * p21.y() + p13.z() * p21.z();
d4343 = p43.x() * p43.x() + p43.y() * p43.y() + p43.z() * p43.z();
d2121 = p21.x() * p21.x() + p21.y() * p21.y() + p21.z() * p21.z();
denom = d2121 * d4343 - d4321 * d4321;
if ( fabs(denom) < EPS )
return false;
numer = d1343 * d4321 - d1321 * d4343;
*mua = numer / denom;
*mub = (d1343 + d4321 * (*mua)) / d4343;
pa->x() = p1.x() + (*mua) * p21.x();
pa->y() = p1.y() + (*mua) * p21.y();
pa->z() = p1.z() + (*mua) * p21.z();
pb->x() = p3.x() + (*mub) * p43.x();
pb->y() = p3.y() + (*mub) * p43.y();
pb->z() = p3.z() + (*mub) * p43.z();
return true;
}
/*!
Sets this quaternion by converting the rotation from angle-axis formate.
The angle is specified in radians.
*/
void
Quaternion::set(const double& angle, const vec3& axis)
{
// assert: axis[] is unit length
//
// The quaternion representing the rotation is
// q = cos(A/2)+sin(A/2)*(x*i+y*j+z*k)
double halfAngle = 0.5*angle;
double sn = sin(halfAngle);
w = cos(halfAngle);
x = sn*axis.x();
y = sn*axis.y();
z = sn*axis.z();
normalise();
}
bool Skeleton::setBoneRotationsAngles(const unsigned int& idx, const vec3& angles)
{
if(idx >= mBones.size())
{
return false;
}
// rotation around x
anax rotX(angles.x(), vec3::UnitX()); // pitch
anax rotY(angles.y(), vec3::UnitY()); // yaw
anax rotZ(angles.z(), vec3::UnitZ()); // roll
quat q = rotX * rotY * rotZ;
mBones[idx].R = q.matrix();
std::vector<REAL> len;
getBoneLengths(len);
fitToBoneLengths(len);
return true;
}
vec3 flipy(const vec3& v) {
return vec3(v.x(), -v.y(), v.z());
}
