Matrix4f Arm::rodrigues(Vector3f rot) {
Vector3f R; R << rot(0), rot(1), rot(2);
if(R.norm() > 0.0f) {
R.normalize();
}
Matrix3f identity;
identity << Matrix3f::Identity();
Matrix3f crossProd(3,3);
crossProd(0,0) = 0.0f; crossProd(0, 1) = -(R(0)); crossProd(0,2) = R(1);
crossProd(1,0) = R(2); crossProd(1, 1) = 0.0; crossProd(1,2) = -(R(0));
crossProd(2,0) = -(R(1)); crossProd(2, 1) = R(0); crossProd(2,2) = 0.0;
Matrix3f crossProd_squ(3,3);
crossProd_squ = crossProd * crossProd;
float theta = rot.norm();
MatrixXf result = identity + sin(theta) * crossProd + (1-cos(theta))*crossProd_squ;
result.conservativeResize(4,4);
result(0, 3) = 0.0f; result(1, 3) = 0.0f; result(2,3) = 0.0f; result(3,3) = 1.0f;
result(3, 0) = 0.0f; result(3, 1) = 0.0f; result(3,2) = 0.0f;
return result;
}
bool KoConnectionShapePrivate::intersects(const QPointF &p1, const QPointF &d1, const QPointF &p2, const QPointF &d2, QPointF &isect)
{
qreal sp1 = scalarProd(d1, p2 - p1);
if (sp1 < 0.0)
return false;
qreal sp2 = scalarProd(d2, p1 - p2);
if (sp2 < 0.0)
return false;
// use cross product to check if rays intersects at all
qreal cp = crossProd(d1, d2);
if (cp == 0.0) {
// rays are parallel or coincidient
if (p1.x() == p2.x() && d1.x() == 0.0 && d1.y() != d2.y()) {
// vertical, coincident
isect = 0.5 * (p1 + p2);
} else if (p1.y() == p2.y() && d1.y() == 0.0 && d1.x() != d2.x()) {
// horizontal, coincident
isect = 0.5 * (p1 + p2);
} else {
return false;
}
} else {
// they are intersecting normally
isect = p1 + sp1 * d1;
}
return true;
}
Matrix4 LookAt(const Real3& pos, const Real3& target, const Real3& up) {
Matrix4 ret(Identity4());
Real3 z=normalize(target-pos);
Real3 x=normalize(crossProd(z,up));
Real3 y=crossProd(x,z);
// camera rotation
SetRow(ret,0,x);
SetRow(ret,1,y);
SetRow(ret,2,(-1.f)*z);
// camera translation
Real3 tr = Matrix4RotateReal3(ret,(-1)*pos);
SetCol(ret,3,tr);
return ret;
}
/**
* Add a quad to the mesh with clockwise front face vertex order
*/
void addQuad(ofVec3f const & bottomLeft, ofVec3f const & topLeft, ofVec3f const & topRight, ofVec3f const & bottomRight) {
mesh.enableNormals();
vbo.enableNormals();
ofVec3f v1 = topLeft;
ofVec3f v2 = topRight;
ofVec3f v3 = bottomRight;
ofVec3f v4 = bottomLeft;
ofVec3f v1N, v2N, v3N, v4N;
v1N = crossProd(v1,v2,v3);
v3N = crossProd(v3,v4,v1);
v2N = crossProd(v2,v1,v4);
v4N = crossProd(v4,v3,v2);
mesh.addVertex(v4);
mesh.addNormal(v4N);
mesh.addVertex(v1);
mesh.addNormal(v1N);
mesh.addVertex(v2);
mesh.addNormal(v2N);
mesh.addVertex(v3);
mesh.addNormal(v3N);
}
int main(void){
/*Dot prod test*/
int32_t a[LENGTH] = {1, 3, -5};
int32_t b[LENGTH] = {4, -2, -1};
printf("%d\n", dotProd(a, b, LENGTH));
/*cross prod test*/
vector x = {2, 1, -1};
vector y = {-3, 4, 1};
vector c = crossProd(&x, &y);
printVector(&c);
}
void initModel(Object& obj, float data[][3], unsigned indices[][3], unsigned vsize, unsigned fsize,
const Color3& color,real scale) {
obj.colors_.reserve(vsize);
for (unsigned i=0;i<vsize;i++) {
obj.colors_.push_back(color);
}
obj.tvx_.resize(vsize);
obj.vx_.reserve(vsize);
for (unsigned i=0;i<vsize;i++) {
obj.vx_.push_back(scale*Real3(data[i][0],data[i][1],data[i][2]));
}
obj.faces_.reserve(fsize);
for (unsigned i=0;i<fsize;++i) {
obj.faces_.push_back(Int3(indices[i][0],indices[i][1],indices[i][2]));
}
obj.tnormals_.resize(vsize);
obj.normals_.reserve(vsize);
for (unsigned i = 0; i < vsize; i++) {
obj.normals_[i]+=Real3(0.);
}
for (unsigned i = 0; i < fsize; i++) {
unsigned ia=indices[i][0];
unsigned ib=indices[i][1];
unsigned ic=indices[i][2];
Real3 a = obj.vx_[ia];
Real3 b = obj.vx_[ib];
Real3 c = obj.vx_[ic];
Real3 n = normalize( crossProd( b-a, c-a) );
obj.normals_[ia]+=n;
obj.normals_[ib]+=n;
obj.normals_[ic]+=n;
}
for (unsigned i = 0; i < vsize; i++) {
obj.normals_[i]=normalize( obj.normals_[i] );
}
}
QPointF KoConnectionShapePrivate::escapeDirection(int handleId) const
{
Q_Q(const KoConnectionShape);
QPointF direction;
if (handleConnected(handleId)) {
KoShape *attachedShape = handleId == KoConnectionShape::StartHandle ? shape1 : shape2;
int connectionPointId = handleId == KoConnectionShape::StartHandle ? connectionPointId1 : connectionPointId2;
KoConnectionPoint::EscapeDirection ed = attachedShape->connectionPoint(connectionPointId).escapeDirection;
if (ed == KoConnectionPoint::AllDirections) {
QPointF handlePoint = q->shapeToDocument(handles[handleId]);
QPointF centerPoint = attachedShape->absolutePosition(KoFlake::CenteredPosition);
/*
* Determine the best escape direction from the position of the handle point
* and the position and orientation of the attached shape.
* The idea is to define 4 sectors, one for each edge of the attached shape.
* Each sector starts at the center point of the attached shape and has it
* left and right edge going through the two points which define the edge.
* Then we check which sector contains our handle point, for which we can
* simply calculate the corresponding direction which is orthogonal to the
* corresponding bounding box edge.
* From that we derive the escape direction from looking at the main coordinate
* of the orthogonal direction.
*/
// define our edge points in the right order
const KoFlake::Position corners[4] = {
KoFlake::BottomRightCorner,
KoFlake::BottomLeftCorner,
KoFlake::TopLeftCorner,
KoFlake::TopRightCorner
};
QPointF vHandle = handlePoint-centerPoint;
for (int i = 0; i < 4; ++i) {
// first point of bounding box edge
QPointF p1 = attachedShape->absolutePosition(corners[i]);
// second point of bounding box edge
QPointF p2 = attachedShape->absolutePosition(corners[(i+1)%4]);
// check on which side of the first sector edge our second sector edge is
const qreal c0 = crossProd(p1-centerPoint, p2-centerPoint);
// check on which side of the first sector edge our handle point is
const qreal c1 = crossProd(p1-centerPoint, vHandle);
// second egde and handle point must be on the same side of first edge
if ((c0 < 0 && c1 > 0) || (c0 > 0 && c1 < 0))
continue;
// check on which side of the handle point our second sector edge is
const qreal c2 = crossProd(vHandle, p2-centerPoint);
// second edge must be on the same side of the handle point as on first edge
if ((c0 < 0 && c2 > 0) || (c0 > 0 && c2 < 0))
continue;
// now we found the correct edge
QPointF vDir = 0.5 *(p1+p2) - centerPoint;
// look at coordinate with the greatest absolute value
// and construct our escape direction accordingly
const qreal xabs = qAbs<qreal>(vDir.x());
const qreal yabs = qAbs<qreal>(vDir.y());
if (xabs > yabs) {
direction.rx() = vDir.x() > 0 ? 1.0 : -1.0;
direction.ry() = 0.0;
} else {
direction.rx() = 0.0;
direction.ry() = vDir.y() > 0 ? 1.0 : -1.0;
}
break;
}
} else if (ed == KoConnectionPoint::HorizontalDirections) {
QPointF handlePoint = q->shapeToDocument(handles[handleId]);
QPointF centerPoint = attachedShape->absolutePosition(KoFlake::CenteredPosition);
// use horizontal direction pointing away from center point
if (handlePoint.x() < centerPoint.x())
direction = QPointF(-1.0, 0.0);
else
direction = QPointF(1.0, 0.0);
} else if (ed == KoConnectionPoint::VerticalDirections) {
QPointF handlePoint = q->shapeToDocument(handles[handleId]);
QPointF centerPoint = attachedShape->absolutePosition(KoFlake::CenteredPosition);
// use vertical direction pointing away from center point
if (handlePoint.y() < centerPoint.y())
direction = QPointF(0.0, -1.0);
else
direction = QPointF(0.0, 1.0);
} else if (ed == KoConnectionPoint::LeftDirection) {
direction = QPointF(-1.0, 0.0);
} else if (ed == KoConnectionPoint::RightDirection) {
direction = QPointF(1.0, 0.0);
} else if (ed == KoConnectionPoint::UpDirection) {
direction = QPointF(0.0, -1.0);
} else if (ed == KoConnectionPoint::DownDirection) {
direction = QPointF(0.0, 1.0);
}
// transform escape direction by using our own transformation matrix
QTransform invMatrix = q->absoluteTransformation(0).inverted();
direction = invMatrix.map(direction) - invMatrix.map(QPointF());
direction /= sqrt(direction.x() * direction.x() + direction.y() * direction.y());
}
return direction;
}
void cogJacobianRightFootDes(int left_leg)
{
int i;
double x_com_i[N_CART+1], sumTmp[N_CART+1], vecTmp[N_CART+1];
double sumMassNum, sumMassDenum;
// precalculate total mass:
sumMassDenum = 0.0;
if (left_leg)
{
for (i = BASE; i <= N_DOFS; ++i)
sumMassDenum += links[i].m;
}
else
{
for (i = BASE; i < L_HFE; ++i)
sumMassDenum += links[i].m;
for (i = L_AAA+1; i <= N_DOFS; ++i)
sumMassDenum += links[i].m;
}
// Right arm!
sumTmp[1] = sumTmp[2] = sumTmp[3] = sumMassNum = 0;
for (i = R_WAA; i >= R_SFE; i--)
{
// moving mass
sumTmp[1] += joint_cog_mpos_des[i][_X_];
sumTmp[2] += joint_cog_mpos_des[i][_Y_];
sumTmp[3] += joint_cog_mpos_des[i][_Z_];
sumMassNum += links[i].m;
x_com_i[1] = sumTmp[1] / sumMassNum - joint_origin_pos_des[i][_X_];
x_com_i[2] = sumTmp[2] / sumMassNum - joint_origin_pos_des[i][_Y_];
x_com_i[3] = sumTmp[3] / sumMassNum - joint_origin_pos_des[i][_Z_];
crossProd(joint_axis_pos_des[i], x_com_i, vecTmp);
Jcogdes[1][i] = sumMassNum / sumMassDenum * vecTmp[1];
Jcogdes[2][i] = sumMassNum / sumMassDenum * vecTmp[2];
Jcogdes[3][i] = sumMassNum / sumMassDenum * vecTmp[3];
Jcogdes[4][i] = joint_axis_pos_des[i][_X_];
Jcogdes[5][i] = joint_axis_pos_des[i][_Y_];
Jcogdes[6][i] = joint_axis_pos_des[i][_Z_];
}
// Left arm!
sumTmp[1] = sumTmp[2] = sumTmp[3] = sumMassNum = 0;
for (i = L_WAA; i >= L_SFE; i--)
{
// moving mass
sumTmp[1] += joint_cog_mpos_des[i][_X_];
sumTmp[2] += joint_cog_mpos_des[i][_Y_];
sumTmp[3] += joint_cog_mpos_des[i][_Z_];
sumMassNum += links[i].m;
x_com_i[1] = sumTmp[1] / sumMassNum - joint_origin_pos_des[i][_X_];
x_com_i[2] = sumTmp[2] / sumMassNum - joint_origin_pos_des[i][_Y_];
x_com_i[3] = sumTmp[3] / sumMassNum - joint_origin_pos_des[i][_Z_];
crossProd(joint_axis_pos_des[i], x_com_i, vecTmp);
Jcogdes[1][i] = sumMassNum / sumMassDenum * vecTmp[1];
Jcogdes[2][i] = sumMassNum / sumMassDenum * vecTmp[2];
Jcogdes[3][i] = sumMassNum / sumMassDenum * vecTmp[3];
Jcogdes[4][i] = joint_axis_pos_des[i][_X_];
Jcogdes[5][i] = joint_axis_pos_des[i][_Y_];
Jcogdes[6][i] = joint_axis_pos_des[i][_Z_];
}
// Torso!
sumTmp[1] = sumTmp[2] = sumTmp[3] = sumMassNum = 0;
// arms mass:
for (i = L_SFE; i <= R_WAA; ++i)
{
sumTmp[1] += joint_cog_mpos_des[i][_X_];
sumTmp[2] += joint_cog_mpos_des[i][_Y_];
sumTmp[3] += joint_cog_mpos_des[i][_Z_];
sumMassNum += links[i].m;
}
// head + eyes mass:
for (i = B_HN; i <= L_ET; ++i)
{
sumTmp[1] += joint_cog_mpos_des[i][_X_];
sumTmp[2] += joint_cog_mpos_des[i][_Y_];
sumTmp[3] += joint_cog_mpos_des[i][_Z_];
sumMassNum += links[i].m;
}
for (i = B_TFE; i >= B_TR; i--)
{
// moving mass
sumTmp[1] += joint_cog_mpos_des[i][_X_];
sumTmp[2] += joint_cog_mpos_des[i][_Y_];
sumTmp[3] += joint_cog_mpos_des[i][_Z_];
sumMassNum += links[i].m;
x_com_i[1] = sumTmp[1] / sumMassNum - joint_origin_pos_des[i][_X_];
x_com_i[2] = sumTmp[2] / sumMassNum - joint_origin_pos_des[i][_Y_];
x_com_i[3] = sumTmp[3] / sumMassNum - joint_origin_pos_des[i][_Z_];
crossProd(joint_axis_pos_des[i], x_com_i, vecTmp);
Jcogdes[1][i] = sumMassNum / sumMassDenum * vecTmp[1];
Jcogdes[2][i] = sumMassNum / sumMassDenum * vecTmp[2];
Jcogdes[3][i] = sumMassNum / sumMassDenum * vecTmp[3];
Jcogdes[4][i] = joint_axis_pos_des[i][_X_];
Jcogdes[5][i] = joint_axis_pos_des[i][_Y_];
Jcogdes[6][i] = joint_axis_pos_des[i][_Z_];
}
// Head!
sumTmp[1] = sumTmp[2] = sumTmp[3] = sumMassNum = 0;
// eye mass
for (i = R_EP; i <= L_ET; ++i)
{
sumTmp[1] += joint_cog_mpos_des[i][_X_];
sumTmp[2] += joint_cog_mpos_des[i][_Y_];
sumTmp[3] += joint_cog_mpos_des[i][_Z_];
sumMassNum += links[i].m;
}
for (i = B_HR; i >= B_HN; i--)
{
// moving mass:
sumTmp[1] += joint_cog_mpos_des[i][_X_];
sumTmp[2] += joint_cog_mpos_des[i][_Y_];
sumTmp[3] += joint_cog_mpos_des[i][_Z_];
sumMassNum += links[i].m;
x_com_i[1] = sumTmp[1] / sumMassNum - joint_origin_pos_des[i][_X_];
x_com_i[2] = sumTmp[2] / sumMassNum - joint_origin_pos_des[i][_Y_];
x_com_i[3] = sumTmp[3] / sumMassNum - joint_origin_pos_des[i][_Z_];
crossProd(joint_axis_pos_des[i], x_com_i, vecTmp);
Jcogdes[1][i] = sumMassNum / sumMassDenum * vecTmp[1];
Jcogdes[2][i] = sumMassNum / sumMassDenum * vecTmp[2];
Jcogdes[3][i] = sumMassNum / sumMassDenum * vecTmp[3];
Jcogdes[4][i] = joint_axis_pos_des[i][_X_];
Jcogdes[5][i] = joint_axis_pos_des[i][_Y_];
Jcogdes[6][i] = joint_axis_pos_des[i][_Z_];
}
// Right eye
sumTmp[1] = sumTmp[2] = sumTmp[3] = sumMassNum = 0;
for (i = R_ET; i >= R_EP; i--)
{
// moving mass:
sumTmp[1] += joint_cog_mpos_des[i][_X_];
sumTmp[2] += joint_cog_mpos_des[i][_Y_];
sumTmp[3] += joint_cog_mpos_des[i][_Z_];
sumMassNum += links[i].m;
x_com_i[1] = sumTmp[1] / sumMassNum - joint_origin_pos_des[i][_X_];
x_com_i[2] = sumTmp[2] / sumMassNum - joint_origin_pos_des[i][_Y_];
x_com_i[3] = sumTmp[3] / sumMassNum - joint_origin_pos_des[i][_Z_];
crossProd(joint_axis_pos_des[i], x_com_i, vecTmp);
Jcogdes[1][i] = sumMassNum / sumMassDenum * vecTmp[1];
Jcogdes[2][i] = sumMassNum / sumMassDenum * vecTmp[2];
Jcogdes[3][i] = sumMassNum / sumMassDenum * vecTmp[3];
Jcogdes[4][i] = joint_axis_pos_des[i][_X_];
Jcogdes[5][i] = joint_axis_pos_des[i][_Y_];
Jcogdes[6][i] = joint_axis_pos_des[i][_Z_];
}
// Left eye
sumTmp[1] = sumTmp[2] = sumTmp[3] = sumMassNum = 0;
for (i = L_ET; i >= L_EP; i--)
{
// moving mass:
sumTmp[1] += joint_cog_mpos_des[i][_X_];
sumTmp[2] += joint_cog_mpos_des[i][_Y_];
sumTmp[3] += joint_cog_mpos_des[i][_Z_];
sumMassNum += links[i].m;
x_com_i[1] = sumTmp[1] / sumMassNum - joint_origin_pos_des[i][_X_];
x_com_i[2] = sumTmp[2] / sumMassNum - joint_origin_pos_des[i][_Y_];
x_com_i[3] = sumTmp[3] / sumMassNum - joint_origin_pos_des[i][_Z_];
crossProd(joint_axis_pos_des[i], x_com_i, vecTmp);
Jcogdes[1][i] = sumMassNum / sumMassDenum * vecTmp[1];
Jcogdes[2][i] = sumMassNum / sumMassDenum * vecTmp[2];
Jcogdes[3][i] = sumMassNum / sumMassDenum * vecTmp[3];
Jcogdes[4][i] = joint_axis_pos_des[i][_X_];
Jcogdes[5][i] = joint_axis_pos_des[i][_Y_];
Jcogdes[6][i] = joint_axis_pos_des[i][_Z_];
}
// Right leg!
sumTmp[1] = sumTmp[2] = sumTmp[3] = sumMassNum = 0;
// arms mass:
for (i = L_SFE; i <= R_WAA; ++i)
{
sumTmp[1] += joint_cog_mpos_des[i][_X_];
sumTmp[2] += joint_cog_mpos_des[i][_Y_];
sumTmp[3] += joint_cog_mpos_des[i][_Z_];
sumMassNum += links[i].m;
}
// torso + head + eyes mass:
for (i = B_TR; i <= L_ET; ++i)
{
sumTmp[1] += joint_cog_mpos_des[i][_X_];
sumTmp[2] += joint_cog_mpos_des[i][_Y_];
sumTmp[3] += joint_cog_mpos_des[i][_Z_];
sumMassNum += links[i].m;
}
if (left_leg)
{
// left leg mass:
for (i = L_HFE; i <= L_AAA; ++i)
{
sumTmp[1] += joint_cog_mpos_des[i][_X_];
sumTmp[2] += joint_cog_mpos_des[i][_Y_];
sumTmp[3] += joint_cog_mpos_des[i][_Z_];
sumMassNum += links[i].m;
}
}
for (i = R_HFE; i <= R_AAA; i++)
{
// moving mass:
if (i != R_HFE)
{
sumTmp[1] += joint_cog_mpos_des[i-1][_X_];
sumTmp[2] += joint_cog_mpos_des[i-1][_Y_];
sumTmp[3] += joint_cog_mpos_des[i-1][_Z_];
sumMassNum += links[i-1].m;
}
else
{
// Base mass
sumTmp[1] += joint_cog_mpos_des[BASE][_X_];
sumTmp[2] += joint_cog_mpos_des[BASE][_Y_];
sumTmp[3] += joint_cog_mpos_des[BASE][_Z_];
sumMassNum += links[BASE].m;
}
x_com_i[1] = sumTmp[1] / sumMassNum - joint_origin_pos_des[i][_X_];
x_com_i[2] = sumTmp[2] / sumMassNum - joint_origin_pos_des[i][_Y_];
x_com_i[3] = sumTmp[3] / sumMassNum - joint_origin_pos_des[i][_Z_];
crossProd(joint_axis_pos_des[i], x_com_i, vecTmp);
Jcogdes[1][i] = minus * sumMassNum / sumMassDenum * vecTmp[1];
Jcogdes[2][i] = minus * sumMassNum / sumMassDenum * vecTmp[2];
Jcogdes[3][i] = minus * sumMassNum / sumMassDenum * vecTmp[3];
Jcogdes[4][i] = joint_axis_pos_des[i][_X_];
Jcogdes[5][i] = joint_axis_pos_des[i][_Y_];
Jcogdes[6][i] = joint_axis_pos_des[i][_Z_];
}
// Left leg!
sumTmp[1] = sumTmp[2] = sumTmp[3] = sumMassNum = 0;
for (i = L_AAA; i >= L_HFE; i--)
{
sumTmp[1] += joint_cog_mpos_des[i][_X_];
sumTmp[2] += joint_cog_mpos_des[i][_Y_];
sumTmp[3] += joint_cog_mpos_des[i][_Z_];
sumMassNum += links[i].m;
x_com_i[1] = sumTmp[1] / sumMassNum - joint_origin_pos_des[i][_X_];
x_com_i[2] = sumTmp[2] / sumMassNum - joint_origin_pos_des[i][_Y_];
x_com_i[3] = sumTmp[3] / sumMassNum - joint_origin_pos_des[i][_Z_];
crossProd(joint_axis_pos_des[i], x_com_i, vecTmp);
if (left_leg)
{
Jcogdes[1][i] = sumMassNum/sumMassDenum * vecTmp[1];
Jcogdes[2][i] = sumMassNum/sumMassDenum * vecTmp[2];
Jcogdes[3][i] = sumMassNum/sumMassDenum * vecTmp[3];
Jcogdes[4][i] = joint_axis_pos_des[i][_X_];
Jcogdes[5][i] = joint_axis_pos_des[i][_Y_];
Jcogdes[6][i] = joint_axis_pos_des[i][_Z_];
}
else
{
Jcogdes[1][i] = 0.0;
Jcogdes[2][i] = 0.0;
Jcogdes[3][i] = 0.0;
Jcogdes[4][i] = 0.0;
Jcogdes[5][i] = 0.0;
Jcogdes[6][i] = 0.0;
}
}
}
void cogJacobianDes(void)
{
int i, j;
double x_com_i[N_CART+1], sumTmp[N_CART+1], vecTmp[N_CART+1];
double sumMassNum, sumMassDenum;
// precalculate total mass:
sumMassDenum = 0.0;
for (i = BASE; i <= N_DOFS; ++i)
sumMassDenum += links[i].m;
// Right arm!
sumTmp[1] = sumTmp[2] = sumTmp[3] = sumMassNum = 0;
for (i = R_WAA; i >= R_SFE; i--)
{
// moving mass
sumTmp[1] += joint_cog_mpos_des[i][_X_];
sumTmp[2] += joint_cog_mpos_des[i][_Y_];
sumTmp[3] += joint_cog_mpos_des[i][_Z_];
sumMassNum += links[i].m;
x_com_i[1] = sumTmp[1] / sumMassNum - joint_origin_pos_des[i][_X_];
x_com_i[2] = sumTmp[2] / sumMassNum - joint_origin_pos_des[i][_Y_];
x_com_i[3] = sumTmp[3] / sumMassNum - joint_origin_pos_des[i][_Z_];
crossProd(joint_axis_pos_des[i], x_com_i, vecTmp);
Jcogdes[1][i] = sumMassNum / sumMassDenum * vecTmp[1];
Jcogdes[2][i] = sumMassNum / sumMassDenum * vecTmp[2];
Jcogdes[3][i] = sumMassNum / sumMassDenum * vecTmp[3];
Jcogdes[4][i] = joint_axis_pos_des[i][_X_];
Jcogdes[5][i] = joint_axis_pos_des[i][_Y_];
Jcogdes[6][i] = joint_axis_pos_des[i][_Z_];
}
// Left arm!
sumTmp[1] = sumTmp[2] = sumTmp[3] = sumMassNum = 0;
for (i = L_WAA; i >= L_SFE; i--)
{
// moving mass
sumTmp[1] += joint_cog_mpos_des[i][_X_];
sumTmp[2] += joint_cog_mpos_des[i][_Y_];
sumTmp[3] += joint_cog_mpos_des[i][_Z_];
sumMassNum += links[i].m;
x_com_i[1] = sumTmp[1] / sumMassNum - joint_origin_pos_des[i][_X_];
x_com_i[2] = sumTmp[2] / sumMassNum - joint_origin_pos_des[i][_Y_];
x_com_i[3] = sumTmp[3] / sumMassNum - joint_origin_pos_des[i][_Z_];
crossProd(joint_axis_pos_des[i], x_com_i, vecTmp);
Jcogdes[1][i] = sumMassNum / sumMassDenum * vecTmp[1];
Jcogdes[2][i] = sumMassNum / sumMassDenum * vecTmp[2];
Jcogdes[3][i] = sumMassNum / sumMassDenum * vecTmp[3];
Jcogdes[4][i] = joint_axis_pos_des[i][_X_];
Jcogdes[5][i] = joint_axis_pos_des[i][_Y_];
Jcogdes[6][i] = joint_axis_pos_des[i][_Z_];
}
// Torso!
sumTmp[1] = sumTmp[2] = sumTmp[3] = sumMassNum = 0;
// arms mass:
for (i = L_SFE; i <= R_WAA; ++i)
{
sumTmp[1] += joint_cog_mpos_des[i][_X_];
sumTmp[2] += joint_cog_mpos_des[i][_Y_];
sumTmp[3] += joint_cog_mpos_des[i][_Z_];
sumMassNum += links[i].m;
}
// head + eyes mass:
for (i = B_HN; i <= L_ET; ++i)
{
sumTmp[1] += joint_cog_mpos_des[i][_X_];
sumTmp[2] += joint_cog_mpos_des[i][_Y_];
sumTmp[3] += joint_cog_mpos_des[i][_Z_];
sumMassNum += links[i].m;
}
for (i = B_TFE; i >= B_TR; i--)
{
// moving mass
sumTmp[1] += joint_cog_mpos_des[i][_X_];
sumTmp[2] += joint_cog_mpos_des[i][_Y_];
sumTmp[3] += joint_cog_mpos_des[i][_Z_];
sumMassNum += links[i].m;
x_com_i[1] = sumTmp[1] / sumMassNum - joint_origin_pos_des[i][_X_];
x_com_i[2] = sumTmp[2] / sumMassNum - joint_origin_pos_des[i][_Y_];
x_com_i[3] = sumTmp[3] / sumMassNum - joint_origin_pos_des[i][_Z_];
crossProd(joint_axis_pos_des[i], x_com_i, vecTmp);
Jcogdes[1][i] = sumMassNum / sumMassDenum * vecTmp[1];
Jcogdes[2][i] = sumMassNum / sumMassDenum * vecTmp[2];
Jcogdes[3][i] = sumMassNum / sumMassDenum * vecTmp[3];
Jcogdes[4][i] = joint_axis_pos_des[i][_X_];
Jcogdes[5][i] = joint_axis_pos_des[i][_Y_];
Jcogdes[6][i] = joint_axis_pos_des[i][_Z_];
}
// Right leg!
sumTmp[1] = sumTmp[2] = sumTmp[3] = sumMassNum = 0;
for (i = R_AAA; i >= R_HFE; i--)
{
// moving mass:
sumTmp[1] += joint_cog_mpos_des[i][_X_];
sumTmp[2] += joint_cog_mpos_des[i][_Y_];
sumTmp[3] += joint_cog_mpos_des[i][_Z_];
sumMassNum += links[i].m;
x_com_i[1] = sumTmp[1] / sumMassNum - joint_origin_pos_des[i][_X_];
x_com_i[2] = sumTmp[2] / sumMassNum - joint_origin_pos_des[i][_Y_];
x_com_i[3] = sumTmp[3] / sumMassNum - joint_origin_pos_des[i][_Z_];
crossProd(joint_axis_pos_des[i], x_com_i, vecTmp);
Jcogdes[1][i] = sumMassNum / sumMassDenum * vecTmp[1];
Jcogdes[2][i] = sumMassNum / sumMassDenum * vecTmp[2];
Jcogdes[3][i] = sumMassNum / sumMassDenum * vecTmp[3];
Jcogdes[4][i] = joint_axis_pos_des[i][_X_];
Jcogdes[5][i] = joint_axis_pos_des[i][_Y_];
Jcogdes[6][i] = joint_axis_pos_des[i][_Z_];
}
// Left leg!
sumTmp[1] = sumTmp[2] = sumTmp[3] = sumMassNum = 0;
for (i = L_AAA; i >= L_HFE; i--)
{
sumTmp[1] += joint_cog_mpos_des[i][_X_];
sumTmp[2] += joint_cog_mpos_des[i][_Y_];
sumTmp[3] += joint_cog_mpos_des[i][_Z_];
sumMassNum += links[i].m;
x_com_i[1] = sumTmp[1] / sumMassNum - joint_origin_pos_des[i][_X_];
x_com_i[2] = sumTmp[2] / sumMassNum - joint_origin_pos_des[i][_Y_];
x_com_i[3] = sumTmp[3] / sumMassNum - joint_origin_pos_des[i][_Z_];
crossProd(joint_axis_pos_des[i], x_com_i, vecTmp);
Jcogdes[1][i]= sumMassNum/sumMassDenum * vecTmp[1];
Jcogdes[2][i]= sumMassNum/sumMassDenum * vecTmp[2];
Jcogdes[3][i]= sumMassNum/sumMassDenum * vecTmp[3];
Jcogdes[4][i]= joint_axis_pos_des[i][_X_];
Jcogdes[5][i]= joint_axis_pos_des[i][_Y_];
Jcogdes[6][i]= joint_axis_pos_des[i][_Z_];
}
// Head!
sumTmp[1] = sumTmp[2] = sumTmp[3] = sumMassNum = 0;
// eye mass
for (i = R_EP; i <= L_ET; ++i)
{
sumTmp[1] += joint_cog_mpos_des[i][_X_];
sumTmp[2] += joint_cog_mpos_des[i][_Y_];
sumTmp[3] += joint_cog_mpos_des[i][_Z_];
sumMassNum += links[i].m;
}
for (i = B_HR; i >= B_HN; i--)
{
// moving mass:
sumTmp[1] += joint_cog_mpos_des[i][_X_];
sumTmp[2] += joint_cog_mpos_des[i][_Y_];
sumTmp[3] += joint_cog_mpos_des[i][_Z_];
sumMassNum += links[i].m;
x_com_i[1] = sumTmp[1] / sumMassNum - joint_origin_pos_des[i][_X_];
x_com_i[2] = sumTmp[2] / sumMassNum - joint_origin_pos_des[i][_Y_];
x_com_i[3] = sumTmp[3] / sumMassNum - joint_origin_pos_des[i][_Z_];
crossProd(joint_axis_pos_des[i], x_com_i, vecTmp);
Jcogdes[1][i] = sumMassNum/sumMassDenum * vecTmp[1];
Jcogdes[2][i] = sumMassNum/sumMassDenum * vecTmp[2];
Jcogdes[3][i] = sumMassNum/sumMassDenum * vecTmp[3];
Jcogdes[4][i] = joint_axis_pos_des[i][_X_];
Jcogdes[5][i] = joint_axis_pos_des[i][_Y_];
Jcogdes[6][i] = joint_axis_pos_des[i][_Z_];
}
// Right eye
sumTmp[1] = sumTmp[2] = sumTmp[3] = sumMassNum = 0;
for (i = R_ET; i >= R_EP; i--)
{
// moving mass:
sumTmp[1] += joint_cog_mpos_des[i][_X_];
sumTmp[2] += joint_cog_mpos_des[i][_Y_];
sumTmp[3] += joint_cog_mpos_des[i][_Z_];
sumMassNum += links[i].m;
x_com_i[1] = sumTmp[1] / sumMassNum - joint_origin_pos_des[i][_X_];
x_com_i[2] = sumTmp[2] / sumMassNum - joint_origin_pos_des[i][_Y_];
x_com_i[3] = sumTmp[3] / sumMassNum - joint_origin_pos_des[i][_Z_];
crossProd(joint_axis_pos_des[i], x_com_i, vecTmp);
Jcogdes[1][i] = sumMassNum / sumMassDenum * vecTmp[1];
Jcogdes[2][i] = sumMassNum / sumMassDenum * vecTmp[2];
Jcogdes[3][i] = sumMassNum / sumMassDenum * vecTmp[3];
Jcogdes[4][i] = joint_axis_pos_des[i][_X_];
Jcogdes[5][i] = joint_axis_pos_des[i][_Y_];
Jcogdes[6][i] = joint_axis_pos_des[i][_Z_];
}
// Left eye
sumTmp[1] = sumTmp[2] = sumTmp[3] = sumMassNum = 0;
for (i = L_ET; i >= L_EP; i--)
{
// moving mass:
sumTmp[1] += joint_cog_mpos_des[i][_X_];
sumTmp[2] += joint_cog_mpos_des[i][_Y_];
sumTmp[3] += joint_cog_mpos_des[i][_Z_];
sumMassNum += links[i].m;
x_com_i[1] = sumTmp[1] / sumMassNum - joint_origin_pos_des[i][_X_];
x_com_i[2] = sumTmp[2] / sumMassNum - joint_origin_pos_des[i][_Y_];
x_com_i[3] = sumTmp[3] / sumMassNum - joint_origin_pos_des[i][_Z_];
crossProd(joint_axis_pos_des[i], x_com_i, vecTmp);
Jcogdes[1][i] = sumMassNum/sumMassDenum * vecTmp[1];
Jcogdes[2][i] = sumMassNum/sumMassDenum * vecTmp[2];
Jcogdes[3][i] = sumMassNum/sumMassDenum * vecTmp[3];
Jcogdes[4][i] = joint_axis_pos_des[i][_X_];
Jcogdes[5][i] = joint_axis_pos_des[i][_Y_];
Jcogdes[6][i] = joint_axis_pos_des[i][_Z_];
}
}
void SurfaceObject::draw(bool drawPoints, bool drawLines, bool drawTransparent, bool pocketView, double * offset, double * c)
{
int i = 0;
int j = 0;
int pt1, pt2, pt3;
////Declare Materials
GLfloat mat_solid[] = { 1.0, 1.0, 1.0, 1.0 };
GLfloat mat_zero[] = { 0.0, 0.0, 0.0, 1.0 };
GLfloat mat_transparent[] = { 0.2, 0.2, 0.2, 0.2 };
GLfloat mat_emission[] = { 0.0, 0.0, 0.0, 1.0 };
GLfloat mat_specular[] = { 0.1, 0.1, 0.1, 0.0 };
GLfloat low_shininess[] = { .5 };
float amb[] = {0.20f, 0.50f, 1.0f, 0.1f};
float diff[] = {0.20f, 0.50f, 1.0f, 0.1f};
float spec[] = {1.0f, 1.0f, 1.0f, 1.0f};
float shine[] = {0.8 * 128.0f}; // The glass is very shiny
double * cent = new double[3];
cent[0] = 0; cent[1] = 0; cent[2] = 0;
if(offset != NULL){
delete[](cent);
cent = offset;
}
///drawPoints
if(drawPoints){
glBegin(GL_POINTS);
glColor3f(1.00000f, 1.00000f, 1.00000f);
for(i = 0; i<numTriangles; i++){
pt1 = triangles[3*i+0];
pt2 = triangles[3*i+1];
pt3 = triangles[3*i+2];
glVertex3f( surfacePoints [3*pt1+0]-cent[0],surfacePoints [3*pt1+1]-cent[1],surfacePoints [3*pt1+2]-cent[2] );
glVertex3f( surfacePoints [3*pt2+0]-cent[0],surfacePoints [3*pt2+1]-cent[1],surfacePoints [3*pt2+2]-cent[2] );
glVertex3f( surfacePoints [3*pt3+0]-cent[0],surfacePoints [3*pt3+1]-cent[1],surfacePoints [3*pt3+2]-cent[2] );
}
glEnd();
}
if(drawLines){
glBegin(GL_LINES);
glColor3f(0.00000f, 1.00000f, 0.00000f);
for(i = 0; i<numTriangles; i++){
pt1 = triangles[3*i+0];
pt2 = triangles[3*i+1];
pt3 = triangles[3*i+2];
glVertex3f( surfacePoints [3*pt1+0]-cent[0],surfacePoints [3*pt1+1]-cent[1],surfacePoints [3*pt1+2]-cent[2] );
glVertex3f( surfacePoints [3*pt2+0]-cent[0],surfacePoints [3*pt2+1]-cent[1],surfacePoints [3*pt2+2]-cent[2] );
glVertex3f( surfacePoints [3*pt2+0]-cent[0],surfacePoints [3*pt2+1]-cent[1],surfacePoints [3*pt2+2]-cent[2] );
glVertex3f( surfacePoints [3*pt3+0]-cent[0],surfacePoints [3*pt3+1]-cent[1],surfacePoints [3*pt3+2]-cent[2] );
glVertex3f( surfacePoints [3*pt3+0]-cent[0],surfacePoints [3*pt3+1]-cent[1],surfacePoints [3*pt3+2]-cent[2] );
glVertex3f( surfacePoints [3*pt1+0]-cent[0],surfacePoints [3*pt1+1]-cent[1],surfacePoints [3*pt1+2]-cent[2] );
}
glEnd();
}
bool drawNormals = false;
if(drawNormals){
glColor3f(1.00000f, 1.00000f, 1.00000f);
glBegin(GL_LINES);
for(i = 0; i<numTriangles; i++){
pt1 = triangles[3*i+0];
pt2 = triangles[3*i+1];
pt3 = triangles[3*i+2];
double * p1 = new double[3];
p1[0] = surfacePoints [3*pt1+0]-cent[0]; p1[1] = surfacePoints [3*pt1+1]-cent[1]; p1[2] = surfacePoints [3*pt1+2]-cent[2];
double * p2 = new double[3];
p2[0] = surfacePoints [3*pt2+0]-cent[0]; p2[1] = surfacePoints [3*pt2+1]-cent[1]; p2[2] = surfacePoints [3*pt2+2]-cent[2];
double * p3 = new double[3];
p3[0] = surfacePoints [3*pt3+0]-cent[0]; p3[1] = surfacePoints [3*pt3+1]-cent[1]; p3[2] = surfacePoints [3*pt3+2]-cent[2];
double * v1 = new double[3];
v1[0] = p2[0] - p1[0]; v1[1] = p2[1] - p1[1]; v1[2] = p2[2] - p1[2];
double * v2 = new double[3];
v2[0] = p3[0] - p1[0]; v2[1] = p3[1] - p1[1]; v2[2] = p3[2] - p1[2];
double * cross = crossProd(v1, v2);
double * norm = normalizeVector(cross);
double * avg = new double[3];
avg[0] = (p1[0]+p2[0]+p3[0])/3; avg[1] = (p1[1]+p2[1]+p3[1])/3; avg[2] = (p1[2]+p2[2]+p3[2])/3;
double * newPt = new double[3];
newPt[0] = avg[0] + norm[0];
newPt[1] = avg[1] + norm[1];
newPt[2] = avg[2] + norm[2];
glVertex3f( avg[0], avg[1], avg[2] );
glVertex3f( newPt[0], newPt[1], newPt[2] );
//printf("vectorSize: %lf\n", vectorSize( newPt[0]-avg[0], newPt[1]-avg[1], newPt[2]-avg[2]) );
delete[](p1); delete[](p2); delete[](p3); delete[](v1); delete[](v2);
delete[](cross); delete[](norm); delete[](avg); delete[](newPt);
}
glEnd();
}
//set material properties
if(drawTransparent == true){
glLightModeli(GL_LIGHT_MODEL_TWO_SIDE, 0);
glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT, amb);
glMaterialfv(GL_FRONT_AND_BACK, GL_DIFFUSE, diff);
glMaterialfv(GL_FRONT_AND_BACK, GL_SPECULAR, spec);
glMaterialfv(GL_FRONT_AND_BACK, GL_SHININESS, shine);
glEnable(GL_BLEND);
glDepthMask(GL_FALSE);
// glBlendFunc(GL_SRC_ALPHA, GL_ONE);
glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
}
else{
glMaterialfv(GL_FRONT, GL_EMISSION, mat_zero);
glMaterialfv(GL_FRONT, GL_DIFFUSE, mat_solid);
}
double * color = new double[3];
if(c != NULL){ color[0] = c[0]; color[1] = c[1]; color[2] = c[2]; }
else{
if(drawTransparent){ color[0] = 0.5f; color[1] = 0.5f; color[2] = 0.0f; }
else{ color[0] = 0.0f; color[1] = 0.5f; color[2] = 0.5f; }
}
set_t debug_triangleSet = alloc_set(0);
// debug_triangleSet = put_set(debug_triangleSet, 37);
// debug_triangleSet = put_set(debug_triangleSet, 38);
// debug_triangleSet = put_set(debug_triangleSet, 39);
// debug_triangleSet = put_set(debug_triangleSet, 213);
// debug_triangleSet = put_set(debug_triangleSet, 215);
///render the triangles
glBegin(GL_TRIANGLES);
for(i = 0; i<numTriangles; i++){
pt1 = triangles[3*i+0]; pt2 = triangles[3*i+1]; pt3 = triangles[3*i+2];
double s = 1.00; ///surface geometry hack - scales up slightly so there isnt so much z-buffer collision.
// if(i == 215){ glColor4f(1, 0, 0, .5); }
if(drawTransparent){///surface geometry hack
glColor4f(color[0], color[1], color[2], .5);
if( contains_set(debug_triangleSet, i) ){ glColor4f(1, 0, 0, .5); }
glNormal3f( surfaceNormals[3*pt1+0], surfaceNormals[3*pt1+1], surfaceNormals[3*pt1+2] );
glVertex3f( s*(surfacePoints [3*pt1+0]-cent[0]),s*(surfacePoints [3*pt1+1]-cent[1]),s*(surfacePoints [3*pt1+2]-cent[2]) );
glNormal3f( surfaceNormals[3*pt2+0], surfaceNormals[3*pt2+1], surfaceNormals[3*pt2+2] );
glVertex3f( s*(surfacePoints [3*pt2+0]-cent[0]),s*(surfacePoints [3*pt2+1]-cent[1]),s*(surfacePoints [3*pt2+2]-cent[2]) );
glNormal3f( surfaceNormals[3*pt3+0], surfaceNormals[3*pt3+1], surfaceNormals[3*pt3+2] );
glVertex3f( s*(surfacePoints [3*pt3+0]-cent[0]),s*(surfacePoints [3*pt3+1]-cent[1]),s*(surfacePoints [3*pt3+2]-cent[2]) );
}
else{
if(colorFlag == true){
glColor4f( colors[3*pt1+0], colors[3*pt1+1], colors[3*pt1+2], 1.0 );
glNormal3f( surfaceNormals[3*pt1+0], surfaceNormals[3*pt1+1], surfaceNormals[3*pt1+2] );
glVertex3f( surfacePoints [3*pt1+0]-cent[0],surfacePoints [3*pt1+1]-cent[1],surfacePoints [3*pt1+2]-cent[2] );
glColor4f( colors[3*pt2+0], colors[3*pt2+1], colors[3*pt2+2], 1.0 );
glNormal3f( surfaceNormals[3*pt2+0], surfaceNormals[3*pt2+1], surfaceNormals[3*pt2+2] );
glVertex3f( surfacePoints [3*pt2+0]-cent[0],surfacePoints [3*pt2+1]-cent[1],surfacePoints [3*pt2+2]-cent[2] );
glColor4f( colors[3*pt3+0], colors[3*pt3+1], colors[3*pt3+2], 1.0 );
glNormal3f( surfaceNormals[3*pt3+0], surfaceNormals[3*pt3+1], surfaceNormals[3*pt3+2] );
glVertex3f( surfacePoints [3*pt3+0]-cent[0],surfacePoints [3*pt3+1]-cent[1],surfacePoints [3*pt3+2]-cent[2] );
}
else{
glColor4f(color[0], color[1], color[2], 1.0);
if( contains_set(debug_triangleSet, i) ){ glColor4f(1, 0, 0, 1); }
glNormal3f( surfaceNormals[3*pt1+0], surfaceNormals[3*pt1+1], surfaceNormals[3*pt1+2] );
glVertex3f( surfacePoints [3*pt1+0]-cent[0],surfacePoints [3*pt1+1]-cent[1],surfacePoints [3*pt1+2]-cent[2] );
glNormal3f( surfaceNormals[3*pt2+0], surfaceNormals[3*pt2+1], surfaceNormals[3*pt2+2] );
glVertex3f( surfacePoints [3*pt2+0]-cent[0],surfacePoints [3*pt2+1]-cent[1],surfacePoints [3*pt2+2]-cent[2] );
glNormal3f( surfaceNormals[3*pt3+0], surfaceNormals[3*pt3+1], surfaceNormals[3*pt3+2] );
glVertex3f( surfacePoints [3*pt3+0]-cent[0],surfacePoints [3*pt3+1]-cent[1],surfacePoints [3*pt3+2]-cent[2] );
}
}
}
for(i = 0; i<numTriangles; i++){
double s = 1.00; ///surface geometry hack - scales up slightly so there isnt so much z-buffer collision.
pt1 = triangles[3*i+0]; pt2 = triangles[3*i+1]; pt3 = triangles[3*i+2];
if(pocketView){/// we render both sides by drawing backwards triangles too.
//glColor4f(color[0]/2, color[1]/2, color[2]/2, 1.0);
if(drawTransparent){///surface geometry hack
glColor4f(color[0], color[1], color[2], .3);
if( contains_set(debug_triangleSet, i) ){ glColor4f(1, 0, 0, .5); }
glNormal3f( -surfaceNormals[3*pt1+0], -surfaceNormals[3*pt1+1], -surfaceNormals[3*pt1+2] );
glVertex3f( s*(surfacePoints [3*pt1+0]-cent[0]),s*(surfacePoints [3*pt1+1]-cent[1]),s*(surfacePoints [3*pt1+2]-cent[2]) );
glNormal3f( -surfaceNormals[3*pt3+0], -surfaceNormals[3*pt3+1], -surfaceNormals[3*pt3+2] );
glVertex3f( s*(surfacePoints [3*pt3+0]-cent[0]),s*(surfacePoints [3*pt3+1]-cent[1]),s*(surfacePoints [3*pt3+2]-cent[2]) );
glNormal3f( -surfaceNormals[3*pt2+0], -surfaceNormals[3*pt2+1], -surfaceNormals[3*pt2+2] );
glVertex3f( s*(surfacePoints [3*pt2+0]-cent[0]),s*(surfacePoints [3*pt2+1]-cent[1]),s*(surfacePoints [3*pt2+2]-cent[2]) );
}
else{
glColor4f(color[0]/4, color[1]/4, color[2]/4, 1.0);
glColor3f(0.00000f, 0.250000f, 0.2500000f);
if( contains_set(debug_triangleSet, i) ){ glColor4f(1, 0, 0, 1); }
glNormal3f( surfaceNormals[3*pt1+0], surfaceNormals[3*pt1+1], surfaceNormals[3*pt1+2] );
glVertex3f( surfacePoints [3*pt1+0]-cent[0],surfacePoints [3*pt1+1]-cent[1],surfacePoints [3*pt1+2]-cent[2] );
glNormal3f( surfaceNormals[3*pt3+0], surfaceNormals[3*pt3+1], surfaceNormals[3*pt3+2] );
glVertex3f( surfacePoints [3*pt3+0]-cent[0],surfacePoints [3*pt3+1]-cent[1],surfacePoints [3*pt3+2]-cent[2] );
glNormal3f( surfaceNormals[3*pt2+0], surfaceNormals[3*pt2+1], surfaceNormals[3*pt2+2] );
glVertex3f( surfacePoints [3*pt2+0]-cent[0],surfacePoints [3*pt2+1]-cent[1],surfacePoints [3*pt2+2]-cent[2] );
}
}
}
glEnd();
free_set(debug_triangleSet);
glColor4f(1.0f, 1.0f, 1.0f, 1.0f);
if( highlights != NULL ){
glBegin(GL_POINTS);
for(i = 0; i<numPoints; i++){
if(contains_set(highlights, i)){
//glPushMatrix();
glVertex3f( surfacePoints[3*i+0]-cent[0], surfacePoints[3*i+1]-cent[1], surfacePoints[3*i+2]-cent[2] );
//glTranslatef(surfacePoints[3*i+0], surfacePoints[3*i+1], surfacePoints[3*i+2] );
//glutSolidSphere ( .3, 4, 4) ;
//glPopMatrix();
}
}
glEnd();
}
glColor4f(1.0f, 1.0f, 1.0f, 1.0f);
if(edges != NULL && drawTransparent == false){
glBegin(GL_LINES);
for(i = 0; i<size_set(edges); i++){
set_t tempSet = (set_t) mapsto_set(edges, edges[i]);
for(j = 0; j<size_set(tempSet); j++){
if(tempSet[j] > edges[i]){
glVertex3f( surfacePoints[3*edges[i]+0]-cent[0], surfacePoints[3*edges[i]+1]-cent[1], surfacePoints[3*edges[i]+2]-cent[2] );
glVertex3f( surfacePoints[3*tempSet[j]+0]-cent[0], surfacePoints[3*tempSet[j]+1]-cent[1], surfacePoints[3*tempSet[j]+2]-cent[2] );
}
}
}
glEnd();
}
///fix material properties
if(drawTransparent == true){
glDepthMask(GL_TRUE);
glDisable(GL_BLEND);
}
if(offset == NULL){
delete[](cent);
}
delete[](color);
}
void collideWithPolyTrees(Particles& particles, const Matrix4& txMx, const std::vector<Object>& objs, std::vector<ozcollide::AABBTreePoly*>& coltrees) {
Particles::Positions pos = particles.pos_;
Particles::Velocities& vel = particles.vel_;
Particles::Velocities& dv = particles.dv_;
// Particles::Accelerations& da = particles.da_;
const unsigned size = pos.size();
for (unsigned i=0;i<size;i++) {
pos[i] = Matrix4AffineReal3(txMx, pos[i]);
}
for (unsigned i=0;i<size;i++) {
Real3 pi = pos[i];
Real3 pf = pi+dv[i];
std::vector<ozcollide::AABBTreePoly::SegmentColResult> colres(coltrees.size());
for (unsigned j=0;j<coltrees.size();j++) {
coltrees[j]->collideWithSegment(toVec3f(pi),toVec3f(pf), colres[j]);
}
for (unsigned j=0;j<coltrees.size();j++) {
real tmin = std::numeric_limits<float>::max();
Real3 nmin(0.);
const ozcollide::Polygon* pmin = 0;
for (unsigned k=0;k<colres[j].polys_.size();k++) {
const ozcollide::Polygon* poly= colres[j].polys_[k];
const Real3& a = objs[j].vx_[poly->getIndex(0)];
const Real3& b = objs[j].vx_[poly->getIndex(1)];
const Real3& c = objs[j].vx_[poly->getIndex(2)];
// const Real3& an = objs[j].vn_[poly->getIndex(0)];
// const Real3& bn = objs[j].vn_[poly->getIndex(1)];
// const Real3& cn = objs[j].vn_[poly->getIndex(2)];
ozcollide::Plane plane;
plane.fromPoints(toVec3f(a),toVec3f(b), toVec3f(c) );
real t;
if (plane.intersectWithLine(toVec3f(pi),toVec3f(pf),t)) {
if(t<tmin) {
tmin=t;
pmin = poly;
nmin = normalize( crossProd( b-a, c-a) );
}
}
// reaction
if (pmin) {
// Real3 dvi = dv[i];
// Real3 x = pi+tmin*dvi;
// pos[i]=x+(1.-tmin)*(dvi.norm())*nmin; // separate from geometry
dv[i] = tmin*dv[i]+(1.-tmin)*dv[i].norm()*nmin;
// perfect bounce == 2 slip walls = 1
real k = 1.8;
vel[i]+=-k*dotProd(vel[i],nmin)*nmin; // bounce
vel[i]*=0.95;
// this gets inside geometry often
// pos[i]=x+(1.-tmin)*(dv[i].norm())*nmin;
// vel[i]=vel[i].norm()*nmin;
// dv[i]=Real3(0.);
// da[i]= Real3(0.);
}
}
}
}
}
sta::StateVector
cartesianTOrotating (int mode, const StaBody* firstbody, const StaBody* secondbody, sta::StateVector cartesian, double JD)
{
sta::StateVector rotating;
const StaBody* sun = STA_SOLAR_SYSTEM->sun();
double mu1=getGravParam_user(sun->mu(), secondbody->mu());
double mu2=getGravParam_user(firstbody->mu(),secondbody->mu());
//double velocity1=secondbody->linearV();
double velocity1=sqrt(sun->mu()/secondbody->distance());
//ephemerides of the 2nd body relative to the Sun
Eigen::Vector3d positionBody2 = secondbody->stateVector(JD, sun, sta::COORDSYS_EME_J2000).position;
Eigen::Vector3d velocityBody2 = secondbody->stateVector(JD, sun, sta::COORDSYS_EME_J2000).velocity;
velocity1=velocityBody2.norm();
//velocity1=secondbody->stateVector(JD, sun, sta::COORDSYS_EME_J2000).velocity.norm();
double distance1=positionBody2.norm();
double i;
double alfa=0; double beta=0;
Eigen::Vector3d baricenter;
//coefficients for coming computations
//double A, B, a, b, c;
if (firstbody != sun) //we are considering a system like Earth-Moon or Saturn-Titan (the Sun is NOT the first body!)
{
//qDebug()<<"CASE 1: NO SUN";
//velocity1=sqrt(firstbody->mu()/secondbody->distance());
Eigen::Vector3d positionBody1 = firstbody->stateVector(JD, sun, sta::COORDSYS_EME_J2000).position;
Eigen::Vector3d positionBody1_Body2 = positionBody2 - positionBody1;
//distance1=secondbody->distance()-firstbody->distance();
distance1=positionBody1_Body2.norm();
Eigen::Vector3d velocityBody1 = firstbody->stateVector(JD, sun, sta::COORDSYS_EME_J2000).velocity;
Eigen::Vector3d velocityBody2 = secondbody->stateVector(JD, sun, sta::COORDSYS_EME_J2000).velocity;
Eigen::Vector3d velocityBody1_Body2 = velocityBody2 - velocityBody1;
velocity1=velocityBody1_Body2.norm();
//alfa = acos (positionBody1_Body2.z()/distance2);
double nodeJD;
ascendingNodeAngle (firstbody, secondbody, JD, nodeJD, alfa);
beta=trueAnomalyFromAscNode (firstbody, secondbody, JD, nodeJD, alfa);
//beta = atan2 (sqrt(pow(positionBody1_Body2.x(),2.0)+pow(positionBody1_Body2.y(),2.0)),positionBody1_Body2.z());
//alfa = atan2 (positionBody1_Body2.y(), positionBody1_Body2.x());
Eigen::Vector3d velocityCart=secondbody->stateVector(JD, firstbody, sta::COORDSYS_EME_J2000).velocity;
Eigen::Vector3d crossProd=positionBody1_Body2.cross(velocityCart);
//double inclinationReq;
i=acos(crossProd(2)/crossProd.norm());
//qDebug()<<sta::radToDeg(i);
//mu1=mu2;
if (mode==0 || mode==1 || mode==3) //the initial orbit was around the 1st body
{
baricenter(0)= (-mu2);//*cos(beta)*sin(alfa); baricenter(1)= (mu2)*sin(beta)*sin(alfa); baricenter(2)= (mu2)*cos(alfa);
//a=b=c=0;
}
else if (mode==2 || mode==4) //the initial orbit was around the 2nd body
{
baricenter(0)= (+1-mu2);//*cos(beta)*sin(alfa); baricenter(1)= (-1+mu2)*sin(beta)*sin(alfa); baricenter(2)= (-1+mu2)*cos(alfa);
//a=b=c=0;
}
//A=-1/cos(alfa)*((cartesian.position.x()/distance1-baricenter(0))*cos(beta)+(cartesian.position.y()/distance1-baricenter(1))*sin(beta));
//B=1/(cos(beta)*sin(alfa))*((cartesian.position.y()/distance1-baricenter(1))*sin(alfa)+(cartesian.position.z()/distance1-baricenter(2))*sin(beta)*cos(alfa));
rotating.position.x()=(cartesian.position.x()*(cos(alfa)*cos(beta)-cos(i)*sin(alfa)*sin(beta))+cartesian.position.y()*(cos(alfa)*sin(beta)*cos(i)+sin(alfa)*cos(beta))+cartesian.position.z()*sin(i)*sin(alfa))/distance1+baricenter(0);
rotating.position.y()=(cartesian.position.x()*(-cos(i)*sin(alfa)*cos(beta)-cos(alfa)*sin(beta))+cartesian.position.y()*(cos(i)*cos(alfa)*cos(beta)-sin(alfa)*sin(beta))-cartesian.position.z()*(-sin(i)*cos(alfa)))/distance1;
rotating.position.z()=(cartesian.position.x()*(sin(i)*sin(beta))+cartesian.position.y()*(sin(i)*cos(beta))+cartesian.position.z()*cos(i))/distance1;
}
else
{
//same transformation applied to a Sun-body system. This has to be done in any case
//position of body 2 relative to body1
//qDebug()<<"CASE 2: SUN";
//alfa = acos (positionBody2.z()/distance1);
double nodeJD;
ascendingNodeAngle (firstbody, secondbody, JD, nodeJD, alfa);
beta=trueAnomalyFromAscNode (firstbody, secondbody, JD, nodeJD, alfa);
Eigen::Vector3d velocityCart=secondbody->stateVector(JD, firstbody, sta::COORDSYS_EME_J2000).velocity;
Eigen::Vector3d crossProd=positionBody2.cross(velocityCart);
//double inclinationReq;
i=acos(crossProd(2)/crossProd.norm());
if (mode==0 || mode==1 || mode==3) //the initial orbit was around the 1st body
{
baricenter(0)= (-mu1);//*cos(beta)*sin(alfa); baricenter(1)= (mu1)*sin(beta)*sin(alfa); baricenter(2)= (mu1)*cos(alfa);
//a=b=c=0;
}
else if (mode==2 || mode==4) //the initial orbit was around the 2nd body
{
baricenter(0)= (+1-mu1);//*cos(beta)*sin(alfa); baricenter(1)= (-1+mu1)*sin(beta)*sin(alfa); baricenter(2)= (-1+mu1)*cos(alfa);
//a=b=c=0;
}
//A=-1/cos(alfa)*((cartesian.position.x()/distance1-baricenter(0))*cos(beta)+(cartesian.position.y()/distance1-baricenter(1))*sin(beta));
//B=1/(cos(beta)*sin(alfa))*((cartesian.position.y()/distance1-baricenter(1))*sin(alfa)+(cartesian.position.z()/distance1-baricenter(2))*sin(beta)*cos(alfa));
rotating.position.x()=(cartesian.position.x()*(cos(alfa)*cos(beta)-cos(i)*sin(alfa)*sin(beta))+cartesian.position.y()*(cos(alfa)*sin(beta)*cos(i)+sin(alfa)*cos(beta))+cartesian.position.z()*sin(i)*sin(alfa))/distance1+baricenter(0);
rotating.position.y()=(cartesian.position.x()*(-cos(i)*sin(alfa)*cos(beta)-cos(alfa)*sin(beta))+cartesian.position.y()*(cos(i)*cos(alfa)*cos(beta)-sin(alfa)*sin(beta))-cartesian.position.z()*(-sin(i)*cos(alfa)))/distance1;
rotating.position.z()=(cartesian.position.x()*(sin(i)*sin(beta))+cartesian.position.y()*(sin(i)*cos(beta))+cartesian.position.z()*cos(i))/distance1;
}
//A=-1/cos(alfa)*((cartesian.velocity.x()/velocity1)*cos(beta)+(cartesian.velocity.y()/velocity1)*sin(beta));
//B=1/(cos(beta)*sin(alfa))*((cartesian.velocity.y()/velocity1)*sin(alfa)+(cartesian.velocity.z()/velocity1)*sin(beta)*cos(alfa));
rotating.velocity.x()=(cartesian.velocity.x()*(cos(alfa)*cos(beta)-cos(i)*sin(alfa)*sin(beta))+cartesian.velocity.y()*(cos(alfa)*sin(beta)*cos(i)+sin(alfa)*cos(beta))+cartesian.velocity.z()*sin(i)*sin(alfa))/velocity1;
rotating.velocity.y()=(cartesian.velocity.x()*(-cos(i)*sin(alfa)*cos(beta)-cos(alfa)*sin(beta))+cartesian.velocity.y()*(cos(i)*cos(alfa)*cos(beta)-sin(alfa)*sin(beta))-cartesian.velocity.z()*(-sin(i)*cos(alfa)))/velocity1;
rotating.velocity.z()=(cartesian.velocity.x()*(sin(i)*sin(beta))+cartesian.velocity.y()*(sin(i)*cos(beta))+cartesian.velocity.z()*cos(i))/velocity1;
return rotating;
}
