void Galaxy::renderGalaxyPointSprites(const GLContext&,
const Vec3f& offset,
const Quatf& viewerOrientation,
float brightness,
float pixelSize)
{
if (form == NULL)
return;
/* We'll first see if the galaxy's apparent size is big enough to
   be noticeable on screen; if it's not we'll break right here,
   avoiding all the overhead of the matrix transformations and
   GL state changes: */
float distanceToDSO = offset.length() - getRadius();
if (distanceToDSO < 0)
distanceToDSO = 0;
float minimumFeatureSize = pixelSize * distanceToDSO;
float size = 2 * getRadius();
if (size < minimumFeatureSize)
return;
if (galaxyTex == NULL)
{
galaxyTex = CreateProceduralTexture(width, height, GL_RGBA,
GalaxyTextureEval);
}
assert(galaxyTex != NULL);
glEnable(GL_TEXTURE_2D);
galaxyTex->bind();
Mat3f viewMat = viewerOrientation.toMatrix3();
Vec3f v0 = Vec3f(-1, -1, 0) * viewMat;
Vec3f v1 = Vec3f( 1, -1, 0) * viewMat;
Vec3f v2 = Vec3f( 1, 1, 0) * viewMat;
Vec3f v3 = Vec3f(-1, 1, 0) * viewMat;
//Mat4f m = (getOrientation().toMatrix4() *
// Mat4f::scaling(form->scale) *
// Mat4f::scaling(getRadius()));
Mat3f m =
Mat3f::scaling(form->scale)*getOrientation().toMatrix3()*Mat3f::scaling(size);
// Note: fixed missing factor of 2 in getRadius() scaling of galaxy diameter!
// Note: fixed correct ordering of (non-commuting) operations!
int pow2 = 1;
vector<Blob>* points = form->blobs;
unsigned int nPoints = (unsigned int) (points->size() * clamp(getDetail()));
// corrections to avoid excessive brightening if viewed e.g. edge-on
float brightness_corr = 1.0f;
float cosi;
if (type < E0 || type > E3) //all galaxies, except ~round elliptics
{
cosi = Vec3f(0,1,0) * getOrientation().toMatrix3()
* offset/offset.length();
brightness_corr = (float) sqrt(abs(cosi));
if (brightness_corr < 0.2f)
brightness_corr = 0.2f;
}
if (type > E3) // only elliptics with higher ellipticities
{
cosi = Vec3f(1,0,0) * getOrientation().toMatrix3()
* offset/offset.length();
brightness_corr = brightness_corr * (float) abs((cosi));
if (brightness_corr < 0.45f)
brightness_corr = 0.45f;
}
glBegin(GL_QUADS);
for (unsigned int i = 0; i < nPoints; ++i)
{
if ((i & pow2) != 0)
{
pow2 <<= 1;
size /= 1.55f;
if (size < minimumFeatureSize)
break;
}
Blob b = (*points)[i];
Point3f p = b.position * m;
float br = b.brightness / 255.0f;
Color c = colorTable[b.colorIndex]; // lookup static color table
Point3f relPos = p + offset;
float screenFrac = size / relPos.distanceFromOrigin();
if (screenFrac < 0.1f)
{
float btot = ((type > SBc) && (type < Irr))? 2.5f: 5.0f;
float a = btot * (0.1f - screenFrac) * brightness_corr * brightness * br;
glColor4f(c.red(), c.green(), c.blue(), (4.0f*lightGain + 1.0f)*a);
glTexCoord2f(0, 0); glVertex(p + (v0 * size));
glTexCoord2f(1, 0); glVertex(p + (v1 * size));
glTexCoord2f(1, 1); glVertex(p + (v2 * size));
glTexCoord2f(0, 1); glVertex(p + (v3 * size));
}
}
glEnd();
}
void Camera::setViewDirection( const Vec3f &aViewDirection )
{
mViewDirection = aViewDirection.normalized();
mOrientation = Quatf( Vec3f( 0.0f, 0.0f, -1.0f ), mViewDirection );
mModelViewCached = false;
}
//Computes the normals, if they haven't been computed yet
void computeNormals() {
if (computedNormals) {
return;
}
//Compute the rough version of the normals
Vec3f** normals2 = new Vec3f*[l];
for (int i = 0; i < l; i++) {
normals2[i] = new Vec3f[w];
}
for (int z = 0; z < l; z++) {
for (int x = 0; x < w; x++) {
Vec3f sum(0.0f, 0.0f, 0.0f);
Vec3f out;
if (z > 0) {
out = Vec3f(0.0f, hs[z - 1][x] - hs[z][x], -1.0f);
}
Vec3f in;
if (z < l - 1) {
in = Vec3f(0.0f, hs[z + 1][x] - hs[z][x], 1.0f);
}
Vec3f left;
if (x > 0) {
left = Vec3f(-1.0f, hs[z][x - 1] - hs[z][x], 0.0f);
}
Vec3f right;
if (x < w - 1) {
right = Vec3f(1.0f, hs[z][x + 1] - hs[z][x], 0.0f);
}
if (x > 0 && z > 0) {
sum += out.cross(left).normalize();
}
if (x > 0 && z < l - 1) {
sum += left.cross(in).normalize();
}
if (x < w - 1 && z < l - 1) {
sum += in.cross(right).normalize();
}
if (x < w - 1 && z > 0) {
sum += right.cross(out).normalize();
}
normals2[z][x] = sum;
}
}
//Smooth out the normals
const float FALLOUT_RATIO = 0.5f;
for (int z = 0; z < l; z++) {
for (int x = 0; x < w; x++) {
Vec3f sum = normals2[z][x];
if (x > 0) {
sum += normals2[z][x - 1] * FALLOUT_RATIO;
}
if (x < w - 1) {
sum += normals2[z][x + 1] * FALLOUT_RATIO;
}
if (z > 0) {
sum += normals2[z - 1][x] * FALLOUT_RATIO;
}
if (z < l - 1) {
sum += normals2[z + 1][x] * FALLOUT_RATIO;
}
if (sum.magnitude() == 0) {
sum = Vec3f(0.0f, 1.0f, 0.0f);
}
normals[z][x] = sum;
}
}
for (int i = 0; i < l; i++) {
delete[] normals2[i];
}
delete[] normals2;
computedNormals = true;
}
void VRShadowEngine::setupCamera(Light *pLight,
LightTypeE eType,
RenderAction *pAction,
EngineDataPtr pEngineData)
{
if(eType == Directional)
{
DirectionalLight *pDLight =
dynamic_cast<DirectionalLight *>(pLight);
MatrixCameraUnrecPtr pCam =
dynamic_cast<MatrixCamera *>(pEngineData->getCamera());
if(pCam == NULL)
{
pCam = MatrixCamera::createLocal();
pEngineData->setCamera(pCam);
}
Vec3f diff;
Pnt3f center;
Matrix transMatrix;
Node *pNode = pAction->getActNode();
// tmpDir = DirectionalLightPtr::dcast(_lights[i]);
diff = (pNode->getVolume().getMax() -
pNode->getVolume().getMin());
Real32 sceneWidth = diff.length() * 0.5f;
// Not final values. May get tweaked in the future
Real32 sceneHeight = diff.length() * 0.5f;
pNode->getVolume().getCenter(center);
Vec3f lightdir = pDLight->getDirection();
if(pLight->getBeacon() != NULL)
{
Matrix m = pLight->getBeacon()->getToWorld();
m.mult(lightdir, lightdir);
}
MatrixLookAt(transMatrix,
center + lightdir,
center,
Vec3f(0,1,0));
transMatrix.invert();
Matrix proMatrix;
proMatrix.setIdentity();
MatrixOrthogonal( proMatrix,
-sceneWidth, sceneWidth, -sceneHeight,
sceneHeight, -sceneWidth, sceneWidth);
pCam->setProjectionMatrix(proMatrix );
pCam->setModelviewMatrix (transMatrix);
}
else if(eType == Point)
{
PointLight *pPLight = dynamic_cast<PointLight *>(pLight);
MatrixCameraUnrecPtr pCam =
dynamic_cast<MatrixCamera *>(pEngineData->getCamera());
if(pCam == NULL)
{
pCam = MatrixCamera::createLocal();
pEngineData->setCamera(pCam);
}
Real32 angle;
Vec3f dist;
Pnt3f center;
Vec3f diff;
Matrix transMatrix;
Node *pNode = pAction->getActNode();
pNode->getVolume().getCenter(center);
Pnt3f lightpos = pPLight->getPosition();
if(pLight->getBeacon() != NULL)
{
Matrix m = pLight->getBeacon()->getToWorld();
m.mult(lightpos, lightpos);
}
MatrixLookAt(transMatrix,
lightpos,
center,
Vec3f(0,1,0));
transMatrix.invert();
diff = (pNode->getVolume().getMax() -
pNode->getVolume().getMin());
dist = lightpos - center;
angle = atan((diff.length() * 0.5) / dist.length());
Matrix proMatrix;
proMatrix.setIdentity();
MatrixPerspective( proMatrix,
2.f * angle,
1,
pAction->getActivePartition()->getNear(),
pAction->getActivePartition()->getFar ());
pCam->setProjectionMatrix(proMatrix );
pCam->setModelviewMatrix (transMatrix);
}
}
void Tube::buildFrenet()
{
mFrames.clear();
int n = mPs.size();
mFrames.resize( n );
for( int i = 0; i < n; ++i ) {
Vec3f p0, p1, p2;
if( i < (n - 2) ) {
p0 = mPs[i];
p1 = mPs[i + 1];
p2 = mPs[i + 2];
}
else if( i == (n - 2) ) {
p0 = mPs[i - 1];
p1 = mPs[i];
p2 = mPs[i + 1];
}
else if( i == (n - 1) ) {
p0 = mPs[i - 3];
p1 = mPs[i - 2];
p2 = mPs[i - 1];
}
Vec3f t = (p1 - p0).normalized();
Vec3f n = t.cross(p2 - p0).normalized();
if( n.length() == 0.0f ) {
int i = fabs( t[0] ) < fabs( t[1] ) ? 0 : 1;
if( fabs( t[2] ) < fabs( t[i] ) )
i = 2;
Vec3f v( 0.0f, 0.0f, 0.0f );
v[i] = 1.0;
n = t.cross( v ).normalized();
}
Vec3f b = t.cross( n );
Matrix44f& m = mFrames[i];
m.at( 0, 0 ) = b.x;
m.at( 1, 0 ) = b.y;
m.at( 2, 0 ) = b.z;
m.at( 3, 0 ) = 0;
m.at( 0, 1 ) = n.x;
m.at( 1, 1 ) = n.y;
m.at( 2, 1 ) = n.z;
m.at( 3, 1 ) = 0;
m.at( 0, 2 ) = t.x;
m.at( 1, 2 ) = t.y;
m.at( 2, 2 ) = t.z;
m.at( 3, 2 ) = 0;
m.at( 0, 3 ) = mPs[i].x;
m.at( 1, 3 ) = mPs[i].y;
m.at( 2, 3 ) = mPs[i].z;
m.at( 3, 3 ) = 1;
}
}
void CPlanetFinderEngine::buildSolarSystemList( std::vector< star3map::Sprite > & solarsystem )
{
solarsystem.clear();
float moonN0 = 125.1228;
float moonw0 = 318.0634;
//float moonM0 = 115.3654;
float moonN = moonN0 - 0.0529538083 * daysSince2000;
float moonw = moonw0 + 0.1643573223 * daysSince2000;
//float MoonM = moonM0 + 13.0649929509 * daysSince2000;
moon.Init("Moon",1.23e-02,27.322,2.569519e-03,0.0549,5.145,
moonN,
moonN+moonw, //-- lon of peri = N + w
/*moonN0+moonw0+moonM0*/ // meanlong2000 = N+w+M
218.32, //use Dave's data instead
false,0.,0.,0.,0.);
// This is the correction for parallax due to the earth's rotation.
Vec3f earthPosition = earth.position( daysSince2000, 0.000001 ); // + (zenith * (float)(4.3e-05));
Vec3f earthToMoon = moon.position( daysSince2000, 0.000001 );
//--- Nonkeplerian perturbations for the moon:
//First convert vector to spherical coords:
float moonRad = earthToMoon.Length();
float moonLat = acos( -earthToMoon.z / moonRad ) - M_PI/2.0;
float moonLon = atan2(earthToMoon.y, earthToMoon.x);
if ( moonLon < -M_PI/2.0 ) moonLon += M_PI;
if ( moonLon > M_PI/2.0 ) moonLon -= M_PI;
if ( earthToMoon.x < 0.0 ) moonLon += M_PI;
if ( moonLon < 0.0 ) moonLon += 2*M_PI;
if ( moonLon > 2.0*M_PI ) moonLon -= 2*M_PI;
moonLon = moonLon + MoonPerturbations::moonLongitudeCorrectionDegrees(daysSince2000)*M_PI/180.0;
moonLat = moonLat + MoonPerturbations::moonLatitudeCorrectionDegrees(daysSince2000)*M_PI/180.0;
earthToMoon = latLongToUnitVector(moonLat,moonLon);
earthToMoon *= moonRad;
Vec3f moonPosition = earthToMoon + earthPosition;
//================================================================================
// Show planets, moon & sun
//================================================================================
Vec3f planetsCenterOfMass(0, 0 , 0);
Vec3f currentPosition[9];
int i;
for (i= 0; i<PLANETS_NUMBER; i++)
{
currentPosition[i] = planets[i]->position( daysSince2000, 0.000001 );
planetsCenterOfMass += currentPosition[i] * planets[i]->mass;
}
Vec3f sunPosition = planetsCenterOfMass*(float)(-1.0/sunMass);
for (i= -1; i<PLANETS_NUMBER; i++)
{ // Yuck.
//==moon is when you do earth, sun is i== -1
Vec3f directionFromEarth;
SetColor( Vec4f( 1, 1, 1 ) );
Texture2D *tex = NULL;
std::string name;
bool isSun,isMoon;
isSun=false;
isMoon=false;
float scale = 1.0;
if (i!= -1 && planets[i]!=&earth)
{
// a planet other than earth
directionFromEarth = currentPosition[i] - earthPosition;
directionFromEarth.Normalize();
tex = planets[ i ]->texture;
name = planets[i]->name;
scale = planets[ i ]->scale;
}
else
{
if (i== -1)
{
//-- sun
directionFromEarth = sunPosition - earthPosition;
directionFromEarth.Normalize();
tex = sunTexture;
name = "Sun";
isSun = true;
}
else
{
//-- moon
directionFromEarth = moonPosition - earthPosition;
directionFromEarth.Normalize();
tex = moon.texture;
name = "Moon";
isMoon=true;
}
}
int dotRadius = (isSun || isMoon) ? 5 : 2;
dotRadius *= scale;
star3map::Sprite sp;
sp.direction = directionFromEarth;
sp.magnitude = 0;
sp.scale = dotRadius;
sp.name = name;
sp.color = Vec4f( 1, 1, 1, 1 );
sp.tex = tex;
solarsystem.push_back( sp );
}
}
void GLWidget::drawBoundingBox()
{
Vec3f volumeScale = Vec3f(volumeDim);
volumeScale = volumeScale / volumeScale.norm();
std::vector<Vec3f> BoundingBoxPlane1;
BoundingBoxPlane1.push_back(Vec3f(volumeScale.x, volumeScale.y, volumeScale.z));
BoundingBoxPlane1.push_back(Vec3f(volumeScale.x, -volumeScale.y, volumeScale.z));
BoundingBoxPlane1.push_back(Vec3f(volumeScale.x, -volumeScale.y, -volumeScale.z));
BoundingBoxPlane1.push_back(Vec3f(volumeScale.x, volumeScale.y, -volumeScale.z));
// BoundingBoxPlane1.push_back(Vec3f(volumeScale.x, volumeScale.y, -volumeScale.z));
// BoundingBoxPlane1.push_back(Vec3f(volumeScale.x, -volumeScale.y, -volumeScale.z));
// BoundingBoxPlane1.push_back(Vec3f(-volumeScale.x, -volumeScale.y, -volumeScale.z));
// BoundingBoxPlane1.push_back(Vec3f(-volumeScale.x, volumeScale.y, -volumeScale.z));
GLArrayBuffer::Ptr boundingBoxArrayBuffer = GLArrayBuffer::create(GL_ARRAY_BUFFER);
boundingBoxArrayBuffer->update(BoundingBoxPlane1.size() * sizeof(Vec3f), &BoundingBoxPlane1.front(), GL_STATIC_DRAW);
GLVertexArray::Ptr bBoxVertexArray = GLVertexArray::create();
volumeRayCastingProgram->setVertexAttribute("Vertex", bBoxVertexArray, boundingBoxArrayBuffer, 3, GL_FLOAT, false);
boundBoxProgram->setUniform("projectM", projectionMatrix);
boundBoxProgram->setUniform("modelview", modelViewMatrix);
boundBoxProgram->begin();
bBoxVertexArray->drawArrays(GL_LINE_LOOP, 4);
boundBoxProgram->end();
BoundingBoxPlane1.clear();
BoundingBoxPlane1.push_back(Vec3f(volumeScale.x, volumeScale.y, -volumeScale.z));
BoundingBoxPlane1.push_back(Vec3f(volumeScale.x, -volumeScale.y, -volumeScale.z));
BoundingBoxPlane1.push_back(Vec3f(-volumeScale.x, -volumeScale.y, -volumeScale.z));
BoundingBoxPlane1.push_back(Vec3f(-volumeScale.x, volumeScale.y, -volumeScale.z));
boundingBoxArrayBuffer->update(BoundingBoxPlane1.size() * sizeof(Vec3f), &BoundingBoxPlane1.front(), GL_STATIC_DRAW);
volumeRayCastingProgram->setVertexAttribute("Vertex", bBoxVertexArray, boundingBoxArrayBuffer, 3, GL_FLOAT, false);
boundBoxProgram->setUniform("projectM", projectionMatrix);
boundBoxProgram->setUniform("modelview", modelViewMatrix);
boundBoxProgram->begin();
bBoxVertexArray->drawArrays(GL_LINE_LOOP, 4);
boundBoxProgram->end();
BoundingBoxPlane1.clear();
BoundingBoxPlane1.push_back(Vec3f(-volumeScale.x, volumeScale.y, -volumeScale.z));
BoundingBoxPlane1.push_back(Vec3f(-volumeScale.x, -volumeScale.y, -volumeScale.z));
BoundingBoxPlane1.push_back(Vec3f(-volumeScale.x, -volumeScale.y, volumeScale.z));
BoundingBoxPlane1.push_back(Vec3f(-volumeScale.x, volumeScale.y, volumeScale.z));
boundingBoxArrayBuffer->update(BoundingBoxPlane1.size() * sizeof(Vec3f), &BoundingBoxPlane1.front(), GL_STATIC_DRAW);
volumeRayCastingProgram->setVertexAttribute("Vertex", bBoxVertexArray, boundingBoxArrayBuffer, 3, GL_FLOAT, false);
boundBoxProgram->setUniform("projectM", projectionMatrix);
boundBoxProgram->setUniform("modelview", modelViewMatrix);
boundBoxProgram->begin();
bBoxVertexArray->drawArrays(GL_LINE_LOOP, 4);
boundBoxProgram->end();
BoundingBoxPlane1.clear();
BoundingBoxPlane1.push_back(Vec3f(-volumeScale.x, volumeScale.y, volumeScale.z));
BoundingBoxPlane1.push_back(Vec3f(-volumeScale.x, -volumeScale.y, volumeScale.z));
BoundingBoxPlane1.push_back(Vec3f(volumeScale.x, -volumeScale.y, volumeScale.z));
BoundingBoxPlane1.push_back(Vec3f(volumeScale.x, volumeScale.y, volumeScale.z));
boundingBoxArrayBuffer->update(BoundingBoxPlane1.size() * sizeof(Vec3f), &BoundingBoxPlane1.front(), GL_STATIC_DRAW);
volumeRayCastingProgram->setVertexAttribute("Vertex", bBoxVertexArray, boundingBoxArrayBuffer, 3, GL_FLOAT, false);
boundBoxProgram->setUniform("projectM", projectionMatrix);
boundBoxProgram->setUniform("modelview", modelViewMatrix);
boundBoxProgram->begin();
bBoxVertexArray->drawArrays(GL_LINE_LOOP, 4);
boundBoxProgram->end();
}
uint32_t GatherForestBuilder::_SplitPoints(vector<GatherNode> &tree, vector<GatherPoint> &gps, vector<GatherPoint>::iterator start, vector<GatherPoint>::iterator end )
{
Range3f pBox(Vec3f(FLT_MAX, FLT_MAX, FLT_MAX), Vec3f(-FLT_MAX, -FLT_MAX, -FLT_MAX));
Range3f nBox(Vec3f(FLT_MAX, FLT_MAX, FLT_MAX), Vec3f(-FLT_MAX, -FLT_MAX, -FLT_MAX));
Conef cone;
vector<GatherPoint>::iterator it = start;
while (it != end)
{
pBox.Grow(it->position);
nBox.Grow(it->normal);
if (it == start)
cone = Conef(it->normal);
else
cone.Grow(it->normal);
it++;
}
GatherNode node(_tsample);
node.bbox = pBox;
node.cone = cone;
tree.push_back(node);
uint32_t curIdx = tree.size() - 1;
#ifdef _DEBUG
vector<GatherPoint> gp(start, end);
#endif // _DEBUG
if (end - start > 2)
{
Vec3f nbSize = nBox.GetSize();
Vec3f pbSize = pBox.GetSize();
bool splitType;
uint32_t splitDim;
if (pbSize.MaxComponent() > nbSize.MaxComponent())
{
splitType = true;
splitDim = pbSize.MaxComponentIndex();
}
else
{
splitType = false;
splitDim = nbSize.MaxComponentIndex();
}
vector<GatherPoint>::iterator splitPos = start + (end - start) / 2;
std::nth_element(start, splitPos, end, CompareNode<GatherPoint>(splitType, splitDim));
#ifdef _DEBUG
vector<GatherPoint> gp1(start, splitPos);
vector<GatherPoint> gp2(splitPos, end);
#endif // _DEBUG
uint32_t leftIdx = _SplitPoints(tree, gps, start, splitPos);
uint32_t rightIdx = _SplitPoints(tree, gps, splitPos, end);
GatherNode &curNode = tree[curIdx];
curNode.leftIdx = leftIdx;
curNode.rightIdx = rightIdx;
vector<uint32_t> &reps = curNode.reps;
vector<uint32_t> &lreps = tree[leftIdx].reps;
vector<uint32_t> &rreps = tree[rightIdx].reps;
for (uint32_t i = 0; i < reps.size(); i++)
{
if (lreps[i] != -1 && rreps[i] != -1)
{
//GatherPoint &p1 = gps[lreps[i]];
//GatherPoint &p2 = gps[reps[i]];
reps[i] = (__rand() > 0.5f) ? lreps[i] : rreps[i];
}
else
{
reps[i] = lreps[i] + 1 + rreps[i];
}
}
}
else
{
vector<GatherPoint>::iterator it = start;
while (it != end)
{
gps.push_back(*it);
tree[curIdx].reps[it->timeInst] = gps.size() - 1;
it++;
}
}
return curIdx;
}
Vec2f EquirectangularCamera::directionToUV(const Vec3f &wi, float &sinTheta) const
{
Vec3f wLocal = _invRot*wi;
sinTheta = std::sqrt(max(1.0f - wLocal.y()*wLocal.y(), 0.0f));
return Vec2f(std::atan2(wLocal.z(), wLocal.x())*INV_TWO_PI + 0.5f, 1.0f - std::acos(clamp(-wLocal.y(), -1.0f, 1.0f))*INV_PI);
}
void Figure::setBallVelocityAfterCollsision( Vec3f* vel,Vec3f center, float radius )
{
// TODO missing projection to center axis!!!!!!!!!!!!!!
// !!MUCH better shape dependent collision
//*vel = *vel * ((mass - this->mMass)/( mass + this->mMass)) + this->mVel * ((this->mMass*2)/( mass + this->mMass));
// get projection on center line (only if the collision occurs on the "side")
float len = (center - this->mPos).dot(this->mHalfAxe.safeNormalized());
if( len>(-this->mHalfAxe.length()) && len<this->mHalfAxe.length())
{
Vec3f ray = (this->mPos + this->mHalfAxe.safeNormalized()*len - center);
float penalty =ray.length();
float scale=((radius + this->mRadius)/penalty);//*((radius + this->mRadius)/penalty);
ray = ray.safeNormalized();
float l=ray.dot(this->mVel);
l = l>0?l:-l;
*vel = *vel - ray*(ray.dot(*vel))*2 - ray*l*scale;
}
else
{
if( len>(this->mHalfAxe.length()) )
{
Vec3f ray = this->mHalfAxe.safeNormalized();
float penalty =(center-this->mPos).length();
float scale=((radius + this->mHalfAxe.length())/penalty);//*((radius + this->mHalfAxe.length())/penalty);
*vel = *vel - ray*ray.dot(*vel)*2 + ray*(ray.dot(this->mVel))*scale;
}
else
{
Vec3f ray = - this->mHalfAxe.safeNormalized();
float penalty =(center-this->mPos).length();
float scale=((radius + this->mHalfAxe.length())/penalty);//*((radius + this->mHalfAxe.length())/penalty);
*vel = *vel - ray*ray.dot(*vel)*2 + ray*ray.dot(this->mVel)*scale;
}
}
}
void MyAppli::onExec () {
if (getClock("RequestTime").getElapsedTime().asSeconds() >= timeBtwnTwoReq.asSeconds()) {
std::string request = "GETCARPOS";
sf::Packet packet;
packet<<request;
Network::sendUdpPacket(packet);
getClock("RequestTime").restart();
received = false;
}
std::string response;
if (Network::getResponse("STOPCARMOVE", response)) {
std::vector<std::string> infos = split(response, "*");
int id = conversionStringInt(infos[0]);
Vec3f newPos (conversionStringFloat(infos[1]), conversionStringFloat(infos[2]), 0);
Caracter* caracter = static_cast<Caracter*>(World::getEntity(id));
Vec3f actualPos = Vec3f(caracter->getCenter().x, caracter->getCenter().y, 0);
if (hero->getId() == id) {
for (unsigned int i = 0; i < getRenderComponentManager().getNbComponents(); i++) {
View view = getRenderComponentManager().getRenderComponent(i)->getView();
Vec3f d = newPos - view.getPosition();
view.move(d.x, d.y, d.y);
getRenderComponentManager().getRenderComponent(i)->setView(view);
}
Vec3f d = newPos - getView().getPosition();
getView().move(d.x, d.y, d.y);
}
Vec3f d = newPos - actualPos;
World::moveEntity(caracter, d.x, d.y, d.y);
caracter->setMoving(false);
World::update();
}
if (Network::getResponse("MONSTERONMOUSE", response)) {
std::cout<<"monster on mouse!"<<std::endl;
}
if (Network::getResponse("NEWPATH", response)) {
std::vector<std::string> infos = split(response, "*");
std::vector<Vec2f> path;
int size = conversionStringInt(infos[0]);
int id = conversionStringInt(infos[1]);
Caracter* caracter = static_cast<Caracter*>(World::getEntity(id));
Vec2f actualPos (conversionStringFloat(infos[2]), conversionStringFloat(infos[3]));
Vec2f newPos = Computer::getPosOnPathFromTime(actualPos, caracter->getPath(),ping,caracter->getSpeed());
for (int i = 0; i < size; i++) {
path.push_back(Vec2f(conversionStringFloat(infos[i*2+4]), conversionStringFloat(infos[i*2+5])));
}
Vec2f d = newPos - actualPos;
Vec2f dir = d.normalize();
if (dir != caracter->getDir())
caracter->setDir(dir);
World::moveEntity(caracter, d.x, d.y, d.y);
caracter->setPath(path);
caracter->setMoving(true);
caracter->interpolation.first = caracter->getCenter();
caracter->interpolation.second = Computer::getPosOnPathFromTime(caracter->interpolation.first, caracter->getPath(),ping + timeBtwnTwoReq.asMicroseconds(),caracter->getSpeed());
caracter->getClkTransfertTime().restart();
}
if (Network::getResponse("NEWPOS", response)) {
std::vector<std::string> infos = split(response, "*");
if (infos.size() == 4) {
int id = conversionStringInt(infos[0]);
ping = conversionStringLong(infos[1]);
Caracter* caracter = static_cast<Caracter*>(World::getEntity(id));
Vec3f actualPos = Vec3f(caracter->getCenter().x, caracter->getCenter().y, 0);
Vec3f newPos (conversionStringFloat(infos[2]), conversionStringFloat(infos[3]), 0);
Vec3f d = newPos - actualPos;
if (id == hero->getId()) {
for (unsigned int i = 0; i < getRenderComponentManager().getNbComponents(); i++) {
View view = getRenderComponentManager().getRenderComponent(i)->getView();
view.move(d.x, d.y, d.y);
getRenderComponentManager().getRenderComponent(i)->setView(view);
}
getView().move (d.x, d.y, d.y);
}
World::moveEntity(caracter, d.x, d.y, d.y);
World::update();
caracter->interpolation.first = Vec3f(caracter->getCenter().x, caracter->getCenter().y, 0);
if (caracter->isMoving()) {
if (caracter->isMovingFromKeyboard()) {
caracter->interpolation.second = caracter->interpolation.first + Vec3f(caracter->getDir().x,caracter->getDir().y,0) * caracter->getSpeed() * (ping + timeBtwnTwoReq.asMicroseconds());
} else {
caracter->interpolation.second = Computer::getPosOnPathFromTime(caracter->interpolation.first, caracter->getPath(),ping + timeBtwnTwoReq.asMicroseconds(),caracter->getSpeed());
}
} else {
caracter->interpolation.second = caracter->interpolation.first;
}
caracter->getClkTransfertTime().restart();
}
} else {
std::vector<Entity*> caracters = World::getEntities("E_MONSTER+E_HERO");
for (unsigned int i = 0; i < caracters.size(); i++) {
Caracter* caracter = static_cast<Caracter*>(caracters[i]);
if (caracter->isMoving()) {
if (caracter->isMovingFromKeyboard()) {
Vec3f actualPos = Vec3f(caracter->getCenter().x, caracter->getCenter().y, 0);
sf::Int64 elapsedTime = caracter->getClkTransfertTime().getElapsedTime().asMicroseconds();
Vec3f newPos = caracter->interpolation.first + (caracter->interpolation.second - caracter->interpolation.first) * ((float) elapsedTime / (float) (ping + timeBtwnTwoReq.asMicroseconds()));
Ray ray(actualPos, newPos);
if (World::collide(caracter, ray)) {
newPos = actualPos;
}
for (unsigned int i = 0; i < getRenderComponentManager().getNbComponents(); i++) {
View view = getRenderComponentManager().getRenderComponent(i)->getView();
Vec3f d = newPos - view.getPosition();
view.move(d.x, d.y, d.y);
getRenderComponentManager().getRenderComponent(i)->setView(view);
}
Vec3f d = newPos - actualPos;
World::moveEntity(caracter, d.x, d.y, d.y);
getView().move(d.x, d.y, d.y);
World::update();
} else {
Vec3f actualPos (caracter->getCenter().x, caracter->getCenter().y, 0);
sf::Int64 elapsedTime = caracter->getClkTransfertTime().getElapsedTime().asMicroseconds();
Vec3f newPos = caracter->interpolation.first + (caracter->interpolation.second - caracter->interpolation.first) * ((float) elapsedTime / (float) (ping + timeBtwnTwoReq.asMicroseconds()));
Vec3f d = newPos - actualPos;
if (newPos.computeDist(caracter->getPath()[caracter->getPath().size() - 1]) <= PATH_ERROR_MARGIN) {
caracter->setMoving(false);
newPos = caracter->getPath()[caracter->getPath().size() - 1];
}
if (caracter->getId() == hero->getId()) {
for (unsigned int i = 0; i < getRenderComponentManager().getNbComponents(); i++) {
View view = getRenderComponentManager().getRenderComponent(i)->getView();
view.move(d.x, d.y, d.y);
getRenderComponentManager().getRenderComponent(i)->setView(view);
}
getView().move(d.x, d.y, d.y);
}
Vec2f dir = d.normalize();
if (dir != caracter->getDir())
caracter->setDir(dir);
World::moveEntity(caracter, d.x, d.y, d.y);
World::update();
}
}
}
}
if (hero->isInFightingMode()) {
if (hero->getFocusedCaracter() != nullptr) {
int distToEnnemi = hero->getCenter().computeDist(hero->getFocusedCaracter()->getCenter());
if (distToEnnemi <= hero->getRange()) {
if (hero->isMoving())
hero->setMoving(false);
hero->setAttacking(true);
hero->attackFocusedCaracter();
} else {
hero->setAttacking(false);
}
}
}
}
void Cone::Parameters(const Vec3f &p, std::pair< float, float > *param) const
{
// parametrize
Vec3f s = p - m_center;
float height = m_axisDir.dot(s);
float planex = s.dot(m_hcs[0].Data());
float planey = s.dot(m_hcs[1].Data());
float l = planex * planex + planey * planey;
if(l > 0)
{
planex /= l;
planey /= l;
}
float angle = std::atan2(planey, planex);
if(angle < 0)
angle += float(2 * M_PI);
/*Vec3f axisDiff = s - height * m_axisDir;
axisDiff.normalize();
float angle = m_angular.dot(axisDiff);
if(angle < -1) // clamp angle to [-1, 1]
angle = -1;
else if(angle > 1)
angle = 1;
if(m_angular.cross(axisDiff).dot(m_axisDir) < 0)
angle = std::acos(-angle) + M_PI;
else
angle = std::acos(angle);
// angle ok*/
// get length from height
//float length = height / std::cos(m_angle);
//param->first = length;
// this should be more precise than a division with std::cos:
// this is for two sided cone!
// distance to axis
float sqrS = s.sqrLength();
float f = sqrS - (height * height);
if(f <= 0)
f = 0;
else
f = std::sqrt(f);
float sdist = fabs(m_n2d[0] * f + ((height < 0)? -1 : 1) * m_n2d[1] * height);
float length = std::sqrt(sqrS + sdist * sdist);
param->first = /*(height < 0)? -length :*/ length;
param->second = angle;
/*// get normal for p
Vec3f pln = s.cross(m_axisDir);
Vec3f plx = m_axisDir.cross(pln);
Vec3f n;
if(plx.normalize() < 1.0e-6)
{
*param = std::make_pair(0.0f, angle);
return height;
}
if(height < 0)
n = m_normal[0] * plx - m_normalY;
else
n = m_normal[0] * plx + m_normalY;
Vec3f l = n.cross(pln);
l.normalize();
// make sure l points in direction of axis
if(m_axisDir.dot(l) < 0)
l *= -1;
// project p on line m_center + lambda * l
// get lambda
float lambda = s.dot(l);
// make sure l points in direction of axis
if(m_axisDir.dot(l) < 0)
{
if(lambda > 0)
{
*param = std::make_pair(s.length(), angle);
return height;
}
}
else if(lambda < 0)
{
*param = std::make_pair(s.length(), angle);
return height;
}
*param = std::make_pair(*fabs(lambda), angle);*/
}
bool Cone::Init(const Vec3f &p1, const Vec3f &p2, const Vec3f &p3,
const Vec3f &n1, const Vec3f &n2, const Vec3f &n3)
{
//float ncheck = std::max(n2.dot(n3), std::max(n1.dot(n2), n1.dot(n3)));
//if(ncheck > 0.999)
// return false;
// compute center by intersecting the three planes given by (p1, n1)
// (p2, n2) and (p3, n3)
// set up linear system
double a[4 * 3];
double d1 = p1.dot(n1);
double d2 = p2.dot(n2);
double d3 = p3.dot(n3);
// column major
a[0 + 0 * 3] = n1[0];
a[1 + 0 * 3] = n2[0];
a[2 + 0 * 3] = n3[0];
a[0 + 1 * 3] = n1[1];
a[1 + 1 * 3] = n2[1];
a[2 + 1 * 3] = n3[1];
a[0 + 2 * 3] = n1[2];
a[1 + 2 * 3] = n2[2];
a[2 + 2 * 3] = n3[2];
a[0 + 3 * 3] = d1;
a[1 + 3 * 3] = d2;
a[2 + 3 * 3] = d3;
if(dmat_solve(3, 1, a))
return false;
m_center[0] = a[0 + 3 * 3];
m_center[1] = a[1 + 3 * 3];
m_center[2] = a[2 + 3 * 3];
// compute axisDir
Vec3f s1 = p1 - m_center;
Vec3f s2 = p2 - m_center;
Vec3f s3 = p3 - m_center;
s1.normalize();
s2.normalize();
s3.normalize();
Plane pl(s1 + m_center, s2 + m_center, s3 + m_center);
m_axisDir = pl.getNormal();
// make sure axis points in direction of s1
// this defines the side of the cone!!!
if(m_axisDir.dot(s1) < 0)
m_axisDir *= -1;
m_angle = 0;
float angle = m_axisDir.dot(n1);
if(angle < -1) // clamp angle to [-1, 1]
angle = -1;
else if(angle > 1)
angle = 1;
if(angle < 0)
// m_angle = omega + 90
angle = std::acos(angle) - float(M_PI) / 2;
else
// m_angle = 90 - omega
angle = float(M_PI) / 2 - std::acos(angle);
m_angle += angle;
angle = m_axisDir.dot(n2);
if(angle < -1) // clamp angle to [-1, 1]
angle = -1;
else if(angle > 1)
angle = 1;
if(angle < 0)
// m_angle = omega + 90
angle = std::acos(angle) - float(M_PI) / 2;
else
// m_angle = 90 - omega
angle = float(M_PI) / 2 - std::acos(angle);
m_angle += angle;
angle = m_axisDir.dot(n3);
if(angle < -1) // clamp angle to [-1, 1]
angle = -1;
else if(angle > 1)
angle = 1;
if(angle < 0)
// m_angle = omega + 90
angle = std::acos(angle) - float(M_PI) / 2;
else
// m_angle = 90 - omega
angle = float(M_PI) / 2 - std::acos(angle);
m_angle += angle;
m_angle /= 3;
if(m_angle < 1.0e-6 || m_angle > float(M_PI) / 2 - 1.0e-6)
return false;
//if(m_angle > 1.3962634015954636615389526147909) // 80 degrees
if(m_angle > 1.4835298641951801403851371532153f) // 85 degrees
return false;
m_normal = Vec3f(std::cos(-m_angle), std::sin(-m_angle), 0);
m_normalY = m_normal[1] * m_axisDir;
m_n2d[0] = std::cos(m_angle);
m_n2d[1] = -std::sin(m_angle);
m_hcs.FromNormal(m_axisDir);
m_angularRotatedRadians = 0;
return true;
}
void DVRClipGeometry::linkContour( DVRTriangle *startTriangle,
Real32 dist2RefPlane,
const Vec3f &viewDir,
bool positiveWinding)
{
FDEBUG(("DVRClipGeometry - linkcontour dist = %f\n", dist2RefPlane));
bool closed = false;
// first, we have to check for the correct winding direction.
Pnt3f vertex[2];
bool firstEdge;
int first = 0, second = 0;
if(startTriangle->edgeCut[0] && startTriangle->edgeCut[1])
{
vertex[0] = interpolate(startTriangle, 1, 0, dist2RefPlane);
vertex[1] = interpolate(startTriangle, 1, 2, dist2RefPlane);
first = 0; second = 1;
}
else if (startTriangle->edgeCut[1] && startTriangle->edgeCut[2])
{
vertex[0] = interpolate(startTriangle, 2, 1, dist2RefPlane);
vertex[1] = interpolate(startTriangle, 2, 0, dist2RefPlane);
first = 1; second = 2;
}
else if (startTriangle->edgeCut[0] && startTriangle->edgeCut[2])
{
vertex[0] = interpolate(startTriangle, 0, 1, dist2RefPlane);
vertex[1] = interpolate(startTriangle, 0, 2, dist2RefPlane);
first = 0; second = 2;
}
// Now we should have both cut points on our edges.
// If the cross product of the normal of this triangle with the
// vector between the two cut points (cutPoint[1] - cutPoint[0])
// has a positive dot product with the viewing direction, then
// the edge with cutPoint[0] on it is the right direction, otherwise
// we would have to choose the other direction.
Vec3f tmp = vertex[1] - vertex[0];
tmp = tmp.cross(startTriangle->transformedNormal);
if(tmp.dot(viewDir) <= 0.0)
{
firstEdge = false;
}else
{
firstEdge = true;
}
if(!positiveWinding)
firstEdge = !firstEdge;
DVRTriangle *current = startTriangle;
current->inContour = true;
if(firstEdge)
{
current->cutPnt = vertex[0];
current->cutPoint[0] = vertex[0][0];
current->cutPoint[1] = vertex[0][1];
current->cutPoint[2] = vertex[0][2];
current->contourNeighbour = &_mfTriangles[current->neighbours[first]];
// // debugging -> remove
// if(!current->contourNeighbour){
// std::cerr<<"contour neighbour is NULL\n";
// exit(0);
// }
current = current->contourNeighbour;
}
else
{
current->cutPnt = vertex[1];
current->cutPoint[0] = vertex[1][0];
current->cutPoint[1] = vertex[1][1];
current->cutPoint[2] = vertex[1][2];
current->contourNeighbour = &_mfTriangles[current->neighbours[second]];
// // debugging -> remove
// if(!current->contourNeighbour){
// std::cerr<<"contour neighbour is NULL\n";
// exit(0);
// }
current = current->contourNeighbour;
}
//check neighbours
while(!closed)
{
closed = true;
current->inContour = true;
for(UInt32 i = 0; i < 3; i++)
{
// if a neighbour triangle is in the active triangle list and
// not yet in a contour it is our new contour neighbour.
if( current->edgeCut[i] &&
!_mfTriangles[current->neighbours[i]].inContour)
{
// calculate cut point
current->cutPnt = interpolate(current,
i,
(i + 1) % 3,
dist2RefPlane);
current->cutPoint[0] = current->cutPnt[0];
current->cutPoint[1] = current->cutPnt[1];
current->cutPoint[2] = current->cutPnt[2];
current->contourNeighbour =
&_mfTriangles[current->neighbours[i]];
// // debugging -> remove
// if(!current->contourNeighbour){
// std::cerr<<"contour neighbour is NULL\n";
// exit(0);
// }
current = current->contourNeighbour;
closed = false;
break;
}// !inContour
} // end for neighbours
} // end while !closed
for(UInt32 i = 0; i < 3; i++)
{
if(&_mfTriangles[current->neighbours[i]] == startTriangle)
{
current->cutPnt = interpolate(current,
i,
(i + 1) % 3,
dist2RefPlane);
current->cutPoint[0] = current->cutPnt[0];
current->cutPoint[1] = current->cutPnt[1];
current->cutPoint[2] = current->cutPnt[2];
// now the ring is closed.
current->contourNeighbour = startTriangle;
// // debugging -> remove
// if(!current->contourNeighbour){
// std::cerr<<"contour neighbour is NULL\n";
// exit(0);
// }
break;
}
} // end for neighbours
// // debugging -> remove
// if(!current->contourNeighbour){
// std::cerr <<"contour could not closed\n";
// std::cerr <<current->edgeCut[0]<<current->edgeCut[1]
// <<current->edgeCut[2]<<std::endl;
// exit(0);
// }
}
void TriangleDistance::segPoints(const Vec3f& P, const Vec3f& A, const Vec3f& Q, const Vec3f& B,
Vec3f& VEC, Vec3f& X, Vec3f& Y)
{
Vec3f T;
BVH_REAL A_dot_A, B_dot_B, A_dot_B, A_dot_T, B_dot_T;
Vec3f TMP;
T = Q - P;
A_dot_A = A.dot(A);
B_dot_B = B.dot(B);
A_dot_B = A.dot(B);
A_dot_T = A.dot(T);
B_dot_T = B.dot(T);
// t parameterizes ray P,A
// u parameterizes ray Q,B
BVH_REAL t, u;
// compute t for the closest point on ray P,A to
// ray Q,B
BVH_REAL denom = A_dot_A*B_dot_B - A_dot_B*A_dot_B;
t = (A_dot_T*B_dot_B - B_dot_T*A_dot_B) / denom;
// clamp result so t is on the segment P,A
if((t < 0) || isnan(t)) t = 0; else if(t > 1) t = 1;
// find u for point on ray Q,B closest to point at t
u = (t*A_dot_B - B_dot_T) / B_dot_B;
// if u is on segment Q,B, t and u correspond to
// closest points, otherwise, clamp u, recompute and
// clamp t
if((u <= 0) || isnan(u))
{
Y = Q;
t = A_dot_T / A_dot_A;
if((t <= 0) || isnan(t))
{
X = P;
VEC = Q - P;
}
else if(t >= 1)
{
X = P + A;
VEC = Q - X;
}
else
{
X = P + A * t;
TMP = T.cross(A);
VEC = A.cross(TMP);
}
}
else if (u >= 1)
{
Y = Q + B;
t = (A_dot_B + A_dot_T) / A_dot_A;
if((t <= 0) || isnan(t))
{
X = P;
VEC = Y - P;
}
else if(t >= 1)
{
X = P + A;
VEC = Y - X;
}
else
{
X = P + A * t;
T = Y - P;
TMP = T.cross(A);
VEC= A.cross(TMP);
}
}
else
{
Y = Q + B * u;
if((t <= 0) || isnan(t))
{
X = P;
TMP = T.cross(B);
VEC = B.cross(TMP);
}
else if(t >= 1)
{
X = P + A;
T = Q - X;
TMP = T.cross(B);
VEC = B.cross(TMP);
}
else
{
X = P + A * t;
VEC = A.cross(B);
if(VEC.dot(T) < 0)
{
VEC = VEC * (-1);
}
}
}
}
//////////////////////////////////////////////////////////////////////////////////////////
// Detects all collisions of various types with each object.
void PerformCollisionDetection(GameRoom* room, GamePlayer* player, double dt, float xScale, float yScale, float zNear, float zFar){
dt/=1000;
vector<GameObject*> Objects = room->GetGameObjects();
PSystems* ps;// = Render::gameState->GetParticleSystems();
list<Projectile*>* projs = ps->GetBullets();
/////////////////////////////////////////////////////////////////////////////
// PARTICLE COLLISION DETECTION
/////////////////////////////////////////////////////////////////////////////
for(unsigned int i = 0; i<Objects.size(); i++){
GameObject* o = Objects[i];
if(o->CollisionTierNum<1) continue;
vector<Vec3f> aBox = o->boundingBox;
Vector3f pos = o->GetPosition();
Vec3f p(pos.x(), pos.y(), pos.z());
Vec4f r = o->GetRotation();
Vec4f wv = o->angularVelocity;
Vec3f v = o->velocity;
UpdateCoords(aBox, p, v, r, wv, 0, 1.f, true, 1.0, xScale, yScale, zNear, zFar);
for(list<Projectile*>::iterator it = projs->begin(); it != projs->end(); ++it){
vector<Vec3f> bBox = (*(*it)).boundingBox;
Vector3f pos2 = (*(*it)).getPosition();
Vec3f p2(pos.x(), pos.y(), pos.z());
Vec4f r2(0,0,1,0);
Vec4f wv2(1,0,0,0);
Vector3f vel2 = (*(*it)).getVelocity();
Vec3f v2(vel2.x(), vel2.y(), vel2.z());
UpdateCoords(bBox, p2, v2, r2, wv2, 0, 10.f, true, 1.0, xScale, yScale, zNear, zFar);
Simplex P;
Vec3f V;
V = GJKDistance(aBox, bBox, P);
if(P.GetSize() > 3 || V.norm() < tolerance){ //We have a collision
Vec3f contact = ResolveContact(P);
o->collidedProjectiles.push_back((*it));
CollisionData o1D, o2D;
o1D.pointOfContact = contact;
o1D.contactNormal = V.normalized();
o->projectileCollisionData[(*it)]=o1D;
(*it)->drawCollision = true;
}
}
}
////////////////////////////////////////////////////////////////////////////
// SPECIAL DATA FOR MAIN CHARACTER ONLY
////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////
// Tier 0 collision
//////////////////////////////////////////////////
for(unsigned int a = 0; a<Objects.size()-1; a++){
GameObject* o1 = Objects[a];
if(o1->CollisionTierNum < 1 )continue;
vector<Vec3f> aBox = o1->boundingBox;
Vector3f pos = o1->GetPosition();
Vec3f position = Vec3f(pos.x(), pos.y(), pos.z());
Vec4f rotation = o1->GetRotation();
Vec3f vel = o1->velocity;
Vec4f wvel = o1->angularVelocity;
UpdateCoords(aBox, position, vel, rotation, wvel ,dt, o1->outSideCollisionScale, true, 1.f, xScale, yScale, zNear, zFar);
vector<Vec3f> oldABox = o1->boundingBox;
UpdateCoords(oldABox, position, vel, rotation, wvel,0, o1->outSideCollisionScale, true, 1.f, xScale, yScale, zNear, zFar);
for(unsigned int i = 0; i <oldABox.size(); i++){
aBox.push_back(oldABox[i]);
}
for(unsigned int b = a+1; b<Objects.size(); b++){
GameObject* o2 = Objects[b];
if(o1->objType == WORLD_OBJECT_TYPE && o2->objType == WORLD_OBJECT_TYPE || o2->CollisionTierNum < 1){
continue;
}
vector<Vec3f> bBox = o2->boundingBox;
pos= o2->GetPosition();
position = Vec3f(pos.x(), pos.y(), pos.z());
vel = o2->velocity;
rotation = o2->GetRotation();
wvel = o2->angularVelocity;
UpdateCoords(bBox, position, vel, rotation, wvel,dt, o2->outSideCollisionScale, true, 1.f, xScale, yScale, zNear, zFar);
vector<Vec3f> oldBBox = o2->boundingBox;
UpdateCoords(oldBBox, position, vel, rotation, wvel,0, o2->outSideCollisionScale,true, 1.f, xScale, yScale, zNear, zFar);
for(unsigned int i = 0; i <oldBBox.size(); i++){
bBox.push_back(oldBBox[i]);
}
Simplex P;
Vec3f V;
V = GJKDistance(aBox, bBox, P);
if(V.norm() < tolerance){ //We have a collision
if(!AlreadyIn(o1, o2, 0, room)){
Vec3f contact = ResolveContact(P);
room->collisionTier0List[o1].push_back(o2);
room->collisionTier0List[o2].push_back(o1);
CollisionData o1D, o2D;
o1D.pointOfContact = contact;
o2D.pointOfContact = contact;
o1D.contactNormal = V.normalized();
o2D.contactNormal = -V.normalized();
o1->tier0CollisionData[o2] = o1D;
o2->tier0CollisionData[o1] = o2D;
}
}
}
}
////////////////////////////////////////////////
// Tier 1 collision
///////////////////////////////////////////////
map<GameObject*, vector<GameObject*> >::iterator it = room->collisionTier0List.begin();
while(it!=room->collisionTier0List.end()){
GameObject* o1 = it->first;
if(o1->CollisionTierNum<2)
continue;
Vector3f pos = o1->GetPosition();
Vec3f position(pos.x(), pos.y(), pos.z());
Vec4f rotation = o1->GetRotation();
Vec3f vel = o1->velocity;
Vec4f wvel = o1->angularVelocity;
vector<Vec3f> aBox = o1->boundingBox;
UpdateCoords(aBox,position, vel, rotation, wvel ,0, 1.0,true, 1.f, xScale, yScale, zNear, zFar);
// vector<Vec3f> oldABox = o1->boundingBox;
// UpdateCoords(aBox,position, vel, rotation, wvel ,0, 1.0,true, 1.f, xScale, yScale, zNear, zFar);
// for(unsigned int i = 0; i <oldABox.size(); i++){
// aBox.push_back(oldABox[i]);
// }
vector<GameObject*> Bs = it->second;
it++;
for(unsigned int b = 0; b<Bs.size(); b++){
GameObject* o2 = Bs[b];
if(o2->CollisionTierNum<2)
continue;
vector<Vec3f> bBox = o2->boundingBox;
//pos = o2->GetPosition();
position = Vec3f(pos.x(), pos.y(), pos.z());
vel = o2->velocity;
wvel = o2->angularVelocity;
rotation = o2->GetRotation();
UpdateCoords(bBox, position, vel, rotation, wvel ,0,1.0, true, 1.f, xScale, yScale, zNear, zFar);
vector<Vec3f> oldBBox = o2->boundingBox;
//UpdateCoords(oldBBox,position, vel, rotation, wvel ,0, 1.0, true, 1.f, xScale, yScale, zNear, zFar);
//for(unsigned int i = 0; i <oldBBox.size(); i++){
// bBox.push_back(oldBBox[i]);
//}
Simplex P;
Vec3f V;
V = GJKDistance(aBox, bBox, P);
if(V.norm() < tolerance){ //We have a collision.
if(!AlreadyIn(o1, o2, 1, room)) {
if(o1->objType == ACTIVE_OBJECT_TYPE && o2->objType == ACTIVE_OBJECT_TYPE){
o1->drawCollision = true;
o2->drawCollision = true;
}
Vec3f contact = ResolveContact(P);
room->collisionTier1List[o1].push_back(o2);
room->collisionTier1List[o2].push_back(o1);
CollisionData o1D, o2D;
o1D.pointOfContact = contact;
o2D.pointOfContact = contact;
o1D.contactNormal = V.normalized();
o2D.contactNormal = -V.normalized();
o1->tier1CollisionData[o2] = o1D;
o2->tier1CollisionData[o1] = o2D;
}
}
}
}
//////////////////////////////////////////////////////////////////////////////
//
// Mesh Level Detection
//
/////////////////////////////////////////////////////////////////////////////
}
BVH_REAL TriangleDistance::triDistance(const Vec3f S[3], const Vec3f T[3], Vec3f& P, Vec3f& Q)
{
// Compute vectors along the 6 sides
Vec3f Sv[3];
Vec3f Tv[3];
Vec3f VEC;
Sv[0] = S[1] - S[0];
Sv[1] = S[2] - S[1];
Sv[2] = S[0] - S[2];
Tv[0] = T[1] - T[0];
Tv[1] = T[2] - T[1];
Tv[2] = T[0] - T[2];
// For each edge pair, the vector connecting the closest points
// of the edges defines a slab (parallel planes at head and tail
// enclose the slab). If we can show that the off-edge vertex of
// each triangle is outside of the slab, then the closest points
// of the edges are the closest points for the triangles.
// Even if these tests fail, it may be helpful to know the closest
// points found, and whether the triangles were shown disjoint
Vec3f V, Z, minP, minQ;
BVH_REAL mindd;
int shown_disjoint = 0;
mindd = (S[0] - T[0]).sqrLength() + 1; // Set first minimum safely high
for(int i = 0; i < 3; ++i)
{
for(int j = 0; j < 3; ++j)
{
// Find closest points on edges i & j, plus the
// vector (and distance squared) between these points
segPoints(S[i], Sv[i], T[j], Tv[j], VEC, P, Q);
V = Q - P;
BVH_REAL dd = V.dot(V);
// Verify this closest point pair only if the distance
// squared is less than the minimum found thus far.
if(dd <= mindd)
{
minP = P;
minQ = Q;
mindd = dd;
Z = S[(i+2)%3] - P;
BVH_REAL a = Z.dot(VEC);
Z = T[(j+2)%3] - Q;
BVH_REAL b = Z.dot(VEC);
if((a <= 0) && (b >= 0)) return sqrt(dd);
BVH_REAL p = V.dot(VEC);
if(a < 0) a = 0;
if(b > 0) b = 0;
if((p - a + b) > 0) shown_disjoint = 1;
}
}
}
// No edge pairs contained the closest points.
// either:
// 1. one of the closest points is a vertex, and the
// other point is interior to a face.
// 2. the triangles are overlapping.
// 3. an edge of one triangle is parallel to the other's face. If
// cases 1 and 2 are not true, then the closest points from the 9
// edge pairs checks above can be taken as closest points for the
// triangles.
// 4. possibly, the triangles were degenerate. When the
// triangle points are nearly colinear or coincident, one
// of above tests might fail even though the edges tested
// contain the closest points.
// First check for case 1
Vec3f Sn;
BVH_REAL Snl;
Sn = Sv[0].cross(Sv[1]); // Compute normal to S triangle
Snl = Sn.dot(Sn); // Compute square of length of normal
// If cross product is long enough,
if(Snl > 1e-15)
{
// Get projection lengths of T points
Vec3f Tp;
V = S[0] - T[0];
Tp[0] = V.dot(Sn);
V = S[0] - T[1];
Tp[1] = V.dot(Sn);
V = S[0] - T[2];
Tp[2] = V.dot(Sn);
// If Sn is a separating direction,
// find point with smallest projection
int point = -1;
if((Tp[0] > 0) && (Tp[1] > 0) && (Tp[2] > 0))
{
if(Tp[0] < Tp[1]) point = 0; else point = 1;
if(Tp[2] < Tp[point]) point = 2;
}
else if((Tp[0] < 0) && (Tp[1] < 0) && (Tp[2] < 0))
{
if(Tp[0] > Tp[1]) point = 0; else point = 1;
if(Tp[2] > Tp[point]) point = 2;
}
// If Sn is a separating direction,
if(point >= 0)
{
shown_disjoint = 1;
// Test whether the point found, when projected onto the
// other triangle, lies within the face.
V = T[point] - S[0];
Z = Sn.cross(Sv[0]);
if(V.dot(Z) > 0)
{
V = T[point] - S[1];
Z = Sn.cross(Sv[1]);
if(V.dot(Z) > 0)
{
V = T[point] - S[2];
Z = Sn.cross(Sv[2]);
if(V.dot(Z) > 0)
{
// T[point] passed the test - it's a closest point for
// the T triangle; the other point is on the face of S
P = T[point] + Sn * (Tp[point] / Snl);
Q = T[point];
return (P - Q).length();
}
}
}
}
}
Vec3f Tn;
BVH_REAL Tnl;
Tn = Tv[0].cross(Tv[1]);
Tnl = Tn.dot(Tn);
if(Tnl > 1e-15)
{
Vec3f Sp;
V = T[0] - S[0];
Sp[0] = V.dot(Tn);
V = T[0] - S[1];
Sp[1] = V.dot(Tn);
V = T[0] - S[2];
Sp[2] = V.dot(Tn);
int point = -1;
if((Sp[0] > 0) && (Sp[1] > 0) && (Sp[2] > 0))
{
if(Sp[0] < Sp[1]) point = 0; else point = 1;
if(Sp[2] < Sp[point]) point = 2;
}
else if((Sp[0] < 0) && (Sp[1] < 0) && (Sp[2] < 0))
{
if(Sp[0] > Sp[1]) point = 0; else point = 1;
if(Sp[2] > Sp[point]) point = 2;
}
if(point >= 0)
{
shown_disjoint = 1;
V = S[point] - T[0];
Z = Tn.cross(Tv[0]);
if(V.dot(Z) > 0)
{
V = S[point] - T[1];
Z = Tn.cross(Tv[1]);
if(V.dot(Z) > 0)
{
V = S[point] - T[2];
Z = Tn.cross(Tv[2]);
if(V.dot(Z) > 0)
{
P = S[point];
Q = S[point] + Tn * (Sp[point] / Tnl);
return (P - Q).length();
}
}
}
}
}
// Case 1 can't be shown.
// If one of these tests showed the triangles disjoint,
// we assume case 3 or 4, otherwise we conclude case 2,
// that the triangles overlap.
if(shown_disjoint)
{
P = minP;
Q = minQ;
return sqrt(mindd);
}
else return 0;
}
static void process8uC3( const Mat& image, Mat& fgmask, double learningRate,
Mat& bgmodel, int nmixtures, double backgroundRatio,
double varThreshold, double noiseSigma )
{
int x, y, k, k1, rows = image.rows, cols = image.cols;
float alpha = (float)learningRate, T = (float)backgroundRatio, vT = (float)varThreshold;
int K = nmixtures;
const float w0 = (float)defaultInitialWeight;
const float sk0 = (float)(w0/(defaultNoiseSigma*2*sqrt(3.)));
const float var0 = (float)(defaultNoiseSigma*defaultNoiseSigma*4);
const float minVar = (float)(noiseSigma*noiseSigma);
MixData<Vec3f>* mptr = (MixData<Vec3f>*)bgmodel.data;
for( y = 0; y < rows; y++ )
{
const uchar* src = image.ptr<uchar>(y);
uchar* dst = fgmask.ptr<uchar>(y);
if( alpha > 0 )
{
for( x = 0; x < cols; x++, mptr += K )
{
float wsum = 0;
Vec3f pix(src[x*3], src[x*3+1], src[x*3+2]);
int kHit = -1, kForeground = -1;
for( k = 0; k < K; k++ )
{
float w = mptr[k].weight;
wsum += w;
if( w < FLT_EPSILON )
break;
Vec3f mu = mptr[k].mean;
Vec3f var = mptr[k].var;
Vec3f diff = pix - mu;
float d2 = diff.dot(diff);
if( d2 < vT*(var[0] + var[1] + var[2]) )
{
wsum -= w;
float dw = alpha*(1.f - w);
mptr[k].weight = w + dw;
mptr[k].mean = mu + alpha*diff;
var = Vec3f(max(var[0] + alpha*(diff[0]*diff[0] - var[0]), minVar),
max(var[1] + alpha*(diff[1]*diff[1] - var[1]), minVar),
max(var[2] + alpha*(diff[2]*diff[2] - var[2]), minVar));
mptr[k].var = var;
mptr[k].sortKey = w/sqrt(var[0] + var[1] + var[2]);
for( k1 = k-1; k1 >= 0; k1-- )
{
if( mptr[k1].sortKey >= mptr[k1+1].sortKey )
break;
std::swap( mptr[k1], mptr[k1+1] );
}
kHit = k1+1;
break;
}
}
if( kHit < 0 ) // no appropriate gaussian mixture found at all, remove the weakest mixture and create a new one
{
kHit = k = min(k, K-1);
wsum += w0 - mptr[k].weight;
mptr[k].weight = w0;
mptr[k].mean = pix;
mptr[k].var = Vec3f(var0, var0, var0);
mptr[k].sortKey = sk0;
}
else
for( ; k < K; k++ )
wsum += mptr[k].weight;
float wscale = 1.f/wsum;
wsum = 0;
for( k = 0; k < K; k++ )
{
wsum += mptr[k].weight *= wscale;
mptr[k].sortKey *= wscale;
if( wsum > T && kForeground < 0 )
kForeground = k+1;
}
dst[x] = (uchar)(-(kHit >= kForeground));
}
}
else
{
for( x = 0; x < cols; x++, mptr += K )
{
Vec3f pix(src[x*3], src[x*3+1], src[x*3+2]);
int kHit = -1, kForeground = -1;
for( k = 0; k < K; k++ )
{
if( mptr[k].weight < FLT_EPSILON )
break;
Vec3f mu = mptr[k].mean;
Vec3f var = mptr[k].var;
Vec3f diff = pix - mu;
float d2 = diff.dot(diff);
if( d2 < vT*(var[0] + var[1] + var[2]) )
{
kHit = k;
break;
}
}
if( kHit >= 0 )
{
float wsum = 0;
for( k = 0; k < K; k++ )
{
wsum += mptr[k].weight;
if( wsum > T )
{
kForeground = k+1;
break;
}
}
}
dst[x] = (uchar)(kHit < 0 || kHit >= kForeground ? 255 : 0);
}
}
}
}
float Vec3f::operator*(const Vec3f& other) const
{
return X()*other.X()+Y()*other.Y()+Z()*other.Z();
}
void Molecule3dConstraintsChecker::_cache (int idx)
{
if (_cache_v.find(idx) || _cache_l.find(idx) || _cache_p.find(idx))
return;
const MC::Base &base = _constraints.at(idx);
switch (base.type)
{
case MC::POINT_ATOM:
{
int atom_idx = ((const Molecule3dConstraints::PointByAtom &)base).atom_idx;
_cache_v.insert(idx, _target->getAtomXyz(_mapping[atom_idx]));
break;
}
case MC::POINT_DISTANCE:
{
const MC::PointByDistance &constr = (const MC::PointByDistance &)base;
_cache(constr.beg_id);
_cache(constr.end_id);
const Vec3f &beg = _cache_v.at(constr.beg_id);
const Vec3f &end = _cache_v.at(constr.end_id);
Vec3f dir;
dir.diff(end, beg);
if (!dir.normalize())
throw Error("point-by-distance: degenerate case");
Vec3f res;
res.lineCombin(beg, dir, constr.distance);
_cache_v.insert(idx, res);
break;
}
case MC::POINT_PERCENTAGE:
{
const MC::PointByPercentage &constr = (const MC::PointByPercentage &)base;
_cache(constr.beg_id);
_cache(constr.end_id);
const Vec3f &beg = _cache_v.at(constr.beg_id);
const Vec3f &end = _cache_v.at(constr.end_id);
Vec3f dir;
dir.diff(end, beg);
if (!dir.normalize())
throw Error("point-by-percentage: degenerate case");
Vec3f res;
res.lineCombin2(beg, 1.f - constr.percentage, end, constr.percentage);
_cache_v.insert(idx, res);
break;
}
case MC::POINT_NORMALE:
{
const MC::PointByNormale &constr = (const MC::PointByNormale &)base;
_cache(constr.org_id);
_cache(constr.norm_id);
const Vec3f &org = _cache_v.at(constr.org_id);
const Line3f &norm = _cache_l.at(constr.norm_id);
Vec3f res;
res.lineCombin(org, norm.dir, constr.distance);
_cache_v.insert(idx, res);
break;
}
case MC::POINT_CENTROID:
{
const MC::Centroid &constr = (const MC::Centroid &)base;
Vec3f res;
if (constr.point_ids.size() < 1)
throw Error("centroid: have %d points", constr.point_ids.size());
for (int i = 0; i < constr.point_ids.size(); i++)
{
_cache(constr.point_ids[i]);
const Vec3f &pt = _cache_v.at(constr.point_ids[i]);
res.add(pt);
}
res.scale(1.f / constr.point_ids.size());
_cache_v.insert(idx, res);
break;
}
case MC::LINE_NORMALE:
{
const MC::Normale &constr = (const MC::Normale &)base;
_cache(constr.plane_id);
_cache(constr.point_id);
const Plane3f &plane = _cache_p.at(constr.plane_id);
const Vec3f &point = _cache_v.at(constr.point_id);
Vec3f projection;
Line3f res;
plane.projection(point, projection);
res.dir.copy(plane.getNorm());
res.org.copy(projection);
_cache_l.insert(idx, res);
break;
}
case MC::LINE_BEST_FIT:
{
const MC::BestFitLine &constr = (const MC::BestFitLine &)base;
if (constr.point_ids.size() < 2)
throw Error("best fit line: only %d points", constr.point_ids.size());
QS_DEF(Array<Vec3f>, points);
points.clear();
for (int i = 0; i < constr.point_ids.size(); i++)
{
_cache(constr.point_ids[i]);
points.push(_cache_v.at(constr.point_ids[i]));
}
Line3f res;
res.bestFit(points.size(), points.ptr(), 0);
_cache_l.insert(idx, res);
break;
}
case MC::PLANE_BEST_FIT:
{
const MC::BestFitPlane &constr = (const MC::BestFitPlane &)base;
if (constr.point_ids.size() < 3)
throw Error("best fit line: only %d points", constr.point_ids.size());
QS_DEF(Array<Vec3f>, points);
points.clear();
for (int i = 0; i < constr.point_ids.size(); i++)
{
_cache(constr.point_ids[i]);
points.push(_cache_v.at(constr.point_ids[i]));
}
Plane3f res;
res.bestFit(points.size(), points.ptr(), 0);
_cache_p.insert(idx, res);
break;
}
case MC::PLANE_POINT_LINE:
{
const MC::PlaneByPoint &constr = (const MC::PlaneByPoint &)base;
_cache(constr.point_id);
_cache(constr.line_id);
const Vec3f &point = _cache_v.at(constr.point_id);
const Line3f &line = _cache_l.at(constr.line_id);
Plane3f res;
res.byPointAndLine(point, line);
_cache_p.insert(idx, res);
break;
}
default:
throw Error("unknown constraint type %d", base.type);
}
}
void qRansacSD::doAction()
{
assert(m_app);
if (!m_app)
return;
const ccHObject::Container& selectedEntities = m_app->getSelectedEntities();
size_t selNum = selectedEntities.size();
if (selNum!=1)
{
m_app->dispToConsole("Select only one cloud!",ccMainAppInterface::ERR_CONSOLE_MESSAGE);
return;
}
ccHObject* ent = selectedEntities[0];
assert(ent);
if (!ent || !ent->isA(CC_TYPES::POINT_CLOUD))
{
m_app->dispToConsole("Select a real point cloud!",ccMainAppInterface::ERR_CONSOLE_MESSAGE);
return;
}
ccPointCloud* pc = static_cast<ccPointCloud*>(ent);
//input cloud
unsigned count = pc->size();
bool hasNorms = pc->hasNormals();
CCVector3 bbMin, bbMax;
pc->getBoundingBox(bbMin,bbMax);
const CCVector3d& globalShift = pc->getGlobalShift();
double globalScale = pc->getGlobalScale();
//Convert CC point cloud to RANSAC_SD type
PointCloud cloud;
{
try
{
cloud.reserve(count);
}
catch(...)
{
m_app->dispToConsole("Not enough memory!",ccMainAppInterface::ERR_CONSOLE_MESSAGE);
return;
}
//default point & normal
Point Pt;
Pt.normal[0] = 0.0;
Pt.normal[1] = 0.0;
Pt.normal[2] = 0.0;
for (unsigned i=0; i<count; ++i)
{
const CCVector3* P = pc->getPoint(i);
Pt.pos[0] = static_cast<float>(P->x);
Pt.pos[1] = static_cast<float>(P->y);
Pt.pos[2] = static_cast<float>(P->z);
if (hasNorms)
{
const CCVector3& N = pc->getPointNormal(i);
Pt.normal[0] = static_cast<float>(N.x);
Pt.normal[1] = static_cast<float>(N.y);
Pt.normal[2] = static_cast<float>(N.z);
}
cloud.push_back(Pt);
}
//manually set bounding box!
Vec3f cbbMin,cbbMax;
cbbMin[0] = static_cast<float>(bbMin.x);
cbbMin[1] = static_cast<float>(bbMin.y);
cbbMin[2] = static_cast<float>(bbMin.z);
cbbMax[0] = static_cast<float>(bbMax.x);
cbbMax[1] = static_cast<float>(bbMax.y);
cbbMax[2] = static_cast<float>(bbMax.z);
cloud.setBBox(cbbMin,cbbMax);
}
//cloud scale (useful for setting several parameters
const float scale = cloud.getScale();
//init dialog with default values
ccRansacSDDlg rsdDlg(m_app->getMainWindow());
rsdDlg.epsilonDoubleSpinBox->setValue(.005f * scale); // set distance threshold to 0.5% of bounding box width
rsdDlg.bitmapEpsilonDoubleSpinBox->setValue(.01f * scale); // set bitmap resolution (= sampling resolution) to 1% of bounding box width
rsdDlg.supportPointsSpinBox->setValue(s_supportPoints);
rsdDlg.maxNormDevAngleSpinBox->setValue(s_maxNormalDev_deg);
rsdDlg.probaDoubleSpinBox->setValue(s_proba);
rsdDlg.planeCheckBox->setChecked(s_primEnabled[0]);
rsdDlg.sphereCheckBox->setChecked(s_primEnabled[1]);
rsdDlg.cylinderCheckBox->setChecked(s_primEnabled[2]);
rsdDlg.coneCheckBox->setChecked(s_primEnabled[3]);
rsdDlg.torusCheckBox->setChecked(s_primEnabled[4]);
if (!rsdDlg.exec())
return;
//for parameters persistence
{
s_supportPoints = rsdDlg.supportPointsSpinBox->value();
s_maxNormalDev_deg = rsdDlg.maxNormDevAngleSpinBox->value();
s_proba = rsdDlg.probaDoubleSpinBox->value();
//consistency check
{
unsigned char primCount = 0;
for (unsigned char k=0;k<5;++k)
primCount += (unsigned)s_primEnabled[k];
if (primCount==0)
{
m_app->dispToConsole("No primitive type selected!",ccMainAppInterface::ERR_CONSOLE_MESSAGE);
return;
}
}
s_primEnabled[0] = rsdDlg.planeCheckBox->isChecked();
s_primEnabled[1] = rsdDlg.sphereCheckBox->isChecked();
s_primEnabled[2] = rsdDlg.cylinderCheckBox->isChecked();
s_primEnabled[3] = rsdDlg.coneCheckBox->isChecked();
s_primEnabled[4] = rsdDlg.torusCheckBox->isChecked();
}
//import parameters from dialog
RansacShapeDetector::Options ransacOptions;
{
ransacOptions.m_epsilon = static_cast<float>(rsdDlg.epsilonDoubleSpinBox->value());
ransacOptions.m_bitmapEpsilon = static_cast<float>(rsdDlg.bitmapEpsilonDoubleSpinBox->value());
ransacOptions.m_normalThresh = static_cast<float>(cos(rsdDlg.maxNormDevAngleSpinBox->value() * CC_DEG_TO_RAD));
assert( ransacOptions.m_normalThresh >= 0 );
ransacOptions.m_probability = static_cast<float>(rsdDlg.probaDoubleSpinBox->value());
ransacOptions.m_minSupport = static_cast<unsigned>(rsdDlg.supportPointsSpinBox->value());
}
if (!hasNorms)
{
QProgressDialog pDlg("Computing normals (please wait)",QString(),0,0,m_app->getMainWindow());
pDlg.setWindowTitle("Ransac Shape Detection");
pDlg.show();
QApplication::processEvents();
cloud.calcNormals(.01f * scale);
if (pc->reserveTheNormsTable())
{
for (unsigned i=0; i<count; ++i)
{
Vec3f& Nvi = cloud[i].normal;
CCVector3 Ni = CCVector3::fromArray(Nvi);
//normalize the vector in case of
Ni.normalize();
pc->addNorm(Ni);
}
pc->showNormals(true);
//currently selected entities appearance may have changed!
pc->prepareDisplayForRefresh_recursive();
}
else
{
m_app->dispToConsole("Not enough memory to compute normals!",ccMainAppInterface::ERR_CONSOLE_MESSAGE);
return;
}
}
// set which primitives are to be detected by adding the respective constructors
RansacShapeDetector detector(ransacOptions); // the detector object
if (rsdDlg.planeCheckBox->isChecked())
detector.Add(new PlanePrimitiveShapeConstructor());
if (rsdDlg.sphereCheckBox->isChecked())
detector.Add(new SpherePrimitiveShapeConstructor());
if (rsdDlg.cylinderCheckBox->isChecked())
detector.Add(new CylinderPrimitiveShapeConstructor());
if (rsdDlg.coneCheckBox->isChecked())
detector.Add(new ConePrimitiveShapeConstructor());
if (rsdDlg.torusCheckBox->isChecked())
detector.Add(new TorusPrimitiveShapeConstructor());
unsigned remaining = count;
typedef std::pair< MiscLib::RefCountPtr< PrimitiveShape >, size_t > DetectedShape;
MiscLib::Vector< DetectedShape > shapes; // stores the detected shapes
// run detection
// returns number of unassigned points
// the array shapes is filled with pointers to the detected shapes
// the second element per shapes gives the number of points assigned to that primitive (the support)
// the points belonging to the first shape (shapes[0]) have been sorted to the end of pc,
// i.e. into the range [ pc.size() - shapes[0].second, pc.size() )
// the points of shape i are found in the range
// [ pc.size() - \sum_{j=0..i} shapes[j].second, pc.size() - \sum_{j=0..i-1} shapes[j].second )
{
//progress dialog (Qtconcurrent::run can't be canceled!)
QProgressDialog pDlg("Operation in progress (please wait)",QString(),0,0,m_app->getMainWindow());
pDlg.setWindowTitle("Ransac Shape Detection");
pDlg.show();
QApplication::processEvents();
//run in a separate thread
s_detector = &detector;
s_shapes = &shapes;
s_cloud = &cloud;
QFuture<void> future = QtConcurrent::run(doDetection);
while (!future.isFinished())
{
#if defined(CC_WINDOWS)
::Sleep(500);
#else
usleep(500 * 1000);
#endif
pDlg.setValue(pDlg.value()+1);
QApplication::processEvents();
}
remaining = static_cast<unsigned>(s_remainingPoints);
pDlg.hide();
QApplication::processEvents();
}
//else
//{
// remaining = detector.Detect(cloud, 0, cloud.size(), &shapes);
//}
#if 0 //def _DEBUG
FILE* fp = fopen("RANS_SD_trace.txt","wt");
fprintf(fp,"[Options]\n");
fprintf(fp,"epsilon=%f\n",ransacOptions.m_epsilon);
fprintf(fp,"bitmap epsilon=%f\n",ransacOptions.m_bitmapEpsilon);
fprintf(fp,"normal thresh=%f\n",ransacOptions.m_normalThresh);
fprintf(fp,"min support=%i\n",ransacOptions.m_minSupport);
fprintf(fp,"probability=%f\n",ransacOptions.m_probability);
fprintf(fp,"\n[Statistics]\n");
fprintf(fp,"input points=%i\n",count);
fprintf(fp,"segmented=%i\n",count-remaining);
fprintf(fp,"remaining=%i\n",remaining);
if (shapes.size()>0)
{
fprintf(fp,"\n[Shapes]\n");
for (unsigned i=0;i<shapes.size();++i)
{
PrimitiveShape* shape = shapes[i].first;
size_t shapePointsCount = shapes[i].second;
std::string desc;
shape->Description(&desc);
fprintf(fp,"#%i - %s - %i points\n",i+1,desc.c_str(),shapePointsCount);
}
}
fclose(fp);
#endif
if (remaining == count)
{
m_app->dispToConsole("Segmentation failed...",ccMainAppInterface::ERR_CONSOLE_MESSAGE);
return;
}
if (shapes.size() > 0)
{
ccHObject* group = 0;
for (MiscLib::Vector<DetectedShape>::const_iterator it = shapes.begin(); it != shapes.end(); ++it)
{
const PrimitiveShape* shape = it->first;
unsigned shapePointsCount = static_cast<unsigned>(it->second);
//too many points?!
if (shapePointsCount > count)
{
m_app->dispToConsole("Inconsistent result!",ccMainAppInterface::ERR_CONSOLE_MESSAGE);
break;
}
std::string desc;
shape->Description(&desc);
//new cloud for sub-part
ccPointCloud* pcShape = new ccPointCloud(desc.c_str());
//we fill cloud with sub-part points
if (!pcShape->reserve((unsigned)shapePointsCount))
{
m_app->dispToConsole("Not enough memory!",ccMainAppInterface::ERR_CONSOLE_MESSAGE);
delete pcShape;
break;
}
bool saveNormals = pcShape->reserveTheNormsTable();
for (unsigned j=0; j<shapePointsCount; ++j)
{
pcShape->addPoint(CCVector3::fromArray(cloud[count-1-j].pos));
if (saveNormals)
pcShape->addNorm(CCVector3::fromArray(cloud[count-1-j].normal));
}
//random color
ccColor::Rgb col = ccColor::Generator::Random();
pcShape->setRGBColor(col);
pcShape->showColors(true);
pcShape->showNormals(saveNormals);
pcShape->setVisible(true);
pcShape->setGlobalShift(globalShift);
pcShape->setGlobalScale(globalScale);
//convert detected primitive into a CC primitive type
ccGenericPrimitive* prim = 0;
switch(shape->Identifier())
{
case 0: //plane
{
const PlanePrimitiveShape* plane = static_cast<const PlanePrimitiveShape*>(shape);
Vec3f G = plane->Internal().getPosition();
Vec3f N = plane->Internal().getNormal();
Vec3f X = plane->getXDim();
Vec3f Y = plane->getYDim();
//we look for real plane extents
float minX,maxX,minY,maxY;
for (unsigned j=0; j<shapePointsCount; ++j)
{
std::pair<float,float> param;
plane->Parameters(cloud[count-1-j].pos,¶m);
if (j != 0)
{
if (minX < param.first)
minX = param.first;
else if (maxX > param.first)
maxX = param.first;
if (minY < param.second)
minY = param.second;
else if (maxY > param.second)
maxY = param.second;
}
else
{
minX = maxX = param.first;
minY = maxY = param.second;
}
}
//we recenter plane (as it is not always the case!)
float dX = maxX-minX;
float dY = maxY-minY;
G += X * (minX+dX/2);
G += Y * (minY+dY/2);
//we build matrix from these vectors
ccGLMatrix glMat( CCVector3::fromArray(X.getValue()),
CCVector3::fromArray(Y.getValue()),
CCVector3::fromArray(N.getValue()),
CCVector3::fromArray(G.getValue()) );
//plane primitive
prim = new ccPlane(dX,dY,&glMat);
}
break;
case 1: //sphere
{
const SpherePrimitiveShape* sphere = static_cast<const SpherePrimitiveShape*>(shape);
float radius = sphere->Internal().Radius();
Vec3f CC = sphere->Internal().Center();
pcShape->setName(QString("Sphere (r=%1)").arg(radius,0,'f'));
//we build matrix from these vecctors
ccGLMatrix glMat;
glMat.setTranslation(CC.getValue());
//sphere primitive
prim = new ccSphere(radius,&glMat);
prim->setEnabled(false);
}
break;
case 2: //cylinder
{
const CylinderPrimitiveShape* cyl = static_cast<const CylinderPrimitiveShape*>(shape);
Vec3f G = cyl->Internal().AxisPosition();
Vec3f N = cyl->Internal().AxisDirection();
Vec3f X = cyl->Internal().AngularDirection();
Vec3f Y = N.cross(X);
float r = cyl->Internal().Radius();
float hMin = cyl->MinHeight();
float hMax = cyl->MaxHeight();
float h = hMax-hMin;
G += N * (hMin+h/2);
pcShape->setName(QString("Cylinder (r=%1/h=%2)").arg(r,0,'f').arg(h,0,'f'));
//we build matrix from these vecctors
ccGLMatrix glMat( CCVector3::fromArray(X.getValue()),
CCVector3::fromArray(Y.getValue()),
CCVector3::fromArray(N.getValue()),
CCVector3::fromArray(G.getValue()) );
//cylinder primitive
prim = new ccCylinder(r,h,&glMat);
prim->setEnabled(false);
}
break;
case 3: //cone
{
const ConePrimitiveShape* cone = static_cast<const ConePrimitiveShape*>(shape);
Vec3f CC = cone->Internal().Center();
Vec3f CA = cone->Internal().AxisDirection();
float alpha = cone->Internal().Angle();
//compute max height
Vec3f minP, maxP;
float minHeight, maxHeight;
minP = maxP = cloud[0].pos;
minHeight = maxHeight = cone->Internal().Height(cloud[0].pos);
for (size_t j=1; j<shapePointsCount; ++j)
{
float h = cone->Internal().Height(cloud[j].pos);
if (h < minHeight)
{
minHeight = h;
minP = cloud[j].pos;
}
else if (h > maxHeight)
{
maxHeight = h;
maxP = cloud[j].pos;
}
}
pcShape->setName(QString("Cone (alpha=%1/h=%2)").arg(alpha,0,'f').arg(maxHeight-minHeight,0,'f'));
float minRadius = tan(alpha)*minHeight;
float maxRadius = tan(alpha)*maxHeight;
//let's build the cone primitive
{
//the bottom should be the largest part so we inverse the axis direction
CCVector3 Z = -CCVector3::fromArray(CA.getValue());
Z.normalize();
//the center is halfway between the min and max height
float midHeight = (minHeight + maxHeight)/2;
CCVector3 C = CCVector3::fromArray((CC + CA * midHeight).getValue());
//radial axis
CCVector3 X = CCVector3::fromArray((maxP - (CC + maxHeight * CA)).getValue());
X.normalize();
//orthogonal radial axis
CCVector3 Y = Z * X;
//we build the transformation matrix from these vecctors
ccGLMatrix glMat(X,Y,Z,C);
//eventually create the cone primitive
prim = new ccCone(maxRadius, minRadius, maxHeight-minHeight, 0, 0, &glMat);
prim->setEnabled(false);
}
}
break;
case 4: //torus
{
const TorusPrimitiveShape* torus = static_cast<const TorusPrimitiveShape*>(shape);
if (torus->Internal().IsAppleShaped())
{
m_app->dispToConsole("[qRansacSD] Apple-shaped torus are not handled by CloudCompare!",ccMainAppInterface::WRN_CONSOLE_MESSAGE);
}
else
{
Vec3f CC = torus->Internal().Center();
Vec3f CA = torus->Internal().AxisDirection();
float minRadius = torus->Internal().MinorRadius();
float maxRadius = torus->Internal().MajorRadius();
pcShape->setName(QString("Torus (r=%1/R=%2)").arg(minRadius,0,'f').arg(maxRadius,0,'f'));
CCVector3 Z = CCVector3::fromArray(CA.getValue());
CCVector3 C = CCVector3::fromArray(CC.getValue());
//construct remaining of base
CCVector3 X = Z.orthogonal();
CCVector3 Y = Z * X;
//we build matrix from these vecctors
ccGLMatrix glMat(X,Y,Z,C);
//torus primitive
prim = new ccTorus(maxRadius-minRadius,maxRadius+minRadius,M_PI*2.0,false,0,&glMat);
prim->setEnabled(false);
}
}
break;
}
//is there a primitive to add to part cloud?
if (prim)
{
prim->applyGLTransformation_recursive();
pcShape->addChild(prim);
prim->setDisplay(pcShape->getDisplay());
prim->setColor(col);
prim->showColors(true);
prim->setVisible(true);
}
if (!group)
group = new ccHObject(QString("Ransac Detected Shapes (%1)").arg(ent->getName()));
group->addChild(pcShape);
count -= shapePointsCount;
QApplication::processEvents();
}
if (group)
{
assert(group->getChildrenNumber() != 0);
//we hide input cloud
pc->setEnabled(false);
m_app->dispToConsole("[qRansacSD] Input cloud has been automtically hidden!",ccMainAppInterface::WRN_CONSOLE_MESSAGE);
//we add new group to DB/display
group->setVisible(true);
group->setDisplay_recursive(pc->getDisplay());
m_app->addToDB(group);
m_app->refreshAll();
}
}
}
void Shader::uniformF(const std::string &name, const Vec3f &v)
{
glf->glUniform3f(uniform(name), v.x(), v.y(), v.z());
}
float Vec3f::getAngle(Vec3f v1, Vec3f v2) const //!< Returns angle between two vectors. Added by N. Van Rossum
{
return acos( v1.dot(v2) / (v1.magnitude() * v2.magnitude()));
}
GLuint Positions(std::vector<T>& dest) const
{
unsigned k = 0, n = _vertex_count();
dest.resize(n * 3);
const Vec3f pos(_point - _u - _v);
const Vec3f ustep(_u * (2.0 / _udiv));
const Vec3f vstep(_v * (2.0 / _vdiv));
for(unsigned i=0; i<(_udiv+1); ++i)
{
Vec3f tmp = pos+ustep*i;
dest[k++] = T(tmp.x());
dest[k++] = T(tmp.y());
dest[k++] = T(tmp.z());
tmp += 2.0*_v;
dest[k++] = T(tmp.x());
dest[k++] = T(tmp.y());
dest[k++] = T(tmp.z());
}
for(unsigned j=1; j<(_vdiv); ++j)
{
Vec3f tmp = pos+vstep*j;
dest[k++] = T(tmp.x());
dest[k++] = T(tmp.y());
dest[k++] = T(tmp.z());
tmp += 2.0*_u;
dest[k++] = T(tmp.x());
dest[k++] = T(tmp.y());
dest[k++] = T(tmp.z());
}
assert(k == dest.size());
return 3;
}
void MovingObject::setOrbitPoint(Vec3f v_)
{
orbitPoint=v_.get();
}
void GroundBase::setup(float realY)
{
//ci::app::getAssetPath("floor.png").string();
texture = ph::loadTexture( ci::app::getAssetPath("floor.png").string());
height =(float) texture.getHeight();
width =(float) texture.getWidth();
setHitRect(width , height);
setAlign(al_int);
vector<uint32_t> indices;
vector<Vec2f> texCoords;
vector<Vec3f> vertices;
vector<Vec3f> normals;
int indicesPos =0;
float step=10;
float nScale=180.1;
float mScale=-40;
int mMod=10;
float nFactor =10;
float minZ =-4;
for(int x=0;x <100;x++)
{
for(int y=0;y<100;y++)
{
indices.push_back( indicesPos++);
indices.push_back( indicesPos++);
indices.push_back( indicesPos++);
indices.push_back( indicesPos++ );
float xp =x*step ;
float yp=y*step;
float zp=pNoise->noise (xp/nScale, (yp+realY)/nScale)*nFactor;
if(zp<minZ) zp=minZ;
Vec3f p1 =Vec3f(xp,yp,(int)zp%mMod *mScale );
vertices.push_back( p1 );
xp =x*step+step ;
yp=y*step;
zp=pNoise->noise (xp/nScale,(yp+realY)/nScale)*nFactor;
if(zp<minZ) zp=minZ;
Vec3f p2 =Vec3f(xp,yp,(int)zp%mMod *mScale);
vertices.push_back( p2);
xp =x*step +step;
yp=y*step+step;
zp=pNoise->noise (xp/nScale, (yp+realY)/nScale)*nFactor;
if(zp<minZ) zp=minZ;
Vec3f p3 =Vec3f(xp,yp,(int)zp%mMod*mScale);
vertices.push_back( p3);
xp =x*step ;
yp=y*step+step;
zp=pNoise->noise (xp/nScale, (yp+realY)/nScale)*nFactor;
if(zp<minZ) zp=minZ;
Vec3f p4 =Vec3f(xp,yp,(int)zp%mMod *mScale );
vertices.push_back( p4 );
float u =(float)((int)zp%mMod +4)/mMod ;
float v =(float) (rand()%100)/100.0f;
texCoords.push_back( Vec2f(u,v ) );
//texCoords.push_back( Vec2f(u,v ) );
//texCoords.push_back( Vec2f(u,v ) );
//texCoords.push_back( Vec2f(u,v ) );
texCoords.push_back( Vec2f(u+0.01,v ) );
texCoords.push_back( Vec2f(u+0.01,v+0.01 ) );
texCoords.push_back( Vec2f(u,v+0.01 ) );
Vec3f n1 =p2-p1;
Vec3f n2 =p4-p1;
Vec3f n = n1.cross(n2);
n.normalize();
normals.push_back( n );
normals.push_back( n );
normals.push_back( n );
normals.push_back( n );
}
}
gl::VboMesh::Layout layout;
layout.setStaticIndices();
layout.setStaticPositions();
layout.setStaticNormals();
layout.setStaticTexCoords2d();
mVboMesh = gl::VboMesh::create( indices.size(), indices.size(), layout, GL_QUADS );
mVboMesh->bufferIndices( indices );
mVboMesh->bufferTexCoords2d( 0, texCoords );
mVboMesh->bufferPositions(vertices);
mVboMesh->bufferNormals(normals);
};
void Camera::setWorldUp( const Vec3f &aWorldUp )
{
mWorldUp = aWorldUp.normalized();
mOrientation = Quatf( Matrix44f::alignZAxisWithTarget( -mViewDirection, mWorldUp ) ).normalized();
mModelViewCached = false;
}
BVH_REAL Intersect::distanceToPlane(const Vec3f& n, BVH_REAL t, const Vec3f& v)
{
return n.dot(v) - t;
}
void Trackball::updateRotation(Real32 rLastX, Real32 rLastY,
Real32 rCurrentX, Real32 rCurrentY)
{
Quaternion qCurrVal;
Vec3f gAxis; /* Axis of rotation */
Real32 rPhi = 0.f; /* how much to rotate about axis */
Vec3f gP1;
Vec3f gP2;
Vec3f gDiff;
Real32 rTmp;
if( (osgAbs(rLastX - rCurrentX) > TypeTraits<Real32>::getDefaultEps()) ||
(osgAbs(rLastY - rCurrentY) > TypeTraits<Real32>::getDefaultEps()) )
{
/*
* First, figure out z-coordinates for projection of P1 and P2 to
* deformed sphere
*/
gP1.setValues( rLastX,
rLastY,
projectToSphere(_rTrackballSize, rLastX, rLastY));
gP2.setValues( rCurrentX,
rCurrentY,
projectToSphere(_rTrackballSize, rCurrentX, rCurrentY));
/*
* Now, we want the cross product of P1 and P2
*/
gAxis = gP2;
gAxis.crossThis(gP1);
/*
* Figure out how much to rotate around that axis.
*/
gDiff = gP2;
gDiff -= gP1;
rTmp = gDiff.length() / (2.0f * _rTrackballSize);
/*
* Avoid problems with out-of-control values...
*/
if(rTmp > 1.0)
rTmp = 1.0;
if(rTmp < -1.0)
rTmp = -1.0;
if(_gMode == OSGObject)
rPhi = Real32(-2.0) * osgASin(rTmp);
else
rPhi = Real32( 2.0) * osgASin(rTmp);
}
rPhi *= _rRotScale;
if(_bSum == false)
{
_qVal.setValueAsAxisRad(gAxis, rPhi);
}
else
{
qCurrVal.setValueAsAxisRad(gAxis, rPhi);
_qVal *= qCurrVal;
// _qVal.multLeft(qCurrVal);
}
}
Vec3f PhotonTracer::traceSample(Vec2u pixel, const KdTree<Photon> &surfaceTree,
const KdTree<VolumePhoton> *mediumTree, PathSampleGenerator &sampler,
float gatherRadius)
{
PositionSample point;
if (!_scene->cam().samplePosition(sampler, point))
return Vec3f(0.0f);
DirectionSample direction;
if (!_scene->cam().sampleDirection(sampler, point, pixel, direction))
return Vec3f(0.0f);
sampler.advancePath();
Vec3f throughput = point.weight*direction.weight;
Ray ray(point.p, direction.d);
ray.setPrimaryRay(true);
IntersectionTemporary data;
IntersectionInfo info;
const Medium *medium = _scene->cam().medium().get();
Vec3f result(0.0f);
int bounce = 0;
bool didHit = _scene->intersect(ray, data, info);
while ((medium || didHit) && bounce < _settings.maxBounces - 1) {
if (medium) {
if (mediumTree) {
Vec3f beamEstimate(0.0f);
mediumTree->beamQuery(ray.pos(), ray.dir(), ray.farT(), [&](const VolumePhoton &p, float t, float distSq) {
Ray mediumQuery(ray);
mediumQuery.setFarT(t);
beamEstimate += (3.0f*INV_PI*sqr(1.0f - distSq/p.radiusSq))/p.radiusSq
*medium->phaseFunction(p.pos)->eval(ray.dir(), -p.dir)
*medium->transmittance(mediumQuery)*p.power;
});
result += throughput*beamEstimate;
}
throughput *= medium->transmittance(ray);
}
if (!didHit)
break;
const Bsdf &bsdf = *info.bsdf;
SurfaceScatterEvent event = makeLocalScatterEvent(data, info, ray, &sampler);
Vec3f transparency = bsdf.eval(event.makeForwardEvent(), false);
float transparencyScalar = transparency.avg();
Vec3f wo;
if (sampler.nextBoolean(DiscreteTransparencySample, transparencyScalar)) {
wo = ray.dir();
throughput *= transparency/transparencyScalar;
} else {
event.requestedLobe = BsdfLobes::SpecularLobe;
if (!bsdf.sample(event, false))
break;
wo = event.frame.toGlobal(event.wo);
throughput *= event.weight;
}
bool geometricBackside = (wo.dot(info.Ng) < 0.0f);
medium = info.primitive->selectMedium(medium, geometricBackside);
ray = ray.scatter(ray.hitpoint(), wo, info.epsilon);
if (std::isnan(ray.dir().sum() + ray.pos().sum()))
break;
if (std::isnan(throughput.sum()))
break;
sampler.advancePath();
bounce++;
if (bounce < _settings.maxBounces)
didHit = _scene->intersect(ray, data, info);
}
if (!didHit) {
if (!medium && _scene->intersectInfinites(ray, data, info))
result += throughput*info.primitive->evalDirect(data, info);
return result;
}
if (info.primitive->isEmissive())
result += throughput*info.primitive->evalDirect(data, info);
int count = surfaceTree.nearestNeighbours(ray.hitpoint(), _photonQuery.get(), _distanceQuery.get(),
_settings.gatherCount, gatherRadius);
if (count == 0)
return result;
const Bsdf &bsdf = *info.bsdf;
SurfaceScatterEvent event = makeLocalScatterEvent(data, info, ray, &sampler);
Vec3f surfaceEstimate(0.0f);
for (int i = 0; i < count; ++i) {
event.wo = event.frame.toLocal(-_photonQuery[i]->dir);
// Asymmetry due to shading normals already compensated for when storing the photon,
// so we don't use the adjoint BSDF here
surfaceEstimate += _photonQuery[i]->power*bsdf.eval(event, false)/std::abs(event.wo.z());
}
float radiusSq = count == int(_settings.gatherCount) ? _distanceQuery[0] : gatherRadius*gatherRadius;
result += throughput*surfaceEstimate*(INV_PI/radiusSq);
return result;
}
