void DemoApplication::updateCamera() {
/*
printf("ele=%f, azi=%f, cp=[%f,%f,%f], ct=[%f,%f,%f], cu=[%f,%f,%f]\n",
m_ele, m_azi,
m_cameraPosition[0], m_cameraPosition[1], m_cameraPosition[2],
m_cameraTargetPosition[0], m_cameraTargetPosition[1], m_cameraTargetPosition[2],
m_cameraUp[0], m_cameraUp[1], m_cameraUp[2]);
*/
glMatrixMode(GL_PROJECTION);
glLoadIdentity();
btScalar rele = m_ele * btScalar(0.01745329251994329547);// rads per deg
btScalar razi = m_azi * btScalar(0.01745329251994329547);// rads per deg
btQuaternion rot(m_cameraUp,razi);
btVector3 eyePos(0,0,0);
eyePos[m_forwardAxis] = -m_cameraDistance;
btVector3 forward(eyePos[0],eyePos[1],eyePos[2]);
if (forward.length2() < SIMD_EPSILON)
{
forward.setValue(1.f,0.f,0.f);
}
btVector3 right = m_cameraUp.cross(forward);
btQuaternion roll(right,-rele);
eyePos = btMatrix3x3(rot) * btMatrix3x3(roll) * eyePos;
m_cameraPosition[0] = eyePos.getX();
m_cameraPosition[1] = eyePos.getY();
m_cameraPosition[2] = eyePos.getZ();
m_cameraPosition += m_cameraTargetPosition;
if (m_glutScreenWidth == 0 && m_glutScreenHeight == 0)
return;
btScalar aspect;
btVector3 extents;
aspect = m_glutScreenWidth / (btScalar)m_glutScreenHeight;
extents.setValue(aspect * 1.0f, 1.0f,0);
if (m_ortho)
{
// reset matrix
glLoadIdentity();
extents *= m_cameraDistance;
btVector3 lower = m_cameraTargetPosition - extents;
btVector3 upper = m_cameraTargetPosition + extents;
//gluOrtho2D(lower.x, upper.x, lower.y, upper.y);
glOrtho(lower.getX(), upper.getX(), lower.getY(), upper.getY(),-1000,1000);
glMatrixMode(GL_MODELVIEW);
glLoadIdentity();
//glTranslatef(100,210,0);
} else
{
// glFrustum (-aspect, aspect, -1.0, 1.0, 1.0, 10000.0);
glFrustum (-aspect * m_frustumZNear, aspect * m_frustumZNear, -m_frustumZNear, m_frustumZNear, m_frustumZNear, m_frustumZFar);
glMatrixMode(GL_MODELVIEW);
glLoadIdentity();
gluLookAt(m_cameraPosition[0], m_cameraPosition[1], m_cameraPosition[2],
m_cameraTargetPosition[0], m_cameraTargetPosition[1], m_cameraTargetPosition[2],
m_cameraUp.getX(),m_cameraUp.getY(),m_cameraUp.getZ());
}
}
Eigen::Rotation2D<double> Rototranslation2D::getRotation() const {
Eigen::Rotation2D<double> rot(0);
rot = rot.fromRotationMatrix(this->linear());
return rot;
}
void Light::SetupShadowViews(Camera* mainCamera, Vector<AutoPtr<ShadowView> >& shadowViews, size_t& useIndex)
{
size_t numViews = NumShadowViews();
if (!numViews)
return;
if (shadowViews.Size() < useIndex + numViews)
shadowViews.Resize(useIndex + numViews);
int numVerticalSplits = (lightType == LIGHT_POINT || (lightType == LIGHT_DIRECTIONAL && NumShadowSplits() > 2)) ? 2 : 1;
int actualShadowMapSize = shadowRect.Height() / numVerticalSplits;
for (size_t i = 0; i < numViews; ++i)
{
if (!shadowViews[useIndex + i])
shadowViews[useIndex + i] = new ShadowView();
ShadowView* view = shadowViews[useIndex + i].Get();
view->Clear();
view->light = this;
Camera& shadowCamera = view->shadowCamera;
switch (lightType)
{
case LIGHT_DIRECTIONAL:
{
IntVector2 topLeft(shadowRect.left, shadowRect.top);
if (i & 1)
topLeft.x += actualShadowMapSize;
if (i & 2)
topLeft.y += actualShadowMapSize;
view->viewport = IntRect(topLeft.x, topLeft.y, topLeft.x + actualShadowMapSize, topLeft.y + actualShadowMapSize);
float splitStart = Max(mainCamera->NearClip(), (i == 0) ? 0.0f : ShadowSplit(i - 1));
float splitEnd = Min(mainCamera->FarClip(), ShadowSplit(i));
float extrusionDistance = mainCamera->FarClip();
// Calculate initial position & rotation
shadowCamera.SetTransform(mainCamera->WorldPosition() - extrusionDistance * WorldDirection(), WorldRotation());
// Calculate main camera shadowed frustum in light's view space
Frustum splitFrustum = mainCamera->WorldSplitFrustum(splitStart, splitEnd);
const Matrix3x4& lightView = shadowCamera.ViewMatrix();
Frustum lightViewFrustum = splitFrustum.Transformed(lightView);
// Fit the frustum inside a bounding box
BoundingBox shadowBox;
shadowBox.Define(lightViewFrustum);
// If shadow camera is far away from the frustum, can bring it closer for better depth precision
/// \todo The minimum distance is somewhat arbitrary
float minDistance = mainCamera->FarClip() * 0.25f;
if (shadowBox.min.z > minDistance)
{
float move = shadowBox.min.z - minDistance;
shadowCamera.Translate(Vector3(0.0f, 0.f, move));
shadowBox.min.z -= move,
shadowBox.max.z -= move;
}
shadowCamera.SetOrthographic(true);
shadowCamera.SetFarClip(shadowBox.max.z);
Vector3 center = shadowBox.Center();
Vector3 size = shadowBox.Size();
shadowCamera.SetOrthoSize(Vector2(size.x, size.y));
shadowCamera.SetZoom(1.0f);
// Center shadow camera to the view space bounding box
Vector3 pos(shadowCamera.WorldPosition());
Quaternion rot(shadowCamera.WorldRotation());
Vector3 adjust(center.x, center.y, 0.0f);
shadowCamera.Translate(rot * adjust, TS_WORLD);
// Snap to whole texels
{
Vector3 viewPos(rot.Inverse() * shadowCamera.WorldPosition());
float invSize = 1.0f / actualShadowMapSize;
Vector2 texelSize(size.x * invSize, size.y * invSize);
Vector3 snap(-fmodf(viewPos.x, texelSize.x), -fmodf(viewPos.y, texelSize.y), 0.0f);
shadowCamera.Translate(rot * snap, TS_WORLD);
}
}
break;
case LIGHT_POINT:
{
static const Quaternion pointLightFaceRotations[] = {
Quaternion(0.0f, 90.0f, 0.0f),
Quaternion(0.0f, -90.0f, 0.0f),
Quaternion(-90.0f, 0.0f, 0.0f),
Quaternion(90.0f, 0.0f, 0.0f),
Quaternion(0.0f, 0.0f, 0.0f),
Quaternion(0.0f, 180.0f, 0.0f)
};
IntVector2 topLeft(shadowRect.left, shadowRect.top);
if (i & 1)
topLeft.y += actualShadowMapSize;
topLeft.x += ((unsigned)i >> 1) * actualShadowMapSize;
view->viewport = IntRect(topLeft.x, topLeft.y, topLeft.x + actualShadowMapSize, topLeft.y + actualShadowMapSize);
shadowCamera.SetTransform(WorldPosition(), pointLightFaceRotations[i]);
shadowCamera.SetFov(90.0f);
// Adjust zoom to avoid edge sampling artifacts (there is a matching adjustment in the shadow sampling)
shadowCamera.SetZoom(0.99f);
shadowCamera.SetFarClip(Range());
shadowCamera.SetNearClip(Range() * 0.01f);
shadowCamera.SetOrthographic(false);
shadowCamera.SetAspectRatio(1.0f);
}
break;
case LIGHT_SPOT:
view->viewport = shadowRect;
shadowCamera.SetTransform(WorldPosition(), WorldRotation());
shadowCamera.SetFov(fov);
shadowCamera.SetZoom(1.0f);
shadowCamera.SetFarClip(Range());
shadowCamera.SetNearClip(Range() * 0.01f);
shadowCamera.SetOrthographic(false);
shadowCamera.SetAspectRatio(1.0f);
break;
}
}
// Setup shadow matrices now as camera positions have been finalized
if (lightType != LIGHT_POINT)
{
shadowMatrices.Resize(numViews);
for (size_t i = 0; i < numViews; ++i)
{
ShadowView* view = shadowViews[useIndex + i].Get();
Camera& shadowCamera = view->shadowCamera;
float width = (float)shadowMap->Width();
float height = (float)shadowMap->Height();
Vector3 offset((float)view->viewport.left / width, (float)view->viewport.top / height, 0.0f);
Vector3 scale(0.5f * (float)view->viewport.Width() / width, 0.5f * (float)view->viewport.Height() / height, 1.0f);
offset.x += scale.x;
offset.y += scale.y;
scale.y = -scale.y;
// OpenGL has different depth range
#ifdef TURSO3D_OPENGL
offset.z = 0.5f;
scale.z = 0.5f;
#endif
Matrix4 texAdjust(Matrix4::IDENTITY);
texAdjust.SetTranslation(offset);
texAdjust.SetScale(scale);
shadowMatrices[i] = texAdjust * shadowCamera.ProjectionMatrix() * shadowCamera.ViewMatrix();
}
}
else
{
// Point lights use an extra constant instead
shadowMatrices.Clear();
Vector2 textureSize((float)shadowMap->Width(), (float)shadowMap->Height());
pointShadowParameters = Vector4(actualShadowMapSize / textureSize.x, actualShadowMapSize / textureSize.y,
(float)shadowRect.left / textureSize.x, (float)shadowRect.top / textureSize.y);
}
// Calculate shadow mapping constants
Camera& shadowCamera = shadowViews[useIndex]->shadowCamera;
float nearClip = shadowCamera.NearClip();
float farClip = shadowCamera.FarClip();
float q = farClip / (farClip - nearClip);
float r = -q * nearClip;
shadowParameters = Vector4(0.5f / (float)shadowMap->Width(), 0.5f / (float)shadowMap->Height(), q, r);
useIndex += numViews;
}
void DemoCameraListener::PreUpdate (const NewtonWorld* const world, dFloat timestep)
{
// update the camera;
DemoEntityManager* const scene = (DemoEntityManager*) NewtonWorldGetUserData(world);
NewtonDemos* const mainWin = scene->GetRootWindow();
dMatrix targetMatrix (m_camera->GetNextMatrix());
int mouseX;
int mouseY;
mainWin->GetMousePosition (mouseX, mouseY);
// slow down the Camera if we have a Body
dFloat slowDownFactor = mainWin->IsShiftKeyDown() ? 0.5f/10.0f : 0.5f;
// do camera translation
if (mainWin->GetKeyState ('W')) {
targetMatrix.m_posit += targetMatrix.m_front.Scale(m_frontSpeed * timestep * slowDownFactor);
}
if (mainWin->GetKeyState ('S')) {
targetMatrix.m_posit -= targetMatrix.m_front.Scale(m_frontSpeed * timestep * slowDownFactor);
}
if (mainWin->GetKeyState ('A')) {
targetMatrix.m_posit -= targetMatrix.m_right.Scale(m_sidewaysSpeed * timestep * slowDownFactor);
}
if (mainWin->GetKeyState ('D')) {
targetMatrix.m_posit += targetMatrix.m_right.Scale(m_sidewaysSpeed * timestep * slowDownFactor);
}
if (mainWin->GetKeyState ('Q')) {
targetMatrix.m_posit -= targetMatrix.m_up.Scale(m_sidewaysSpeed * timestep * slowDownFactor);
}
if (mainWin->GetKeyState ('E')) {
targetMatrix.m_posit += targetMatrix.m_up.Scale(m_sidewaysSpeed * timestep * slowDownFactor);
}
// do camera rotation, only if we do not have anything picked
bool buttonState = m_mouseLockState || mainWin->GetMouseKeyState(0);
if (!m_targetPicked && buttonState) {
int mouseSpeedX = mouseX - m_mousePosX;
int mouseSpeedY = mouseY - m_mousePosY;
if (mouseSpeedX > 0) {
m_yaw = dMod(m_yaw + m_yawRate, 2.0f * 3.1416f);
} else if (mouseSpeedX < 0){
m_yaw = dMod(m_yaw - m_yawRate, 2.0f * 3.1416f);
}
if (mouseSpeedY > 0) {
m_pitch += m_pitchRate;
} else if (mouseSpeedY < 0){
m_pitch -= m_pitchRate;
}
m_pitch = dClamp(m_pitch, dFloat (-80.0f * 3.1416f / 180.0f), dFloat (80.0f * 3.1416f / 180.0f));
}
m_mousePosX = mouseX;
m_mousePosY = mouseY;
dMatrix matrix (dRollMatrix(m_pitch) * dYawMatrix(m_yaw));
dQuaternion rot (matrix);
m_camera->SetMatrix (*scene, rot, targetMatrix.m_posit);
UpdatePickBody(scene, timestep);
}
Rototranslation2D Rototranslation2D::inverse() const {
Eigen::Rotation2D<double> rot(-this->getAngle());
Eigen::Vector2d tNew(rot * this->getTranslation());
tNew = -tNew;
return Rototranslation2D(rot, tNew);
}
void LoadSkeleton(const ea::string& skeletonFileName)
{
// Process skeleton first (if found)
XMLElement skeletonRoot;
File skeletonFileSource(context_);
skeletonFileSource.Open(skeletonFileName);
if (!skelFile_->Load(skeletonFileSource))
PrintLine("Failed to load skeleton " + skeletonFileName);
skeletonRoot = skelFile_->GetRoot();
if (skeletonRoot)
{
XMLElement bonesRoot = skeletonRoot.GetChild("bones");
XMLElement bone = bonesRoot.GetChild("bone");
while (bone)
{
unsigned index = bone.GetInt("id");
ea::string name = bone.GetAttribute("name");
if (index >= bones_.size())
bones_.resize(index + 1);
// Convert from right- to left-handed
XMLElement position = bone.GetChild("position");
float x = position.GetFloat("x");
float y = position.GetFloat("y");
float z = position.GetFloat("z");
Vector3 pos(x, y, -z);
XMLElement rotation = bone.GetChild("rotation");
XMLElement axis = rotation.GetChild("axis");
float angle = -rotation.GetFloat("angle") * M_RADTODEG;
x = axis.GetFloat("x");
y = axis.GetFloat("y");
z = axis.GetFloat("z");
Vector3 axisVec(x, y, -z);
Quaternion rot(angle, axisVec);
bones_[index].name_ = name;
bones_[index].parentIndex_ = index; // Fill in the correct parent later
bones_[index].bindPosition_ = pos;
bones_[index].bindRotation_ = rot;
bones_[index].bindScale_ = Vector3::ONE;
bones_[index].collisionMask_ = 0;
bones_[index].radius_ = 0.0f;
bone = bone.GetNext("bone");
}
// Go through the bone hierarchy
XMLElement boneHierarchy = skeletonRoot.GetChild("bonehierarchy");
XMLElement boneParent = boneHierarchy.GetChild("boneparent");
while (boneParent)
{
ea::string bone = boneParent.GetAttribute("bone");
ea::string parent = boneParent.GetAttribute("parent");
unsigned i = 0, j = 0;
for (i = 0; i < bones_.size() && bones_[i].name_ != bone; ++i);
for (j = 0; j < bones_.size() && bones_[j].name_ != parent; ++j);
if (i >= bones_.size() || j >= bones_.size())
ErrorExit("Found indeterminate parent bone assignment");
bones_[i].parentIndex_ = j;
boneParent = boneParent.GetNext("boneparent");
}
// Calculate bone derived positions
for (unsigned i = 0; i < bones_.size(); ++i)
{
Vector3 derivedPosition = bones_[i].bindPosition_;
Quaternion derivedRotation = bones_[i].bindRotation_;
Vector3 derivedScale = bones_[i].bindScale_;
unsigned index = bones_[i].parentIndex_;
if (index != i)
{
for (;;)
{
derivedPosition = bones_[index].bindPosition_ + (bones_[index].bindRotation_ * (bones_[index].bindScale_ * derivedPosition));
derivedRotation = bones_[index].bindRotation_ * derivedRotation;
derivedScale = bones_[index].bindScale_ * derivedScale;
if (bones_[index].parentIndex_ != index)
index = bones_[index].parentIndex_;
else
break;
}
}
bones_[i].derivedPosition_ = derivedPosition;
bones_[i].derivedRotation_ = derivedRotation;
bones_[i].derivedScale_ = derivedScale;
bones_[i].worldTransform_ = Matrix3x4(derivedPosition, derivedRotation, derivedScale);
bones_[i].inverseWorldTransform_ = bones_[i].worldTransform_.Inverse();
}
PrintLine("Processed skeleton");
}
}
void WriteOutput(const ea::string& outputFileName, bool exportAnimations, bool rotationsOnly, bool saveMaterialList)
{
/// \todo Use save functions of Model & Animation classes
// Begin serialization
{
File dest(context_);
if (!dest.Open(outputFileName, FILE_WRITE))
ErrorExit("Could not open output file " + outputFileName);
// ID
dest.WriteFileID("UMD2");
// Vertexbuffers
dest.WriteUInt(vertexBuffers_.size());
for (unsigned i = 0; i < vertexBuffers_.size(); ++i)
vertexBuffers_[i].WriteData(dest);
// Indexbuffers
dest.WriteUInt(indexBuffers_.size());
for (unsigned i = 0; i < indexBuffers_.size(); ++i)
indexBuffers_[i].WriteData(dest);
// Subgeometries
dest.WriteUInt(subGeometries_.size());
for (unsigned i = 0; i < subGeometries_.size(); ++i)
{
// Write bone mapping info from the first LOD level. It does not change for further LODs
dest.WriteUInt(subGeometries_[i][0].boneMapping_.size());
for (unsigned k = 0; k < subGeometries_[i][0].boneMapping_.size(); ++k)
dest.WriteUInt(subGeometries_[i][0].boneMapping_[k]);
// Lod levels for this subgeometry
dest.WriteUInt(subGeometries_[i].size());
for (unsigned j = 0; j < subGeometries_[i].size(); ++j)
{
dest.WriteFloat(subGeometries_[i][j].distance_);
dest.WriteUInt((unsigned)subGeometries_[i][j].primitiveType_);
dest.WriteUInt(subGeometries_[i][j].vertexBuffer_);
dest.WriteUInt(subGeometries_[i][j].indexBuffer_);
dest.WriteUInt(subGeometries_[i][j].indexStart_);
dest.WriteUInt(subGeometries_[i][j].indexCount_);
}
}
// Morphs
dest.WriteUInt(morphs_.size());
for (unsigned i = 0; i < morphs_.size(); ++i)
morphs_[i].WriteData(dest);
// Skeleton
dest.WriteUInt(bones_.size());
for (unsigned i = 0; i < bones_.size(); ++i)
{
dest.WriteString(bones_[i].name_);
dest.WriteUInt(bones_[i].parentIndex_);
dest.WriteVector3(bones_[i].bindPosition_);
dest.WriteQuaternion(bones_[i].bindRotation_);
dest.WriteVector3(bones_[i].bindScale_);
Matrix3x4 offsetMatrix(bones_[i].derivedPosition_, bones_[i].derivedRotation_, bones_[i].derivedScale_);
offsetMatrix = offsetMatrix.Inverse();
dest.Write(offsetMatrix.Data(), sizeof(Matrix3x4));
dest.WriteUByte(bones_[i].collisionMask_);
if (bones_[i].collisionMask_ & 1u)
dest.WriteFloat(bones_[i].radius_);
if (bones_[i].collisionMask_ & 2u)
dest.WriteBoundingBox(bones_[i].boundingBox_);
}
// Bounding box
dest.WriteBoundingBox(boundingBox_);
// Geometry centers
for (unsigned i = 0; i < subGeometryCenters_.size(); ++i)
dest.WriteVector3(subGeometryCenters_[i]);
}
if (saveMaterialList)
{
ea::string materialListName = ReplaceExtension(outputFileName, ".txt");
File listFile(context_);
if (listFile.Open(materialListName, FILE_WRITE))
{
for (unsigned i = 0; i < materialNames_.size(); ++i)
{
// Assume the materials will be located inside the standard Materials subdirectory
listFile.WriteLine("Materials/" + ReplaceExtension(SanitateAssetName(materialNames_[i]), ".xml"));
}
}
else
PrintLine("Warning: could not write material list file " + materialListName);
}
XMLElement skeletonRoot = skelFile_->GetRoot("skeleton");
if (skeletonRoot && exportAnimations)
{
// Go through animations
XMLElement animationsRoot = skeletonRoot.GetChild("animations");
if (animationsRoot)
{
XMLElement animation = animationsRoot.GetChild("animation");
while (animation)
{
ModelAnimation newAnimation;
newAnimation.name_ = animation.GetAttribute("name");
newAnimation.length_ = animation.GetFloat("length");
XMLElement tracksRoot = animation.GetChild("tracks");
XMLElement track = tracksRoot.GetChild("track");
while (track)
{
ea::string trackName = track.GetAttribute("bone");
ModelBone* bone = nullptr;
for (unsigned i = 0; i < bones_.size(); ++i)
{
if (bones_[i].name_ == trackName)
{
bone = &bones_[i];
break;
}
}
if (!bone)
ErrorExit("Found animation track for unknown bone " + trackName);
AnimationTrack newAnimationTrack;
newAnimationTrack.name_ = trackName;
if (!rotationsOnly)
newAnimationTrack.channelMask_ = CHANNEL_POSITION | CHANNEL_ROTATION;
else
newAnimationTrack.channelMask_ = CHANNEL_ROTATION;
XMLElement keyFramesRoot = track.GetChild("keyframes");
XMLElement keyFrame = keyFramesRoot.GetChild("keyframe");
while (keyFrame)
{
AnimationKeyFrame newKeyFrame;
// Convert from right- to left-handed
XMLElement position = keyFrame.GetChild("translate");
float x = position.GetFloat("x");
float y = position.GetFloat("y");
float z = position.GetFloat("z");
Vector3 pos(x, y, -z);
XMLElement rotation = keyFrame.GetChild("rotate");
XMLElement axis = rotation.GetChild("axis");
float angle = -rotation.GetFloat("angle") * M_RADTODEG;
x = axis.GetFloat("x");
y = axis.GetFloat("y");
z = axis.GetFloat("z");
Vector3 axisVec(x, y, -z);
Quaternion rot(angle, axisVec);
// Transform from bind-pose relative into absolute
pos = bone->bindPosition_ + pos;
rot = bone->bindRotation_ * rot;
newKeyFrame.time_ = keyFrame.GetFloat("time");
newKeyFrame.position_ = pos;
newKeyFrame.rotation_ = rot;
newAnimationTrack.keyFrames_.push_back(newKeyFrame);
keyFrame = keyFrame.GetNext("keyframe");
}
// Make sure keyframes are sorted from beginning to end
ea::quick_sort(newAnimationTrack.keyFrames_.begin(), newAnimationTrack.keyFrames_.end(), CompareKeyFrames);
// Do not add tracks with no keyframes
if (newAnimationTrack.keyFrames_.size())
newAnimation.tracks_.push_back(newAnimationTrack);
track = track.GetNext("track");
}
// Write each animation into a separate file
ea::string animationFileName = outputFileName.replaced(".mdl", "");
animationFileName += "_" + newAnimation.name_ + ".ani";
File dest(context_);
if (!dest.Open(animationFileName, FILE_WRITE))
ErrorExit("Could not open output file " + animationFileName);
dest.WriteFileID("UANI");
dest.WriteString(newAnimation.name_);
dest.WriteFloat(newAnimation.length_);
dest.WriteUInt(newAnimation.tracks_.size());
for (unsigned i = 0; i < newAnimation.tracks_.size(); ++i)
{
AnimationTrack& track = newAnimation.tracks_[i];
dest.WriteString(track.name_);
dest.WriteUByte(track.channelMask_);
dest.WriteUInt(track.keyFrames_.size());
for (unsigned j = 0; j < track.keyFrames_.size(); ++j)
{
AnimationKeyFrame& keyFrame = track.keyFrames_[j];
dest.WriteFloat(keyFrame.time_);
if (track.channelMask_ & CHANNEL_POSITION)
dest.WriteVector3(keyFrame.position_);
if (track.channelMask_ & CHANNEL_ROTATION)
dest.WriteQuaternion(keyFrame.rotation_);
if (track.channelMask_ & CHANNEL_SCALE)
dest.WriteVector3(keyFrame.scale_);
}
}
animation = animation.GetNext("animation");
PrintLine("Processed animation " + newAnimation.name_);
}
}
}
}
void dgCollisionChamferCylinder::DebugCollision(const dgMatrix& matrixPtr,
OnDebugCollisionMeshCallback callback, void* const userData) const
{
dgInt32 i;
dgInt32 j;
dgInt32 index;
dgInt32 index0;
dgInt32 brakes;
dgInt32 slices;
dgFloat32 sliceStep;
dgFloat32 sliceAngle;
dgFloat32 breakStep;
dgFloat32 breakAngle;
slices = 12;
brakes = 24;
sliceAngle = dgFloat32(0.0f);
breakAngle = dgFloat32(0.0f);
sliceStep = dgPI / slices;
breakStep = dgPI2 / brakes;
dgTriplex pool[24 * (12 + 1)];
dgMatrix rot(dgPitchMatrix(breakStep));
index = 0;
for (j = 0; j <= slices; j++)
{
dgVector p0(-m_height * dgCos(sliceAngle), dgFloat32(0.0f),
m_radius + m_height * dgSin(sliceAngle), dgFloat32(0.0f));
sliceAngle += sliceStep;
for (i = 0; i < brakes; i++)
{
pool[index].m_x = p0.m_x;
pool[index].m_y = p0.m_y;
pool[index].m_z = p0.m_z;
index++;
p0 = rot.UnrotateVector(p0);
}
}
// const dgMatrix &matrix = myBody.GetCollisionMatrix();
dgMatrix matrix(GetOffsetMatrix() * matrixPtr);
matrix.TransformTriplex(&pool[0].m_x, sizeof(dgTriplex), &pool[0].m_x,
sizeof(dgTriplex), 24 * (12 + 1));
dgTriplex face[32];
index = 0;
for (j = 0; j < slices; j++)
{
index0 = index + brakes - 1;
for (i = 0; i < brakes; i++)
{
face[0] = pool[index];
face[1] = pool[index0];
face[2] = pool[index0 + brakes];
face[3] = pool[index + brakes];
index0 = index;
index++;
callback(userData, 4, &face[0].m_x, 0);
}
}
for (i = 0; i < brakes; i++)
{
face[i] = pool[i];
}
callback(userData, 24, &face[0].m_x, 0);
for (i = 0; i < brakes; i++)
{
face[i] = pool[brakes * (slices + 1) - i - 1];
}
callback(userData, 24, &face[0].m_x, 0);
}
void dgCollisionChamferCylinder::Init(dgFloat32 radius, dgFloat32 height)
{
// dgInt32 i;
// dgInt32 j;
// dgInt32 index;
// dgInt32 index0;
// dgFloat32 sliceStep;
// dgFloat32 sliceAngle;
// dgFloat32 breakStep;
// dgFloat32 breakAngle;
// dgEdge *edge;
m_rtti |= dgCollisionChamferCylinder_RTTI;
m_radius = dgAbsf(radius);
m_height = dgAbsf(height * dgFloat32(0.5f));
m_radius = GetMax(dgFloat32(0.001f), m_radius - m_height);
m_silhuette[0] = dgVector(m_height, m_radius, dgFloat32(0.0f),
dgFloat32(0.0f));
m_silhuette[1] = dgVector(m_height, -m_radius, dgFloat32(0.0f),
dgFloat32(0.0f));
m_silhuette[2] = dgVector(-m_height, -m_radius, dgFloat32(0.0f),
dgFloat32(0.0f));
m_silhuette[3] = dgVector(-m_height, m_radius, dgFloat32(0.0f),
dgFloat32(0.0f));
// m_tethaStep = GetDiscretedAngleStep (m_radius);
// m_tethaStepInv = dgFloat32 (1.0f) / m_tethaStep;
// m_delCosTetha = dgCos (m_tethaStep);
// m_delSinTetha = dgSin (m_tethaStep);
dgFloat32 sliceAngle = dgFloat32(0.0f);
//dgFloat32 breakAngle = dgFloat32 (0.0f);
dgFloat32 sliceStep = dgPI / DG_CHAMFERCYLINDER_SLICES;
dgFloat32 breakStep = dgPI2 / DG_CHAMFERCYLINDER_BRAKES;
dgMatrix rot(dgPitchMatrix(breakStep));
dgInt32 index = 0;
for (dgInt32 j = 0; j <= DG_CHAMFERCYLINDER_SLICES; j++)
{
dgVector p0(-m_height * dgCos(sliceAngle), dgFloat32(0.0f),
m_radius + m_height * dgSin(sliceAngle), dgFloat32(1.0f));
sliceAngle += sliceStep;
for (dgInt32 i = 0; i < DG_CHAMFERCYLINDER_BRAKES; i++)
{
m_vertex[index] = p0;
index++;
p0 = rot.UnrotateVector(p0);
}
}
m_edgeCount = (4 * DG_CHAMFERCYLINDER_SLICES + 2) * DG_CHAMFERCYLINDER_BRAKES;
m_vertexCount = DG_CHAMFERCYLINDER_BRAKES * (DG_CHAMFERCYLINDER_SLICES + 1);
dgCollisionConvex::m_vertex = m_vertex;
if (!m_shapeRefCount)
{
dgPolyhedra polyhedra(m_allocator);
dgInt32 wireframe[DG_CHAMFERCYLINDER_SLICES + 10];
for (dgInt32 i = 0; i < DG_MAX_CHAMFERCYLINDER_DIR_COUNT; i++)
{
dgMatrix matrix(
dgPitchMatrix(
dgFloat32(dgPI2 * i) / DG_MAX_CHAMFERCYLINDER_DIR_COUNT));
m_shapesDirs[i] = matrix.RotateVector(
dgVector(dgFloat32(0.0f), dgFloat32(1.0f), dgFloat32(0.0f),
dgFloat32(0.0f)));
}
dgInt32 index = 0;
for (dgInt32 j = 0; j < DG_CHAMFERCYLINDER_SLICES; j++)
{
dgInt32 index0 = index + DG_CHAMFERCYLINDER_BRAKES - 1;
for (dgInt32 i = 0; i < DG_CHAMFERCYLINDER_BRAKES; i++)
{
wireframe[0] = index;
wireframe[1] = index0;
wireframe[2] = index0 + DG_CHAMFERCYLINDER_BRAKES;
wireframe[3] = index + DG_CHAMFERCYLINDER_BRAKES;
index0 = index;
index++;
polyhedra.AddFace(4, wireframe);
}
}
for (dgInt32 i = 0; i < DG_CHAMFERCYLINDER_BRAKES; i++)
{
wireframe[i] = i;
}
polyhedra.AddFace(DG_CHAMFERCYLINDER_BRAKES, wireframe);
for (dgInt32 i = 0; i < DG_CHAMFERCYLINDER_BRAKES; i++)
{
wireframe[i] = DG_CHAMFERCYLINDER_BRAKES * (DG_CHAMFERCYLINDER_SLICES + 1)
- i - 1;
}
polyhedra.AddFace(DG_CHAMFERCYLINDER_BRAKES, wireframe);
polyhedra.EndFace();
_ASSERTE(SanityCheck (polyhedra));
dgUnsigned64 i = 0;
dgPolyhedra::Iterator iter(polyhedra);
for (iter.Begin(); iter; iter++)
{
dgEdge* const edge = &(*iter);
edge->m_userData = i;
i++;
}
for (iter.Begin(); iter; iter++)
{
dgEdge* const edge = &(*iter);
dgConvexSimplexEdge* const ptr = &m_edgeArray[edge->m_userData];
ptr->m_vertex = edge->m_incidentVertex;
ptr->m_next = &m_edgeArray[edge->m_next->m_userData];
ptr->m_prev = &m_edgeArray[edge->m_prev->m_userData];
ptr->m_twin = &m_edgeArray[edge->m_twin->m_userData];
}
}
m_shapeRefCount++;
dgCollisionConvex::m_simplex = m_edgeArray;
SetVolumeAndCG();
// CalculateDistanceTravel();
}
void AnimClip::copyFromNetworkAnim() {
assert(_networkAnim && _networkAnim->isLoaded() && _skeleton);
_anim.clear();
// build a mapping from animation joint indices to skeleton joint indices.
// by matching joints with the same name.
const FBXGeometry& geom = _networkAnim->getGeometry();
AnimSkeleton animSkeleton(geom);
const auto animJointCount = animSkeleton.getNumJoints();
const auto skeletonJointCount = _skeleton->getNumJoints();
std::vector<int> jointMap;
jointMap.reserve(animJointCount);
for (int i = 0; i < animJointCount; i++) {
int skeletonJoint = _skeleton->nameToJointIndex(animSkeleton.getJointName(i));
if (skeletonJoint == -1) {
qCWarning(animation) << "animation contains joint =" << animSkeleton.getJointName(i) << " which is not in the skeleton, url =" << _url;
}
jointMap.push_back(skeletonJoint);
}
const int frameCount = geom.animationFrames.size();
_anim.resize(frameCount);
for (int frame = 0; frame < frameCount; frame++) {
const FBXAnimationFrame& fbxAnimFrame = geom.animationFrames[frame];
// init all joints in animation to default pose
// this will give us a resonable result for bones in the model skeleton but not in the animation.
_anim[frame].reserve(skeletonJointCount);
for (int skeletonJoint = 0; skeletonJoint < skeletonJointCount; skeletonJoint++) {
_anim[frame].push_back(_skeleton->getRelativeDefaultPose(skeletonJoint));
}
for (int animJoint = 0; animJoint < animJointCount; animJoint++) {
int skeletonJoint = jointMap[animJoint];
const glm::vec3& fbxAnimTrans = fbxAnimFrame.translations[animJoint];
const glm::quat& fbxAnimRot = fbxAnimFrame.rotations[animJoint];
// skip joints that are in the animation but not in the skeleton.
if (skeletonJoint >= 0 && skeletonJoint < skeletonJointCount) {
AnimPose preRot, postRot;
if (usePreAndPostPoseFromAnim) {
preRot = animSkeleton.getPreRotationPose(animJoint);
postRot = animSkeleton.getPostRotationPose(animJoint);
} else {
// In order to support Blender, which does not have preRotation FBX support, we use the models defaultPose as the reference frame for the animations.
preRot = AnimPose(glm::vec3(1.0f), _skeleton->getRelativeBindPose(skeletonJoint).rot, glm::vec3());
postRot = AnimPose::identity;
}
// cancel out scale
preRot.scale = glm::vec3(1.0f);
postRot.scale = glm::vec3(1.0f);
AnimPose rot(glm::vec3(1.0f), fbxAnimRot, glm::vec3());
// adjust translation offsets, so large translation animatons on the reference skeleton
// will be adjusted when played on a skeleton with short limbs.
const glm::vec3& fbxZeroTrans = geom.animationFrames[0].translations[animJoint];
const AnimPose& relDefaultPose = _skeleton->getRelativeDefaultPose(skeletonJoint);
float boneLengthScale = 1.0f;
const float EPSILON = 0.0001f;
if (fabsf(glm::length(fbxZeroTrans)) > EPSILON) {
boneLengthScale = glm::length(relDefaultPose.trans) / glm::length(fbxZeroTrans);
}
AnimPose trans = AnimPose(glm::vec3(1.0f), glm::quat(), relDefaultPose.trans + boneLengthScale * (fbxAnimTrans - fbxZeroTrans));
_anim[frame][skeletonJoint] = trans * preRot * rot * postRot;
}
}
}
// mirrorAnim will be re-built on demand, if needed.
_mirrorAnim.clear();
_poses.resize(skeletonJointCount);
}
void draw(const Eigen::Quaternionf &q, const Eigen::Vector3f &translation, const cv::Scalar &color, std::list<cv::Point2f> &tail, bool draw_tail = true)
{
// Project z axis to ground plane
Eigen::Vector3f z(0.0, 0.0, 1.0);
z = q.matrix() * z + translation;
// Metric dimension in 3D coordinate frame
float d_x = 15.24; // In meter
float d_y = 28.6512; // In meter
// Convert to image coordinate
float s_x = img_draw.cols;
float s_y = img_draw.rows;
float o_x = s_x - translation(1) / d_y * s_x;
float o_y = s_y - translation(0) / d_x * s_y;
float p_x = s_x - z(1) / d_y * s_x;
float p_y = s_y - z(0) / d_x * s_y;
float s = sqrt((p_x - o_x) * (p_x - o_x) + (p_y - o_y) * (p_y - o_y));
float u = (p_x - o_x) / s;
float v = (p_y - o_y) / s;
float theta = atan2(v, u);
// 2D rotation matrix and translation
Eigen::Rotation2Df rot(theta);
Eigen::Vector2f t(o_x, o_y);
// Camera cone
std::vector<Eigen::Vector2f> camera;
float l = 5;
Eigen::Vector2f o = rot * Eigen::Vector2f(0.0, 0.0) + t;
Eigen::Vector2f a = rot * Eigen::Vector2f(3 * l, -3 * l) + t;
Eigen::Vector2f b = rot * Eigen::Vector2f(3 * l, 3 * l) + t;
if (tail.size() == 250)
tail.pop_front();
tail.push_back(cv::Point2f(o(0), o(1)));
cv::circle(img_draw, cv::Point2f(o_x, o_y), 3, color);
cv::line(img_draw, cv::Point2f(o(0), o(1)), cv::Point2f(a(0), a(1)), color, 2, CV_AA);
cv::line(img_draw, cv::Point2f(o(0), o(1)), cv::Point2f(b(0), b(1)), color, 2, CV_AA);
cv::line(img_draw, cv::Point2f(a(0), a(1)), cv::Point2f(b(0), b(1)), color, 2, CV_AA);
if (true == draw_tail)
{
cv::Point2f p1, p2;
for (std::list<cv::Point2f>::iterator iter = tail.begin(); iter != tail.end(); ++iter)
{
if (iter == tail.begin())
{
p1 = *iter;
continue;
}
p2 = *iter;
cv::line(img_draw, p1, p2, color, 2, CV_AA);
p1 = p2;
}
}
}
Edge_Record Edge_Record::rprev() const {
return rot().onext().rot_inv();
}
bool OgreModel::fromJson(const Json::Value &node, Ogre::SceneNode *levelRoot, Ogre::SceneManager *sceneManager, const Ogre::String &resourceGroupName)
{
// data to gather
Ogre::String meshName;
Ogre::Vector3 pos(Ogre::Vector3::ZERO);
Ogre::Quaternion rot(Ogre::Radian(0), -Ogre::Vector3::UNIT_Z);
Ogre::Vector3 scale(Ogre::Vector3::UNIT_SCALE);
Json::Value value;
bool allWasFine = true;
// position
if(node[OgreModel::POSITION_ATTRIBUTE].isNull())
Debug::error(STEEL_METH_INTRO, "position is null: no translation applied.").endl();
else
pos = JsonUtils::asVector3(node[OgreModel::POSITION_ATTRIBUTE], pos);
//rotation
if(!node[OgreModel::ROTATION_ATTRIBUTE].isNull())
rot = JsonUtils::asQuaternion(node[OgreModel::ROTATION_ATTRIBUTE], rot);
//scale
if(!node[OgreModel::SCALE_ATTRIBUTE].isNull())
scale = JsonUtils::asVector3(node[OgreModel::SCALE_ATTRIBUTE], scale);
// mesh name
value = node[OgreModel::ENTITY_MESH_NAME_ATTRIBUTE];
if(value.isNull() && !(allWasFine = false))
Debug::error(STEEL_METH_INTRO, "field ").quotes(OgreModel::ENTITY_MESH_NAME_ATTRIBUTE)(" is null.").endl();
else
meshName = Ogre::String(value.asString());
// custom material
Ogre::String materialName = StringUtils::BLANK;
if(node.isMember(OgreModel::MATERIAL_OVERRIDE_ATTRIBUTE))
{
value = node[OgreModel::MATERIAL_OVERRIDE_ATTRIBUTE];
materialName = JsonUtils::asString(value, StringUtils::BLANK);
if(StringUtils::BLANK == materialName)
Debug::error(STEEL_METH_INTRO, "could not read field ").quotes(OgreModel::MATERIAL_OVERRIDE_ATTRIBUTE)(": ").quotes(value).endl();
}
// agentTags
allWasFine &= deserializeTags(node);
if(!allWasFine)
{
Debug::error("json was:").endl()(node.toStyledString()).endl();
Debug::error("model deserialisation aborted.").endl();
return false;
}
// now whether we have minor changes (and we apply them directly), or major ones (cleanup, then init).
// lets start with major ones
if(mEntity == nullptr || meshName != mEntity->getMesh()->getName())
{
// make sure we've been called with all arguments, because they're all needed now
if(levelRoot == nullptr || sceneManager == nullptr)
{
Debug::error(STEEL_METH_INTRO, "a new mesh is required, but sceneManager or levelRoot are nullptr. Aborting.").endl();
return false;
}
// make sure the new meshName is valid
if(meshName.length() == 0 || !Ogre::ResourceGroupManager::getSingletonPtr()->resourceExistsInAnyGroup(meshName))
{
Debug::error(STEEL_METH_INTRO, "could not find resource ").quotes(meshName)(" in any group. Aborting.").endl();
return false;
}
if(!init(meshName, pos, rot, scale, levelRoot, sceneManager, resourceGroupName))
{
return false;
}
}
else
{
setPosition(pos);
setRotation(rot);
setScale(scale);
}
if(StringUtils::BLANK != materialName)
{
setMaterial(materialName, resourceGroupName);
}
return true;
}
void laserPclCallback(const sensor_msgs::PointCloud2::ConstPtr &msg)
{
pcl::PCLPointCloud2 pcl_pcl2;
ros::Duration t(0.01);
static int counter = 0;
// Convert the sensor_msgs/PointCloud2 data to pcl/PointCloud
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ> );
pcl::fromROSMsg (*msg, *cloud); // processing here
tf::TransformListener tf_listener_;
// Obtain sensor_pose
Eigen::Affine3f sensorPose;
// Eigen::Affine3f sensorPose = Eigen::Affine3f(Eigen::Translation3f(cloud->sensor_origin_[0],
// cloud->sensor_origin_[1],
// cloud->sensor_origin_[2])) *
// Eigen::Affine3f(cloud->sensor_orientation_);
tf::StampedTransform transform;
try
{
tf_listener_.waitForTransform("base_link", "laser",
ros::Time::now(), ros::Duration(1.0));
tf_listener_.lookupTransform("base_link", "laser",
ros::Time(0), transform);
}
catch ( std::runtime_error e )
{
ROS_ERROR("%s",e.what());
}
tf::Vector3 p = transform.getOrigin();
tf::Quaternion q = transform.getRotation();
sensorPose = (Eigen::Affine3f)Eigen::Translation3f(p.getX(),
p.getY(), p.getZ());
Eigen::Quaternion<float> rot(q.getW(), q.getX(), q.getY(), q.getZ());
sensorPose = sensorPose * rot;
// if (tx != 0.0)
// {
// Eigen::Affine3f trans =
// (Eigen::Affine3f)Eigen::Translation3f(-tx/fx , 0, 0);
// sensorPose = sensorPose * trans;
// }
pcl::RangeImagePlanar range_image_planar;
range_image_planar.createFromPointCloudWithFixedSize(*cloud,
imageSizeX,
imageSizeY,
centerX, centerY,
focalLengthX,
focalLengthY,
sensorPose,
pcl::RangeImage::LASER_FRAME,
noiseLevel, minRange);
// // --------------------------
// // -----Show range image-----
// // --------------------------
cv::Mat img(range_image_planar.height, range_image_planar.width, CV_32F);
for (int i = 0; i < imageSizeY; i++)
for (int j = 0; j < imageSizeX; j++)
{
const pcl::PointWithRange p = range_image_planar.getPoint(i * imageSizeX + j) ;
// ROS_INFO("pixel (%d %d) = %f ", i, j, p.z);
img.at<float>(i * imageSizeX + j ) = p.x;
// if (p.z > LASER_RANGE_MAX)
// img.at<float>(i * imageSizeX + j ) = LASER_RANGE_MAX;
}
double min, max;
cv::minMaxIdx(img, &min, &max);
ROS_INFO("max = %f min= %f", max, min);
//if (min<0)min = min*-1;
// ROS_INFO("min: %f", fabs(min));
// Add minimum values
// cv::Mat img_corrected(range_image_planar.height, range_image_planar.width, CV_32F);
for (int i = 0; i < imageSizeY; i++)
for (int j = 0; j < imageSizeX; j++)
{
const pcl::PointWithRange p = range_image_planar.getPoint(i * imageSizeX + j) ;
// ROS_INFO("pixel (%d %d) = %f ", i, j, p.z);
img.at<float>(i * imageSizeX + j ) = p.x - min;
// if (p.z > LASER_RANGE_MAX)
// img.at<float>(i * imageSizeX + j ) = LASER_RANGE_MAX;
}
cv::Mat bwimg;
cv::convertScaleAbs(img, bwimg, 255/max); //(max - min));
cv::Mat gaussimg = bwimg.clone();
cv::GaussianBlur(bwimg, gaussimg, cv::Size(3, 3), 0, 0);
// cv::Mat proba;
// cv::convertScaleAbs(img_corrected, proba, 255/(max - min));
// cv::Mat gaussimg_2 = proba.clone();
// cv::GaussianBlur(proba, gaussimg_2, cv::Size(3, 3), 0, 0);
// if (counter % 2 == 0)
// {
//ROS_INFO("Counter = %d", counter);
viewer.showRangeImage(range_image_planar);
imshow("depth", gaussimg);
//imshow("corrected", gaussimg_2);
// }
// counter = counter + 1;
// cv::Mat img(range_image.width, range_image.height, CV_32F);
// for (size_t i = 0; i < (range_image.points.size()); i++)
// {
// float z = range_image.getPoint(i).z;
// img.at<float>(i)=z;
// }
// imshow("depth", img);
cv::waitKey(1);
// // Convert to ROS data type
// sensor_msgs::PointCloud2 output;
// pcl_conversions::fromPCL(cloud_filtered, output);
// // Publish the data.
// pub.publish (output);
viewer.spinOnce ();
pcl_sleep (0.01);
//t.sleep();
}
void AlchimedesBuoyancy(DemoEntityManager* const scene)
{
// load the sky box
scene->CreateSkyBox();
// load the mesh
CreateLevelMesh (scene, "swimmingPool.ngd", true);
// add a trigger Manager to the world
MyTriggerManager* const triggerManager = new MyTriggerManager(scene->GetNewton());
dMatrix triggerLocation (dGetIdentityMatrix());
triggerLocation.m_posit.m_x = 17.0f;
triggerLocation.m_posit.m_y = -3.5f;
NewtonCollision* const poolBox = NewtonCreateBox (scene->GetNewton(), 30.0f, 6.0f, 20.0f, 0, NULL);
triggerManager->CreateBuoyancyTrigger (triggerLocation, poolBox);
NewtonDestroyCollision (poolBox);
// customize the scene after loading
// set a user friction variable in the body for variable friction demos
// later this will be done using LUA script
dMatrix offsetMatrix (dGetIdentityMatrix());
// place camera into position
dMatrix camMatrix (dGetIdentityMatrix());
dQuaternion rot (camMatrix);
dVector origin (-20.0f, 10.0f, 0.0f, 0.0f);
scene->SetCameraMatrix(rot, origin);
int defaultMaterialID = NewtonMaterialGetDefaultGroupID (scene->GetNewton());
/*
//test buoyancy on scaled collisions
dVector plane (0.0f, 1.0f, 0.0f, 0.0f);
dMatrix L1 (dPitchMatrix(30.0f * 3.141692f / 180.0f) * dYawMatrix(0.0f * 3.141692f / 180.0f) * dRollMatrix(0.0f * 3.141692f / 180.0f));
NewtonCollision* xxx0 = NewtonCreateCompoundCollision(scene->GetNewton(), 0);
NewtonCompoundCollisionBeginAddRemove(xxx0);
NewtonCollision* xxxx0 = NewtonCreateBox(scene->GetNewton(), 1.0f, 2.0f, 1.0f, 0, &L1[0][0]);
NewtonCompoundCollisionAddSubCollision(xxx0, xxxx0);
NewtonCompoundCollisionEndAddRemove(xxx0);
NewtonCollision* xxx1 = NewtonCreateCompoundCollision(scene->GetNewton(), 0);
NewtonCollision* xxxx1 = NewtonCreateBox(scene->GetNewton(), 1.0f, 1.0f, 1.0f, 0, &L1[0][0]);
NewtonCompoundCollisionAddSubCollision(xxx1, xxxx1);
NewtonCompoundCollisionEndAddRemove(xxx1);
NewtonCollisionSetScale(xxx1, 1.0f, 2.0f, 1.0f);
//dMatrix m (dPitchMatrix(45.0f * 3.141692f / 180.0f) * dYawMatrix(40.0f * 3.141692f / 180.0f) * dRollMatrix(70.0f * 3.141692f / 180.0f));
dMatrix m (dPitchMatrix(0.0f * 3.141692f / 180.0f) * dYawMatrix(0.0f * 3.141692f / 180.0f) * dRollMatrix(0.0f * 3.141692f / 180.0f));
dVector gravity (0.0f, 0.0f, -9.8f, 0.0f);
dVector cog0 (0.0f, 0.0f, 0.0f, 0.0f);
dVector accelPerUnitMass0;
dVector torquePerUnitMass0;
NewtonConvexCollisionCalculateBuoyancyAcceleration (xxx0, &m[0][0], &cog0[0], &gravity[0], &plane[0], 1.0f, 0.1f, &accelPerUnitMass0[0], &torquePerUnitMass0[0]);
dVector cog1 (0.0f, 0.0f, 0.0f, 0.0f);
dVector accelPerUnitMass1;
dVector torquePerUnitMass1;
NewtonConvexCollisionCalculateBuoyancyAcceleration (xxx1, &m[0][0], &cog1[0], &gravity[0], &plane[0], 1.0f, 0.1f, &accelPerUnitMass1[0], &torquePerUnitMass1[0]);
*/
int count = 5;
dVector size (1.0f, 0.25f, 0.5f);
dVector location (10.0f, 0.0f, 0.0f, 0.0f);
dMatrix shapeOffsetMatrix (dGetIdentityMatrix());
// AddPrimitiveArray(scene, 10.0f, location, size, count, count, 5.0f, _SPHERE_PRIMITIVE, defaultMaterialID, shapeOffsetMatrix);
AddPrimitiveArray(scene, 10.0f, location, size, count, count, 5.0f, _BOX_PRIMITIVE, defaultMaterialID, shapeOffsetMatrix);
// AddPrimitiveArray(scene, 10.0f, location, size, count, count, 5.0f, _CAPSULE_PRIMITIVE, defaultMaterialID, shapeOffsetMatrix);
// AddPrimitiveArray(scene, 10.0f, location, size, count, count, 5.0f, _CYLINDER_PRIMITIVE, defaultMaterialID, shapeOffsetMatrix);
// AddPrimitiveArray(scene, 10.0f, location, size, count, count, 5.0f, _CONE_PRIMITIVE, defaultMaterialID, shapeOffsetMatrix);
// AddPrimitiveArray(scene, 10.0f, location, size, count, count, 5.0f, _CHAMFER_CYLINDER_PRIMITIVE, defaultMaterialID, shapeOffsetMatrix);
// AddPrimitiveArray(scene, 10.0f, location, size, count, count, 5.0f, _REGULAR_CONVEX_HULL_PRIMITIVE, defaultMaterialID, shapeOffsetMatrix);
// AddPrimitiveArray(scene, 10.0f, location, size, count, count, 5.0f, _COMPOUND_CONVEX_CRUZ_PRIMITIVE, defaultMaterialID, shapeOffsetMatrix);
/*
for (NewtonBody* bodyPtr = NewtonWorldGetFirstBody(scene->GetNewton()); bodyPtr; bodyPtr = NewtonWorldGetNextBody(scene->GetNewton(), bodyPtr)) {
NewtonCollision* collision = NewtonBodyGetCollision(bodyPtr);
if (NewtonCollisionGetType(collision) == SERIALIZE_ID_COMPOUND) {
NewtonCollisionSetScale (collision, 0.5f, 0.5f, 0.5f);
}
}
*/
// AddPrimitiveArray(scene, 10.0f, location, size, count, count, 5.0f, _RANDOM_CONVEX_HULL_PRIMITIVE, defaultMaterialID, shapeOffsetMatrix);
}
namespace MATPACK {
//-----------------------------------------------------------------------------//
static const float defFOV(45);
static const Trafo defT (trans(0,0,-5) // view transformation matrix
* rot(Vector3D(0,1,0),M_PI/5)
* rot(Vector3D(1,0,0),M_PI/3)
* rot(Vector3D(0,1,0),-M_PI/10)
* rot(Vector3D(0,0,1),-M_PI/2)
* rot(Vector3D(1,0,0),-M_PI/2));
//-----------------------------------------------------------------------------//
static bool Valid2dPlotForReal2dArray (int plotype2d)
{
switch(plotype2d) {
case MpViewGraphData::Two_Dim_Contour_Bezier_Plot:
case MpViewGraphData::Two_Dim_Contour_BSpline_Plot:
case MpViewGraphData::Two_Dim_Contour_Linear_Plot:
case MpViewGraphData::Two_Dim_Contour_Cubic_Fill_Plot:
case MpViewGraphData::Two_Dim_Contour_Cubic_Fill_Line_Plot:
case MpViewGraphData::Two_Dim_Density_Plot:
case MpViewGraphData::Two_Dim_Image_Linear_Plot:
case MpViewGraphData::Two_Dim_Image_Linear_Diffusive_Plot:
case MpViewGraphData::Two_Dim_Image_Linear_Ordered_Plot:
case MpViewGraphData::Two_Dim_Image_Linear_Coarse_Plot:
case MpViewGraphData::Two_Dim_Image_Bezier_Plot:
case MpViewGraphData::Two_Dim_Image_Bezier_Diffusive_Plot:
case MpViewGraphData::Two_Dim_Image_Bezier_Ordered_Plot:
case MpViewGraphData::Two_Dim_Image_Bezier_Coarse_Plot:
case MpViewGraphData::Two_Dim_Lambert_Plot:
case MpViewGraphData::Two_Dim_Box_Plot_Squares:
case MpViewGraphData::Two_Dim_Box_Plot_Circles:
case MpViewGraphData::Two_Dim_Density_Checkerboard:
return true;
default:
return false;
}
}
static bool Valid2dPlotForComplex2dArray (int plotype2d)
{
switch(plotype2d) {
case MpViewGraphData::Two_Dim_Field_Abs_Plot:
case MpViewGraphData::Two_Dim_Field_Phase_Plot:
case MpViewGraphData::Two_Dim_Field_Thick_Plot:
return true;
default:
return false;
}
}
static bool Valid3dPlotForReal2dArray (int plotype3d)
{
switch(plotype3d) {
case MpViewGraphData::Three_Dim_Surface_Plot:
case MpViewGraphData::Three_Dim_Contour_Plot:
case MpViewGraphData::Three_Dim_Dots_Plot:
case MpViewGraphData::Three_Dim_Poles_Plot:
case MpViewGraphData::Three_Dim_Scatter_Plot:
case MpViewGraphData::Three_Dim_Project_Plot:
return true;
default:
return false;
}
}
static bool Valid3dPlotForComplex2dArray (int plotype3d)
{
switch(plotype3d) {
case MpViewGraphData::Three_Dim_Complex_Surface_Plot:
return true;
default:
return false;
}
}
static bool Valid3dPlotForXYZArray (int plotype3d)
{
switch(plotype3d) {
case MpViewGraphData::Three_Dim_Poles_Plot:
case MpViewGraphData::Three_Dim_Scatter_Plot:
return true;
default:
return false;
}
}
static bool Valid4dPlotForReal3dArray (int plotype4d)
{
switch(plotype4d) {
case MpViewGraphData::Four_Dim_Contour_Plot:
case MpViewGraphData::Four_Dim_Scatter_Plot:
case MpViewGraphData::Four_Dim_Slice_Plot:
return true;
default:
return false;
}
}
//-----------------------------------------------------------------------------//
// default view settings
//-----------------------------------------------------------------------------//
void MpView::DefaultViewForMatrix (MpViewGraphData *G)
{
if (!G) return;
// define the 2D and 3D viewbox
int nx=0, ny=0;
if (G->DS().isMatrix()) {
nx = G->DS().Z.Rows();
ny = G->DS().Z.Cols();
} else if (G->DS().isComplexMatrix()) {
nx = G->DS().C.Rows();
ny = G->DS().C.Cols();
}
double lim = 0.05; // i.e. 1 : 20 maximal
// make true aspect ratios in x:y (within limits 1:20)
G->vbx = G->vby = 1;
G->shrink2d = 0.15;
if (nx > ny) {
G->vby = max((double)ny/nx,lim);
} else if (ny > nx) {
G->vbx = max((double)nx/ny,lim);
}
// flat box is nicer in most cases
G->vbz = 0.5;
G->viewtrafo = defT;
G->fov = defFOV;
}
//-----------------------------------------------------------------------------//
void MpView::DefaultViewForMatrix3d (MpViewGraphData *G)
{
if (!G) return;
int nx = G->DS().V.Slices(),
ny = G->DS().V.Rows(),
nz = G->DS().V.Columns();
G->vbx = G->vby = G->vbz = 1.0;
// make true aspect ratios in x:y:z (within limits)
double lim = 0.05; // i.e. 1 : 20 maximal
if (nx >= ny && nx >= nz) {
G->vby = max((double)ny/nx,lim);
G->vbz = max((double)nz/nx,lim);
} else if (ny >= nx && ny >= nz) {
G->vbx = max((double)nx/ny,lim);
G->vbz = max((double)nz/ny,lim);
} else if (nz >= nx && nz >= ny) {
G->vbx = max((double)nx/nz,lim);
G->vby = max((double)ny/nz,lim);
}
G->viewtrafo = defT;
G->fov = defFOV;
}
//-----------------------------------------------------------------------------//
void MpView::DefaultViewForXYZSet (MpViewGraphData *G)
{
if (!G) return;
// get bounding box
double bx1 = Min(G->DS().x), bx2 = Max(G->DS().x), nx = abs(bx2-bx1),
by1 = Min(G->DS().y), by2 = Max(G->DS().y), ny = abs(by2-by1),
bz1 = Min(G->DS().z), bz2 = Max(G->DS().z), nz = abs(bz2-bz1);
G->vbx = G->vby = G->vbz = 1.0;
// make true aspect ratios in x:y:z (within limits)
double lim = 0.05; // i.e. 1 : 20 maximal
if (nx >= ny && nx >= nz) {
G->vby = max((double)ny/nx,lim);
G->vbz = max((double)nz/nx,lim);
} else if (ny >= nx && ny >= nz) {
G->vbx = max((double)nx/ny,lim);
G->vbz = max((double)nz/ny,lim);
} else if (nz >= nx && nz >= ny) {
G->vbx = max((double)nx/nz,lim);
G->vby = max((double)ny/nz,lim);
}
G->viewtrafo = defT;
G->fov = defFOV;
}
//-----------------------------------------------------------------------------//
void MpView::DefaultViewForScene (MpViewGraphData *G)
{
if (!G) return;
Scene& S = G->DS().S;
// calculate bounding box
float x1,x2,y1,y2,z1,z2;
S.Extent(x1,x2,y1,y2,z1,z2);
// view box ratios x:y:z
const float f = 0.5; // scale factor
G->vby = f*(x2-x1); // note ordering (y-z-x) - the internal coordinate system
G->vbz = f*(y2-y1);
G->vbx = f*(z2-z1);
// choose an initial viewpoint depending on extent of scene
// look to the center of the scene
float cx = (x1+x2)/2,
cy = (y1+y2)/2,
cz = (z1+z2)/2,
r = 1.7 * hypot(x1-x2,y1-y2,z1-z2);
S.Look(Vector3D(cx+r,cy+r,cz+r), // define camera position
Vector3D(-r,-r,-r), // the look direction
FieldOfView(23)); // and the view angle fov
// set transformation matrix and fov angle
G->viewtrafo = S.GetMatrix();
G->fov = S.GetFieldOfView();
}
//-----------------------------------------------------------------------------//
// MpView::NewMatrix:
// initialize when matrix is newly loaded
//-----------------------------------------------------------------------------//
void MpView::NewMatrix (MpViewGraphData *G)
{
if (!G) return;
MpViewDataSet &DS = G->DS();
// load colormap
if (! G->theColorMap) {
string path = string(getenv(MatpackHome)) + "/cmaps/earth-512.cmp";
G->theColorMap.Load(path.c_str());
}
// set data type
const Matrix &Z = DS.MatrixData();
if (Z.Empty()) {
MpViewDisplay::Message("MpView::NewMatrix: no data available !!");
return;
}
// reset axis labeling
G->Axis[0].LabelMin = Z.Rlo(); // x-axis
G->Axis[0].LabelMax = Z.Rhi();
G->Axis[1].LabelMin = Z.Clo(); // y-axis
G->Axis[1].LabelMax = Z.Chi();
double z1 = Min(Z),
z2 = Max(Z);
// reset logarithmic representation
G->log_z = false;
// reset contours
if (G->num_contour_level_2D == 0) { // if no contour levels are allocated
G->num_contour_level_2D = 10;
G->contour_level_2D = new MpContourLevel[G->num_contour_level_2D];
}
double cstep = (z2-z1) / (G->num_contour_level_2D+1);
for (int i = 0; i < G->num_contour_level_2D; i++)
G->contour_level_2D[i].level = z1 + (i+1) * cstep;
// reset base of needles in scatter plot
G->Three_Dim_Poles_Base = z1;
// reset axis labeling
G->Axis[2].LabelMin = z1; // z-axis
G->Axis[2].LabelMax = z2;
G->colormap_legend_axis.LabelMin = z1;
G->colormap_legend_axis.LabelMax = z2;
DefaultViewForMatrix(G); // define the 3D viewbox
if ( (G->plotdim == MpViewGraphData::Two_Dim_Plot && Valid2dPlotForReal2dArray(G->plotype2d)) ||
G->plotdim == MpViewGraphData::Three_Dim_Plot && Valid3dPlotForReal2dArray(G->plotype3d)) {
// previous settings are left unchanged
} else {
if (Z.Elements() > 10000) {
G->plotdim = MpViewGraphData::Two_Dim_Plot;
G->plotype2d = MpViewGraphData::Two_Dim_Image_Linear_Plot;
} else if (Z.Elements() > 1600) {
G->plotdim = MpViewGraphData::Two_Dim_Plot;
G->plotype2d = MpViewGraphData::Two_Dim_Contour_Bezier_Plot;
} else {
G->plotdim = MpViewGraphData::Three_Dim_Plot;
G->plotype3d = MpViewGraphData::Three_Dim_Surface_Plot;
}
}
// check if only one row or column given, in this case no 2d plots are
// available and in 3d-mode upto now only squares are available
if (Z.Rhi() <= Z.Rlo() || Z.Chi() <= Z.Clo()) {
G->plotdim = MpViewGraphData::Three_Dim_Plot;
G->plotype3d = MpViewGraphData::Three_Dim_Surface_Plot;
}
}
//-----------------------------------------------------------------------------//
// MpView::NewComplexMatrix:
// initialize when complex matrix is newly loaded
//-----------------------------------------------------------------------------//
void MpView::NewComplexMatrix (MpViewGraphData *G, const ComplexMatrix &New)
{
if (!G) return;
MpViewDataSet &DS = G->DS();
// load periodic colormap - only if there is not already a colormap
if (! G->theColorMap) {
string path = string(getenv(MatpackHome)) + "/cmaps/prism-256.cmp";
G->theColorMap.Load(path.c_str());
}
// set data type
const ComplexMatrix &C = DS.ComplexMatrixData();
if (C.Empty()) {
MpViewDisplay::Message("MpView::NewComplexMatrix: no data available !!");
return;
}
if ( (G->plotdim == MpViewGraphData::Two_Dim_Plot && Valid2dPlotForComplex2dArray(G->plotype2d)) ||
G->plotdim == MpViewGraphData::Three_Dim_Plot && Valid3dPlotForComplex2dArray(G->plotype3d)) {
// previous settings are left unchanged
} else {
G->plotdim = MpViewGraphData::Three_Dim_Plot;
G->plotype3d = MpViewGraphData::Three_Dim_Complex_Surface_Plot;
G->complex_representation = MpViewGraphData::Cplx_Norm;
}
// reset axis labeling
G->Axis[0].LabelMin = C.Rlo(); // x-axis
G->Axis[0].LabelMax = C.Rhi();
G->Axis[1].LabelMin = C.Clo(); // y-axis
G->Axis[1].LabelMax = C.Chi();
G->Axis[2].LabelMin = norm(Min(C)); // z-axis
G->Axis[2].LabelMax = norm(Max(C));
G->colormap_legend_axis.LabelMin = G->Axis[2].LabelMin;
G->colormap_legend_axis.LabelMax = G->Axis[2].LabelMax;
DefaultViewForMatrix(G); // define the 3D viewbox
}
//----------------------------------------------------------------------------//
void MpView::NewVolume (MpViewGraphData *G)
{
if (!G) return;
MpViewDataSet &DS = G->DS();
// load colormap
if (! G->theColorMap) {
string path = string(getenv(MatpackHome)) + "/cmaps/prism-256.cmp";
G->theColorMap.Load(path.c_str());
}
// set data type
const MpMatrix3d<double> &V = DS.Matrix3dData();
if (V.Empty()) {
MpViewDisplay::Message("MpView::InitializeForVolume: no data available !!");
return;
}
if (G->plotdim == MpViewGraphData::Four_Dim_Plot && Valid4dPlotForReal3dArray(G->plotype4d)) {
// previous settings are left unchanged
} else {
G->plotdim = MpViewGraphData::Four_Dim_Plot;
G->plotype4d = MpViewGraphData::Four_Dim_Contour_Plot;
}
// lower and upper bound for 4d scatter plot
double maxi = Max(V),
mini = Min(V);
G->Four_Dim_Scatter_Lower = maxi-0.1*(maxi-mini);
G->Four_Dim_Scatter_Upper = maxi;
// computation of a 75%-quantile seems to be a good startup value for the
// surface level, i.e. 75% of the volume is below this value
double qq = 0, sdev = 0;
// catch n <= 2 case to avoid error exit of Quantile function
if (V.Elements() <= 2)
qq = 0.5*(maxi+mini);
else
MpQuantile(V, 2, 0.75, qq, sdev);
// set this value for all isosurfaces
for (int i = 0; i < MpViewIsosurfaceData::MaxIsosurf; i++)
G->IsoSurface[i].Four_Dim_Isosurface_Level = qq;
// activate first one
G->IsoSurface[0].Four_Dim_Isosurface_Active = true;
G->Axis[0].LabelMin = V.Slo(); // x-axis
G->Axis[0].LabelMax = V.Shi();
G->Axis[1].LabelMin = V.Rlo(); // y-axis
G->Axis[1].LabelMax = V.Rhi();
G->Axis[2].LabelMin = V.Clo(); // z-axis
G->Axis[2].LabelMax = V.Chi();
// define the 3D viewbox
DefaultViewForMatrix3d(G);
}
//----------------------------------------------------------------------------//
void MpView::NewXYZSet (MpViewGraphData *G)
{
if (!G) return;
MpViewDataSet &DS = G->DS();
// load colormap
if (! G->theColorMap) {
string path = string(getenv(MatpackHome)) + "/cmaps/prism-256.cmp";
G->theColorMap.Load(path.c_str());
}
// set data type
DS.theDataType = MpViewDataSet::a_XYZSet;
if (DS.x.Empty() || DS.y.Empty() || DS.z.Empty()) {
MpViewDisplay::Message("MpView::NewXYZSet: no data available !!");
return;
}
if (G->plotdim == MpViewGraphData::Three_Dim_Plot && Valid3dPlotForXYZArray(G->plotype3d)) {
// previous settings are left unchanged
} else {
G->plotdim = MpViewGraphData::Three_Dim_Plot;
G->plotype3d = MpViewGraphData::Three_Dim_Scatter_Plot;
}
// define the 3D viewbox
DefaultViewForXYZSet(G);
}
//----------------------------------------------------------------------------//
// MpView::InitializeForScene
// initialize several variables when scene has changed
//----------------------------------------------------------------------------//
void MpView::NewScene (MpViewGraphData *G)
{
if (!G) return;
MpViewDataSet &DS = G->DS();
// load periodic colormap - only if there is not already a colormap
if (! G->theColorMap) {
string path = string(getenv(MatpackHome)) + "/cmaps/prism-256.cmp";
G->theColorMap.Load(path.c_str());
}
// set data type
Scene& S = DS.SceneData();
G->plotdim = MpViewGraphData::Three_Dim_Plot;
float x1,x2,y1,y2,z1,z2; // bounding box
S.Extent(x1,x2,y1,y2,z1,z2);
// set clipping volume
G->cx1 = x1;
G->cx2 = x2;
G->cy1 = y1;
G->cy2 = y2;
G->cz1 = z1;
G->cz2 = z2;
// define the 3D viewbox
DefaultViewForScene(G);
#ifdef DEBUG
cout << "Scene["<<x1<<","<<x2<<","
<<y1<<","<<y2<<","
<<z1<<","<<z2<<"]"<<endl;
#endif
}
//----------------------------------------------------------------------------//
// If graph_index < 0 find all graphs that link into the selected data set and
// switch to the default settings.
// If graph_index > 0 then change settings only for the selected graph.
//----------------------------------------------------------------------------//
void MpView::NewDataSet (int dataset_index, int graph_index)
{
if (dataset_index < 0) return;
for (MpViewGraphMap::iterator i = Graphs.begin(); i != Graphs.end(); ++i) {
int grf = i->first;
if (dataset_index == i->second.iset
&& (graph_index < 0 || graph_index == grf)) {
MpViewGraphData *G = &GD(grf);
if (DataSetExists(dataset_index)) {
switch (G->DS().GetDataType()) {
case MpViewDataSet::a_EmptySet:
// nothing to do
break;
case MpViewDataSet::a_Matrix:
NewMatrix(G);
break;
case MpViewDataSet::a_Matrix3d:
NewVolume(G);
break;
case MpViewDataSet::a_ComplexMatrix:
NewComplexMatrix(G,G->DS().C);
break;
case MpViewDataSet::a_Scene:
NewScene(G);
break;
case MpViewDataSet::a_XYZSet:
NewXYZSet(G);
break;
case MpViewDataSet::a_Image:
cerr<<"MpView::NewDataSet: IMAGE - can't happen"<<endl;
break;
default:
cerr<<"MpView::NewDataSet: found illegal data set type" << endl;
break;
}
} else {
// data set doesn't exist (e.g. was deleted) -> unlink
G->iset = -1;
}
}
}
}
} // namespace MATPACK
