//--------------------------------------------------------------
ofCylinderPrimitive::ofCylinderPrimitive() {
texCoords = ofVec4f(0,0,1,1);
set( 60, 80, 6, 3, 2, true );
}
//--------------------------------------------------------------
ofCylinderPrimitive::ofCylinderPrimitive( float radius, float height, int radiusSegments, int heightSegments, int capSegments, bool bCapped, ofPrimitiveMode mode ) {
texCoords = ofVec4f(0,0,1,1);
set( radius, height, radiusSegments, heightSegments, capSegments, bCapped, mode );
}
//--------------------------tsdf计算-----------------------------
void testApp::compute_tsdf(const ofMatrix4x4 &t_g,cv::Mat &depthimage,const int g_size,const ofVec4f &camP)
{
//ofVec3f iy = nmmul(invKcam,ofVec3f(0,0,1));
// 写入文件
//ofstream out;
//out.open("data.txt",ios::trunc);
// 显示截平面的tsdf数据
cv::Mat color(g_size,g_size,CV_8UC1);
cv::Mat bigcol;//(KINECT_HEIGHT,KINECT_WIDTH,CV_8UC1);
cv::Mat bbcol;
ofMatrix4x4 invTrans = t_g.getInverse();
float changenum = 0.0;
float dist = 0.0;
float tsdf = 0.0;
int wight = 0;
float pretsdf = 0.0;
int preweight = 0;
int i,j,k;
int num = 0;
for(k=0;k<g_size;k++)
{
for(j=0;j<g_size;j++)
for(i=0;i<g_size;i++)
{
ofVec4f spacePostion = ofVec4f(-2.0+i*SIZE_TRIDE,-2.0+j*SIZE_TRIDE,0.5+k*SIZE_TRIDE,1.0);
ofVec4f camPostion = invTrans*spacePostion;
ofVec3f dcamPostion = ofVec3f(camPostion.x,camPostion.y,camPostion.z);
ofVec3f picPosition = nmmul(the_K_cam,dcamPostion);
ofVec2f depthPostion = ofVec2f(picPosition.x/picPosition.z,picPosition.y/picPosition.z);
float minus = 0;
int x = (int)(max(minus,depthPostion.x)+0.5);
int y = (int)(max(minus,depthPostion.y)+0.5);
//cout<<x<<" "<<y<<endl;
x = min(x,KINECT_WIDTH-1);
y = min(y,KINECT_HEIGHT-1);
//cout<<x<<" "<<y<<endl;
ofVec3f rdepthPostion = ofVec3f(x,y,1);
changenum = nmmul(invKcam,rdepthPostion).length();
dist = spacePostion.distance(camP);
float dd = depth_float[x+y*KINECT_WIDTH];
if(dd>0)
tsdf =-(dist/changenum-depth_float[x+y*KINECT_WIDTH]);
else
tsdf = 100;
if(tsdf>0)
{
tsdf = min(1.0,tsdf/(SIZE_TRIDE*2));
}
else
{
tsdf = max(-1.0,tsdf/(SIZE_TRIDE*2));
}
// 写入文件与存入内存
//out<<tsdf<<" ";
//tsdfData[num] = tsdf;
num++;
const int nowweight = 1;
float newtsdf;
if(tsdfFirstTime)
{
newtsdf = tsdf;
wight = 1;
}
else
{
unpack_tsdf(pretsdf,preweight,num);
wight = max(128,preweight+nowweight);
newtsdf = (pretsdf*preweight+tsdf)/(wight,nowweight);
}
pack_tsdf(newtsdf,wight,num);
}
}
//out.close();
tsdfFirstTime = false;
cv::imshow("zzz",color);
}
//----------------------------------------------------------
ofIcoSpherePrimitive::ofIcoSpherePrimitive( float _radius, int iterations ) {
texCoords = ofVec4f(0,0,1,1);
radius = _radius;
setResolution( iterations );
}
RagDoll::RagDoll (btDynamicsWorld* ownerWorld, const btVector3& positionOffset,
btScalar scale_ragdoll) : m_ownerWorld (ownerWorld) {
// setup colors
for (int i = 0; i < BODYPART_COUNT; i++) {
// ofVec4f col = ofVec4f(ofRandom(0.7, 1.0),
// ofRandom(0.5, 1.0),
// ofRandom(0.7, 1.0),
// 1.0);
ofVec4f col = ofVec4f(0.7,
0.7,
0.7,
1.0);
colors.push_back(col);
}
// Setup the geometry
m_shapes[BODYPART_PELVIS] = new btCapsuleShape(btScalar(scale_ragdoll*0.15), btScalar(scale_ragdoll*0.20));
m_shapes[BODYPART_SPINE] = new btCapsuleShape(btScalar(scale_ragdoll*0.15), btScalar(scale_ragdoll*0.28));
m_shapes[BODYPART_HEAD] = new btCapsuleShape(btScalar(scale_ragdoll*0.10), btScalar(scale_ragdoll*0.05));
m_shapes[BODYPART_LEFT_UPPER_LEG] = new btCapsuleShape(btScalar(scale_ragdoll*0.07), btScalar(scale_ragdoll*0.45));
m_shapes[BODYPART_LEFT_LOWER_LEG] = new btCapsuleShape(btScalar(scale_ragdoll*0.05), btScalar(scale_ragdoll*0.37));
m_shapes[BODYPART_RIGHT_UPPER_LEG] = new btCapsuleShape(btScalar(scale_ragdoll*0.07), btScalar(scale_ragdoll*0.45));
m_shapes[BODYPART_RIGHT_LOWER_LEG] = new btCapsuleShape(btScalar(scale_ragdoll*0.05), btScalar(scale_ragdoll*0.37));
m_shapes[BODYPART_LEFT_UPPER_ARM] = new btCapsuleShape(btScalar(scale_ragdoll*0.05), btScalar(scale_ragdoll*0.33));
m_shapes[BODYPART_LEFT_LOWER_ARM] = new btCapsuleShape(btScalar(scale_ragdoll*0.04), btScalar(scale_ragdoll*0.25));
m_shapes[BODYPART_RIGHT_UPPER_ARM] = new btCapsuleShape(btScalar(scale_ragdoll*0.05), btScalar(scale_ragdoll*0.33));
m_shapes[BODYPART_RIGHT_LOWER_ARM] = new btCapsuleShape(btScalar(scale_ragdoll*0.04), btScalar(scale_ragdoll*0.25));
// Setup all the rigid bodies
btTransform offset; offset.setIdentity();
offset.setOrigin(positionOffset);
// fix the rotation
double angle = (double)180*(TWO_PI/360.0);
btVector3 zaxis(0.0,0.0,1.0);
btQuaternion zquat(zaxis,angle);
btVector3 yaxis(0.0,1.0,0.0);
btQuaternion yquat(yaxis,angle);
btQuaternion quat = zquat*yquat;
offset.setRotation(quat);
btTransform transform;
transform.setIdentity();
transform.setOrigin(btVector3(btScalar(0.), btScalar(scale_ragdoll*1.), btScalar(0.)));
m_bodies[BODYPART_PELVIS] = localCreateRigidBody(btScalar(1.), offset*transform, m_shapes[BODYPART_PELVIS]);
transform.setIdentity();
transform.setOrigin(btVector3(btScalar(0.), btScalar(scale_ragdoll*1.2), btScalar(0.)));
m_bodies[BODYPART_SPINE] = localCreateRigidBody(btScalar(1.), offset*transform, m_shapes[BODYPART_SPINE]);
transform.setIdentity();
transform.setOrigin(btVector3(btScalar(0.), btScalar(scale_ragdoll*1.6), btScalar(0.)));
m_bodies[BODYPART_HEAD] = localCreateRigidBody(btScalar(1.), offset*transform, m_shapes[BODYPART_HEAD]);
transform.setIdentity();
transform.setOrigin(btVector3(btScalar(-0.18*scale_ragdoll), btScalar(0.65*scale_ragdoll),
btScalar(0.)));
m_bodies[BODYPART_LEFT_UPPER_LEG] = localCreateRigidBody(btScalar(1.), offset*transform, m_shapes[BODYPART_LEFT_UPPER_LEG]);
transform.setIdentity();
transform.setOrigin(btVector3(btScalar(-0.18*scale_ragdoll), btScalar(0.2*scale_ragdoll), btScalar(0.)));
m_bodies[BODYPART_LEFT_LOWER_LEG] = localCreateRigidBody(btScalar(1.), offset*transform, m_shapes[BODYPART_LEFT_LOWER_LEG]);
transform.setIdentity();
transform.setOrigin(btVector3(btScalar(0.18*scale_ragdoll), btScalar(0.65*scale_ragdoll), btScalar(0.)));
m_bodies[BODYPART_RIGHT_UPPER_LEG] = localCreateRigidBody(btScalar(1.), offset*transform, m_shapes[BODYPART_RIGHT_UPPER_LEG]);
transform.setIdentity();
transform.setOrigin(btVector3(btScalar(0.18*scale_ragdoll), btScalar(0.2*scale_ragdoll), btScalar(0.)));
m_bodies[BODYPART_RIGHT_LOWER_LEG] = localCreateRigidBody(btScalar(1.), offset*transform, m_shapes[BODYPART_RIGHT_LOWER_LEG]);
transform.setIdentity();
transform.setOrigin(btVector3(btScalar(-0.35*scale_ragdoll), btScalar(1.45*scale_ragdoll), btScalar(0.)));
transform.getBasis().setEulerZYX(0,0,SIMD_HALF_PI);
m_bodies[BODYPART_LEFT_UPPER_ARM] = localCreateRigidBody(btScalar(1.), offset*transform, m_shapes[BODYPART_LEFT_UPPER_ARM]);
transform.setIdentity();
transform.setOrigin(btVector3(btScalar(-0.7*scale_ragdoll), btScalar(1.45*scale_ragdoll), btScalar(0.)));
transform.getBasis().setEulerZYX(0,0,SIMD_HALF_PI);
m_bodies[BODYPART_LEFT_LOWER_ARM] = localCreateRigidBody(btScalar(1.), offset*transform, m_shapes[BODYPART_LEFT_LOWER_ARM]);
transform.setIdentity();
transform.setOrigin(btVector3(btScalar(0.35*scale_ragdoll), btScalar(1.45*scale_ragdoll), btScalar(0.)));
transform.getBasis().setEulerZYX(0,0,-SIMD_HALF_PI);
m_bodies[BODYPART_RIGHT_UPPER_ARM] = localCreateRigidBody(btScalar(1.), offset*transform, m_shapes[BODYPART_RIGHT_UPPER_ARM]);
transform.setIdentity();
transform.setOrigin(btVector3(btScalar(0.7*scale_ragdoll), btScalar(1.45*scale_ragdoll), btScalar(0.)));
transform.getBasis().setEulerZYX(0,0,-SIMD_HALF_PI);
m_bodies[BODYPART_RIGHT_LOWER_ARM] = localCreateRigidBody(btScalar(1.), offset*transform, m_shapes[BODYPART_RIGHT_LOWER_ARM]);
// Setup some damping on the m_bodies
for (int i = 0; i < BODYPART_COUNT; ++i)
{
m_bodies[i]->setDamping(0.05, 0.85);
m_bodies[i]->setDeactivationTime(0.8);
m_bodies[i]->setSleepingThresholds(1.6, 2.5);
}
///////////////////////////// SETTING THE CONSTRAINTS /////////////////////////////////////////////7777
// Now setup the constraints
btGeneric6DofConstraint * joint6DOF;
btTransform localA, localB;
bool useLinearReferenceFrameA = true;
/// ******* SPINE HEAD ******** ///
{
localA.setIdentity(); localB.setIdentity();
localA.setOrigin(btVector3(btScalar(0.), btScalar(0.30*scale_ragdoll), btScalar(0.)));
localB.setOrigin(btVector3(btScalar(0.), btScalar(-0.14*scale_ragdoll), btScalar(0.)));
joint6DOF = new btGeneric6DofConstraint(*m_bodies[BODYPART_SPINE], *m_bodies[BODYPART_HEAD], localA, localB,useLinearReferenceFrameA);
#ifdef RIGID
joint6DOF->setAngularLowerLimit(btVector3(-SIMD_EPSILON,-SIMD_EPSILON,-SIMD_EPSILON));
joint6DOF->setAngularUpperLimit(btVector3(SIMD_EPSILON,SIMD_EPSILON,SIMD_EPSILON));
#else
joint6DOF->setAngularLowerLimit(btVector3(-SIMD_PI*0.3f,-SIMD_EPSILON,-SIMD_PI*0.3f));
joint6DOF->setAngularUpperLimit(btVector3(SIMD_PI*0.5f,SIMD_EPSILON,SIMD_PI*0.3f));
#endif
m_joints[JOINT_SPINE_HEAD] = joint6DOF;
m_ownerWorld->addConstraint(m_joints[JOINT_SPINE_HEAD], true);
}
/// *************************** ///
/// ******* LEFT SHOULDER ******** ///
{
localA.setIdentity(); localB.setIdentity();
localA.setOrigin(btVector3(btScalar(-0.2*scale_ragdoll), btScalar(0.15*scale_ragdoll), btScalar(0.)));
localB.getBasis().setEulerZYX(SIMD_HALF_PI,0,-SIMD_HALF_PI);
localB.setOrigin(btVector3(btScalar(0.), btScalar(-0.18*scale_ragdoll), btScalar(0.)));
joint6DOF = new btGeneric6DofConstraint(*m_bodies[BODYPART_SPINE], *m_bodies[BODYPART_LEFT_UPPER_ARM], localA, localB,useLinearReferenceFrameA);
#ifdef RIGID
joint6DOF->setAngularLowerLimit(btVector3(-SIMD_EPSILON,-SIMD_EPSILON,-SIMD_EPSILON));
joint6DOF->setAngularUpperLimit(btVector3(SIMD_EPSILON,SIMD_EPSILON,SIMD_EPSILON));
#else
joint6DOF->setAngularLowerLimit(btVector3(-SIMD_PI*0.8f,-SIMD_EPSILON,-SIMD_PI*0.5f));
joint6DOF->setAngularUpperLimit(btVector3(SIMD_PI*0.8f,SIMD_EPSILON,SIMD_PI*0.5f));
#endif
m_joints[JOINT_LEFT_SHOULDER] = joint6DOF;
m_ownerWorld->addConstraint(m_joints[JOINT_LEFT_SHOULDER], true);
}
/// *************************** ///
/// ******* RIGHT SHOULDER ******** ///
{
localA.setIdentity(); localB.setIdentity();
localA.setOrigin(btVector3(btScalar(0.2*scale_ragdoll), btScalar(0.15*scale_ragdoll), btScalar(0.)));
localB.getBasis().setEulerZYX(0,0,SIMD_HALF_PI);
localB.setOrigin(btVector3(btScalar(0.), btScalar(-0.18*scale_ragdoll), btScalar(0.)));
joint6DOF = new btGeneric6DofConstraint(*m_bodies[BODYPART_SPINE], *m_bodies[BODYPART_RIGHT_UPPER_ARM], localA, localB,useLinearReferenceFrameA);
#ifdef RIGID
joint6DOF->setAngularLowerLimit(btVector3(-SIMD_EPSILON,-SIMD_EPSILON,-SIMD_EPSILON));
joint6DOF->setAngularUpperLimit(btVector3(SIMD_EPSILON,SIMD_EPSILON,SIMD_EPSILON));
#else
joint6DOF->setAngularLowerLimit(btVector3(-SIMD_PI*0.8f,-SIMD_EPSILON,-SIMD_PI*0.5f));
joint6DOF->setAngularUpperLimit(btVector3(SIMD_PI*0.8f,SIMD_EPSILON,SIMD_PI*0.5f));
#endif
m_joints[JOINT_RIGHT_SHOULDER] = joint6DOF;
m_ownerWorld->addConstraint(m_joints[JOINT_RIGHT_SHOULDER], true);
}
/// *************************** ///
/// ******* LEFT ELBOW ******** ///
{
localA.setIdentity(); localB.setIdentity();
localA.setOrigin(btVector3(btScalar(0.), btScalar(0.18*scale_ragdoll), btScalar(0.)));
localB.setOrigin(btVector3(btScalar(0.), btScalar(-0.14*scale_ragdoll), btScalar(0.)));
joint6DOF = new btGeneric6DofConstraint (*m_bodies[BODYPART_LEFT_UPPER_ARM], *m_bodies[BODYPART_LEFT_LOWER_ARM], localA, localB,useLinearReferenceFrameA);
#ifdef RIGID
joint6DOF->setAngularLowerLimit(btVector3(-SIMD_EPSILON,-SIMD_EPSILON,-SIMD_EPSILON));
joint6DOF->setAngularUpperLimit(btVector3(SIMD_EPSILON,SIMD_EPSILON,SIMD_EPSILON));
#else
joint6DOF->setAngularLowerLimit(btVector3(-SIMD_EPSILON,-SIMD_EPSILON,-SIMD_EPSILON));
joint6DOF->setAngularUpperLimit(btVector3(SIMD_PI*0.7,SIMD_EPSILON,SIMD_EPSILON));
#endif
m_joints[JOINT_LEFT_ELBOW] = joint6DOF;
m_ownerWorld->addConstraint(m_joints[JOINT_LEFT_ELBOW], true);
}
/// *************************** ///
/// ******* RIGHT ELBOW ******** ///
{
localA.setIdentity(); localB.setIdentity();
localA.setOrigin(btVector3(btScalar(0.), btScalar(0.18*scale_ragdoll), btScalar(0.)));
localB.setOrigin(btVector3(btScalar(0.), btScalar(-0.14*scale_ragdoll), btScalar(0.)));
joint6DOF = new btGeneric6DofConstraint (*m_bodies[BODYPART_RIGHT_UPPER_ARM], *m_bodies[BODYPART_RIGHT_LOWER_ARM], localA, localB,useLinearReferenceFrameA);
#ifdef RIGID
joint6DOF->setAngularLowerLimit(btVector3(-SIMD_EPSILON,-SIMD_EPSILON,-SIMD_EPSILON));
joint6DOF->setAngularUpperLimit(btVector3(SIMD_EPSILON,SIMD_EPSILON,SIMD_EPSILON));
#else
joint6DOF->setAngularLowerLimit(btVector3(-SIMD_EPSILON,-SIMD_EPSILON,-SIMD_EPSILON));
joint6DOF->setAngularUpperLimit(btVector3(SIMD_PI*0.7,SIMD_EPSILON,SIMD_EPSILON));
#endif
m_joints[JOINT_RIGHT_ELBOW] = joint6DOF;
m_ownerWorld->addConstraint(m_joints[JOINT_RIGHT_ELBOW], true);
}
/// *************************** ///
/// ******* PELVIS ******** ///
{
localA.setIdentity(); localB.setIdentity();
localA.getBasis().setEulerZYX(0,SIMD_HALF_PI,0);
localA.setOrigin(btVector3(btScalar(0.), btScalar(0.15*scale_ragdoll), btScalar(0.)));
localB.getBasis().setEulerZYX(0,SIMD_HALF_PI,0);
localB.setOrigin(btVector3(btScalar(0.), btScalar(-0.15*scale_ragdoll), btScalar(0.)));
joint6DOF = new btGeneric6DofConstraint (*m_bodies[BODYPART_PELVIS], *m_bodies[BODYPART_SPINE], localA, localB,useLinearReferenceFrameA);
#ifdef RIGID
joint6DOF->setAngularLowerLimit(btVector3(-SIMD_EPSILON,-SIMD_EPSILON,-SIMD_EPSILON));
joint6DOF->setAngularUpperLimit(btVector3(SIMD_EPSILON,SIMD_EPSILON,SIMD_EPSILON));
#else
joint6DOF->setAngularLowerLimit(btVector3(-SIMD_PI*0.2,-SIMD_EPSILON,-SIMD_PI*0.3));
joint6DOF->setAngularUpperLimit(btVector3(SIMD_PI*0.2,SIMD_EPSILON,SIMD_PI*0.6));
#endif
m_joints[JOINT_PELVIS_SPINE] = joint6DOF;
m_ownerWorld->addConstraint(m_joints[JOINT_PELVIS_SPINE], true);
}
/// *************************** ///
/// ******* LEFT HIP ******** ///
{
localA.setIdentity(); localB.setIdentity();
localA.setOrigin(btVector3(btScalar(-0.18*scale_ragdoll), btScalar(-0.10*scale_ragdoll), btScalar(0.)));
localB.setOrigin(btVector3(btScalar(0.), btScalar(0.225*scale_ragdoll), btScalar(0.)));
joint6DOF = new btGeneric6DofConstraint(*m_bodies[BODYPART_PELVIS], *m_bodies[BODYPART_LEFT_UPPER_LEG], localA, localB,useLinearReferenceFrameA);
#ifdef RIGID
joint6DOF->setAngularLowerLimit(btVector3(-SIMD_EPSILON,-SIMD_EPSILON,-SIMD_EPSILON));
joint6DOF->setAngularUpperLimit(btVector3(SIMD_EPSILON,SIMD_EPSILON,SIMD_EPSILON));
#else
joint6DOF->setAngularLowerLimit(btVector3(-SIMD_HALF_PI*0.5,-SIMD_EPSILON,-SIMD_EPSILON));
joint6DOF->setAngularUpperLimit(btVector3(SIMD_HALF_PI*0.8,SIMD_EPSILON,SIMD_HALF_PI*0.6f));
#endif
m_joints[JOINT_LEFT_HIP] = joint6DOF;
m_ownerWorld->addConstraint(m_joints[JOINT_LEFT_HIP], true);
}
/// *************************** ///
/// ******* RIGHT HIP ******** ///
{
localA.setIdentity(); localB.setIdentity();
localA.setOrigin(btVector3(btScalar(0.18*scale_ragdoll), btScalar(-0.10*scale_ragdoll), btScalar(0.)));
localB.setOrigin(btVector3(btScalar(0.), btScalar(0.225*scale_ragdoll), btScalar(0.)));
joint6DOF = new btGeneric6DofConstraint(*m_bodies[BODYPART_PELVIS], *m_bodies[BODYPART_RIGHT_UPPER_LEG], localA, localB,useLinearReferenceFrameA);
#ifdef RIGID
joint6DOF->setAngularLowerLimit(btVector3(-SIMD_EPSILON,-SIMD_EPSILON,-SIMD_EPSILON));
joint6DOF->setAngularUpperLimit(btVector3(SIMD_EPSILON,SIMD_EPSILON,SIMD_EPSILON));
#else
joint6DOF->setAngularLowerLimit(btVector3(-SIMD_HALF_PI*0.5,-SIMD_EPSILON,-SIMD_HALF_PI*0.6f));
joint6DOF->setAngularUpperLimit(btVector3(SIMD_HALF_PI*0.8,SIMD_EPSILON,SIMD_EPSILON));
#endif
m_joints[JOINT_RIGHT_HIP] = joint6DOF;
m_ownerWorld->addConstraint(m_joints[JOINT_RIGHT_HIP], true);
}
/// *************************** ///
/// ******* LEFT KNEE ******** ///
{
localA.setIdentity(); localB.setIdentity();
localA.setOrigin(btVector3(btScalar(0.), btScalar(-0.225*scale_ragdoll), btScalar(0.)));
localB.setOrigin(btVector3(btScalar(0.), btScalar(0.185*scale_ragdoll), btScalar(0.)));
joint6DOF = new btGeneric6DofConstraint (*m_bodies[BODYPART_LEFT_UPPER_LEG], *m_bodies[BODYPART_LEFT_LOWER_LEG], localA, localB,useLinearReferenceFrameA);
//
#ifdef RIGID
joint6DOF->setAngularLowerLimit(btVector3(-SIMD_EPSILON,-SIMD_EPSILON,-SIMD_EPSILON));
joint6DOF->setAngularUpperLimit(btVector3(SIMD_EPSILON,SIMD_EPSILON,SIMD_EPSILON));
#else
joint6DOF->setAngularLowerLimit(btVector3(-SIMD_EPSILON,-SIMD_EPSILON,-SIMD_EPSILON));
joint6DOF->setAngularUpperLimit(btVector3(SIMD_PI*0.7f,SIMD_EPSILON,SIMD_EPSILON));
#endif
m_joints[JOINT_LEFT_KNEE] = joint6DOF;
m_ownerWorld->addConstraint(m_joints[JOINT_LEFT_KNEE], true);
}
/// *************************** ///
/// ******* RIGHT KNEE ******** ///
{
localA.setIdentity(); localB.setIdentity();
localA.setOrigin(btVector3(btScalar(0.), btScalar(-0.225*scale_ragdoll), btScalar(0.)));
localB.setOrigin(btVector3(btScalar(0.), btScalar(0.185*scale_ragdoll), btScalar(0.)));
joint6DOF = new btGeneric6DofConstraint (*m_bodies[BODYPART_RIGHT_UPPER_LEG], *m_bodies[BODYPART_RIGHT_LOWER_LEG], localA, localB,useLinearReferenceFrameA);
#ifdef RIGID
joint6DOF->setAngularLowerLimit(btVector3(-SIMD_EPSILON,-SIMD_EPSILON,-SIMD_EPSILON));
joint6DOF->setAngularUpperLimit(btVector3(SIMD_EPSILON,SIMD_EPSILON,SIMD_EPSILON));
#else
joint6DOF->setAngularLowerLimit(btVector3(-SIMD_EPSILON,-SIMD_EPSILON,-SIMD_EPSILON));
joint6DOF->setAngularUpperLimit(btVector3(SIMD_PI*0.7f,SIMD_EPSILON,SIMD_EPSILON));
#endif
m_joints[JOINT_RIGHT_KNEE] = joint6DOF;
m_ownerWorld->addConstraint(m_joints[JOINT_RIGHT_KNEE], true);
}
/// *************************** ///
}
//--------------------------------------------------------------
ofPlanePrimitive::ofPlanePrimitive(float width, float height, int columns, int rows, ofPrimitiveMode mode) {
texCoords = ofVec4f(0,0,1,1);
set(width, height, columns, rows, mode);
}
void pspConcertRoom::createColorsVec(){
colorsVec.clear();
colorsVec.push_back(ofVec4f(220, 20, 60, 255));
colorsVec.push_back(ofVec4f(255, 110, 180, 255));
colorsVec.push_back(ofVec4f(72, 118, 255, 255));
colorsVec.push_back(ofVec4f(0, 238, 118, 255));
colorsVec.push_back(ofVec4f(255, 236, 139, 255));
colorsVec.push_back(ofVec4f(255, 153, 18, 255));
colorsVec.push_back(ofVec4f(50, 205, 50, 255));
colorsVec.push_back(ofVec4f(0, 255, 255, 255));
colorsVec.push_back(ofVec4f(99, 184, 255, 255));
colorsVec.push_back(ofVec4f(238, 0, 238, 255));
colorsVec.push_back(ofVec4f(255, 127, 80, 255));
colorsVec.push_back(ofVec4f(255, 0, 255, 255));
colorsVec.push_back(ofVec4f(255, 255, 255, 255));
colorsVec.push_back(ofVec4f(113, 198, 113, 255));
colorsVec.push_back(ofVec4f(198, 113, 113, 255));
colorsVec.push_back(ofVec4f(113, 113, 198, 255));
colorsVec.push_back(ofVec4f(152, 251, 152, 255));
colorsVec.push_back(ofVec4f(70, 130, 180, 255));
colorsVec.push_back(ofVec4f(192, 255, 62, 255));
colorsVec.push_back(ofVec4f(171, 130, 255, 255));
for(int i=0; i<colorsVec.size(); i++){
colorsVec[i] = colorsVec[i]/255.;
}
}
// BOX PRIMITIVE //
//--------------------------------------------------------------
ofBoxPrimitive::ofBoxPrimitive() {
texCoords = ofVec4f(0,0,1,1);
set(100, 100, 100, 2, 2, 2);
}
// AO: Providing a way to access using ofVec4f
// in case we still need support
ofVec4f ofxObject::getColorVec4f()
{
return ofVec4f(material->color.r, material->color.g, material->color.b, material->color.a); //v4.0
}
ofVec4f ofxPlane::getEqtnParams(){
return ofVec4f(normal.x,normal.y,normal.z,d);
}
// Returns ofColor as a ofVec4f
// Convenience method for ofxMessage calculations
ofVec4f ofxObjectMaterial::getColorVec4f(){
return ofVec4f(color.r, color.g, color.b, color.a);
}
//---------------------------------------------------------------
void testApp::update() {
ofBackground(100, 100, 100);
NUI_IMAGE_FRAME pImageFrame = {0};
NUI_LOCKED_RECT locked_rect = {0};
if(bKinectInitSucessful)
{
HRESULT hr=m_nui->NuiImageStreamGetNextFrame(depthStreamHandle,0,&pImageFrame);
if (SUCCEEDED(hr))
{
hr=pImageFrame.pFrameTexture->LockRect(0,&locked_rect,NULL,0);
if(locked_rect.Pitch!=0) {
// 将数据存入Mat中
cv::Mat depthimage(KINECT_HEIGHT,KINECT_WIDTH,CV_8UC1);
//cv::Mat depthimage2(KINECT_HEIGHT,KINECT_WIDTH,CV_8UC1);
if(test)
{
for (int i=0; i<depthimage.rows; i++)
{
uchar *ptr = depthimage.ptr<uchar>(i); //第i行的指针
//uchar *ptr2 = depthimage2.ptr<uchar>(i);
uchar *pBufferRun = (uchar*)(locked_rect.pBits) + i * locked_rect.Pitch;
USHORT *pBuffer = (USHORT*) pBufferRun;
for (int j=0; j<depthimage.cols; j++)
{
pBuffer[j]=pBuffer[j]>>3;
ptr[j]=(uchar)(256*pBuffer[j]/0x0fff); //直接将数据归一化处理
//ptr2[j]=(uchar)(256*depth_float[i*depthimage.cols+j]/0x0fff);
}
}
//test=false;
}
cv::imshow("orginal",depthimage);
//cv::imshow("sss",depthimage2);
cv::Mat depthimage_filter = depthimage.clone();
// 读取照片
#if 0
const int photonumMax = 1;
if(photonum < photonumMax)
{
char namebuffer[20];
sprintf(namebuffer,"%d.jpg",photonum);
string name = namebuffer;
depthimage_filter = cv::imread(name,CV_LOAD_IMAGE_GRAYSCALE);
++photonum;
}
#endif
// 滤波 双边滤波效果没出现（？）
//cv::medianBlur(depthimage,depthimage_filter,5);
//clock_t t1=clock();
cv::bilateralFilter(depthimage,depthimage_filter,11,20,20);
//clock_t t2=clock();
//double ti=(double)(t2-t1)/CLOCKS_PER_SEC;
//cout<<ti<<endl;
//cv::GaussianBlur(depthimage,depthimage_filter,cv::Size(5,5),0,0);
//cv::imshow("filter",depthimage_filter);
// 保存照片
#if 0
if(photonum < photonumMax)
{
char namebuffer[20];
sprintf(namebuffer,"%d.jpg",photonum);
string name = namebuffer;
cv::imwrite(name,depthimage);
++photonum;
}
#endif
// pyramid creat 高斯降采 论文中提到避免边界被平滑未处理（？）
//cv::Mat downmap1;
//cv::pyrDown(depthimage_filter,downmap1,cv::Size(depthimage_filter.cols/2,depthimage_filter.rows/2));
//cv::imshow("downonce",downmap1);
//cv::Mat downmap2;
//cv::pyrDown(downmap1,downmap2,cv::Size(downmap1.cols/2,downmap1.rows/2));
//cv::imshow("downtwice",downmap2);
// 滤波后深度数据提取与单位转换
//ofstream out;
//if(photonum < photonumMax&&test3)
//{
// char namebuffer[10];
// sprintf(namebuffer,"%d.txt",photonum);
// out.open(namebuffer,ios::trunc);
//}
for (int i=0; i<depthimage_filter.rows; i++)
{
uchar *ptr = depthimage_filter.ptr<uchar>(i); //第i行的指针
for (int j=0; j<depthimage_filter.cols; j++)
{
float s = (float)(ptr[j]*0x0fff)/256;
//if(s>2500&&s<2600)
//{
// cout<<s<<endl;
//}
depth_float[i*depthimage_filter.cols+j] = s/1000;//显示点云用，计算时要除1000，化为米单位
//if(test3)
// out<<s<<" ";
}
}
//out.close();
test3 = false;
// 试验区
ofMatrix4x4 tk=ofMatrix4x4(1,0,0,0,0,-1,0,0,0,0,1,0,0,0,0,1);
ofMatrix4x4 tk_next = tk;
ofVec4f camp = ofVec4f(0,0,0,1);
int size_g = TSDF_SIZE;
const int maxcountnum = 3;
if(countnum < maxcountnum)
{
//compute_tsdf(tk,depthimage,size_g,camp);
++ countnum;
if(countnum == maxcountnum)
test2 = true;
//}
//if(test2)
//{
//cv::imwrite("original.jpg",depthimage_filter);
//cout<<tk<<endl;
clock_t t1=clock();
compute_points(depthimage_filter,depthimage_filter.rows,depthimage_filter.cols,pointsmap_orignal);
compute_tsdf(tk_next,depthimage,size_g,camp);
compute_raycast(tk_next);
compute_normal(pointsmap_orignal,depthimage_filter.rows,depthimage_filter.cols,normalmap_orignal);
//compute_normal(pointsmap_final,depthimage_filter.rows,depthimage_filter.cols,normalmap_final);
for(int i = 0;i < 10; ++ i)
{
compute_pda(tk,i,tk_next);
}
camp = tk_next * camp;
tk = tk_next;
//changePosition(tk_next,pointsmap_orignal,normalmap_orignal);
clock_t t2=clock();
double ti=(double)(t2-t1)/CLOCKS_PER_SEC;
std::cout<<ti<<endl;
//test2=false;
}
// 转换坐标系为摄像机坐标系与法向
//changeType(depthimage_filter,depthimage_filter.rows,depthimage_filter.cols);//无用了
//compute_points(depthimage_filter,depthimage_filter.rows,depthimage_filter.cols,pointsmap_orignal);
//compute_points(downmap1,downmap1.rows,downmap1.cols,pointsmap_downonce);
//compute_points(downmap2,downmap2.rows,downmap2.cols,pointsmap_downtwice);
//
//compute_normal(pointsmap_orignal,depthimage_filter.rows,depthimage_filter.cols,normalmap_orignal);
//compute_normal(pointsmap_downonce,downmap1.rows,downmap1.cols,normalmap_downonce);
//compute_normal(pointsmap_downtwice,downmap2.rows,downmap2.cols,normalmap_downtwice);
if(bThreshWithOpenCV) {
} else {
}
}
m_nui->NuiImageStreamReleaseFrame(depthStreamHandle,&pImageFrame);
}
}
void testApp::compute_pda(const ofMatrix4x4 &preTmatrix,int timeZ,ofMatrix4x4 &newTmatrix)//,const ofVec3f* pointmapNow,const ofVec3f* pointmap_pre)
{
ofMatrix4x4 nowTmatrix;
if(timeZ==0)
nowTmatrix = preTmatrix;
else
{
// 迭代
nowTmatrix = newTmatrix;
//cout<<"z is big than 0ne"<<endl;
}
//ofMatrix4x4 invPreT = nowTmatrix.getInverse();
ofMatrix4x4 invPreT = preTmatrix.getInverse();
ofMatrix3x3 nowRmatrix = getRotationMatrix(nowTmatrix);
ofMatrix3x3 preRmatrix = getRotationMatrix(preTmatrix);
int i=0,j=0;
int dnum = 0;
double total_b = 0;
double total_bb = 0;
final_A = Eigen::MatrixXf::Zero(6,6);
final_B = Eigen::MatrixXf::Zero(6,1);
// 找到对应点对
for(j=0;j<KINECT_HEIGHT;j++)
for(i=0;i<KINECT_WIDTH;i++)
{
int num = j*KINECT_WIDTH+i;
if(pointsmap_orignal[num].z>0)//isvalid_orignal[num])
{
ofVec4f pointNow = ofVec4f(pointsmap_orignal[num].x,pointsmap_orignal[num].y,pointsmap_orignal[num].z,1);
ofVec4f spacePointNow = nowTmatrix*pointNow;
ofVec3f spacenormalNow = nmmul(nowRmatrix,normalmap_orignal[num]);
//cout<<i<<" "<<j<<endl;
ofVec4f pointPreC = invPreT*spacePointNow;
ofVec3f dpointPreC = ofVec3f(pointPreC.x,pointPreC.y,pointPreC.z);
ofVec3f pointPreZ = nmmul(the_K_cam,dpointPreC);
ofVec2f pointPreU = ofVec2f(pointPreZ.x/pointPreZ.z,pointPreZ.y/pointPreZ.z);
//cout<<endl;
int num_p = (int)(pointPreU.x+0.5)+(int)(pointPreU.y+0.5)*KINECT_WIDTH;
int kk = num;
if(pointsmap_final[num_p].z>0)
{
ofVec4f pointPre = ofVec4f(pointsmap_final[num_p].x,pointsmap_final[num_p].y,pointsmap_final[num_p].z,1);
ofVec4f spacePointPre = pointPre;
//ofVec4f spacePointPre = preTmatrix*pointPre;
ofVec3f spacenormalPre = normalmap_final[num_p];
//ofVec3f spacenormalPre = nmmul(preRmatrix,normalmap_final[num_p]);
float pointDistance = spacePointNow.distance(spacePointPre);
float pointAngle = spacenormalNow.angle(spacenormalPre);
if(pointDistance<=THRESHOLD_D&&pointAngle<=THRESHOLD_A)
{
ofVec3f dspacePointNow = ofVec3f(spacePointNow.x,spacePointNow.y,spacePointNow.z);
ofVec3f dspacePointPre = ofVec3f(spacePointPre.x,spacePointPre.y,spacePointPre.z);
ofVec3f minsPoint = (dspacePointPre - dspacePointNow);
float b = minsPoint.dot(spacenormalPre);
if(b > 0)
total_b += b;
else
total_bb += b;
compute_icp(b,dspacePointNow,spacenormalPre);
++dnum;
}
}
}
}
cout<<"dnum = "<<dnum<<" total_b = "<<total_b<<" total_bb = "<<total_bb<<endl;
//double deta = final_A.determinant();
//if(deta>0) // 条件待修改
Eigen::Matrix<float,6,1> result = final_A.llt().solve(final_B).cast<float>();
cout<<"result = "<<endl;
cout<<result<<endl;
float alpha = result(0);
float beta = result(1);
float gamma = result(2);
Eigen::Matrix3f rinc = (Eigen::Matrix3f)Eigen::AngleAxisf(gamma,Eigen::Vector3f::UnitZ())*Eigen::AngleAxisf(beta,Eigen::Vector3f::UnitY())*Eigen::AngleAxisf(alpha,Eigen::Vector3f::UnitX());
ofMatrix4x4 changeMatrix = ofMatrix4x4(rinc(0,0),rinc(0,1),rinc(0,2),result(3),
rinc(1,0),rinc(1,1),rinc(1,2),result(4),
rinc(2,0),rinc(2,1),rinc(2,2),result(5),
0,0,0,1);
cout<<"change = "<<endl;
cout<<changeMatrix<<endl;
newTmatrix = changeMatrix * newTmatrix;
cout<<"newTM = "<<endl;
cout<<newTmatrix<<endl;
}
// Cone Primitive //
//--------------------------------------------------------------
ofConePrimitive::ofConePrimitive() {
texCoords = ofVec4f(0,0,1,1);
set( 20, 70, 8, 3, 2 );
}
void ofApp::moveArm(){
// assign the target pose to the current robot pose
if(parameters.bCopy){
parameters.bCopy = false;
parameters.targetTCP.rotation = ofQuaternion(90, ofVec3f(0, 0, 1));
parameters.targetTCP.rotation*=ofQuaternion(90, ofVec3f(1, 0, 0));
// get the robot's position
parameters.targetTCP.position = parameters.actualTCP.position;
parameters.targetTCP.rotation*=parameters.actualTCP.rotation;
// update the gizmo controller
tcpNode.setPosition(parameters.targetTCP.position*1000);
tcpNode.setOrientation(parameters.targetTCP.rotation);
gizmo.setNode(tcpNode);
// update GUI params
parameters.targetTCPPosition = parameters.targetTCP.position;
parameters.targetTCPOrientation = ofVec4f(parameters.targetTCP.rotation.x(), parameters.targetTCP.rotation.y(), parameters.targetTCP.rotation.z(), parameters.targetTCP.rotation.w());
}
else if (!followRigidBody){
// update the tool tcp
tcpNode.setTransformMatrix(gizmo.getMatrix());
}
// update based on rigid body position and orientation
if (followRigidBody){
tcpNode.setPosition(currentRB.matrix.getTranslation().x*1000, currentRB.matrix.getTranslation().y*1000, currentRB.matrix.getTranslation().z*1000);
// rotate quaternion so that robot aligns to rb's z-axis instead of x-axis
ofQuaternion mocapOrient = currentRB.matrix.getRotate();
mocapOrient *= mocapOrient.conj();
mocapOrient.makeRotate(90, 0, 1, 0);
mocapOrient *= currentRB.matrix.getRotate();
tcpNode.setOrientation(mocapOrient);
// update gizmo
gizmo.setNode(tcpNode);
}
// follow a user-defined position and orientation
if(parameters.bFollow){
parameters.targetTCP.position.interpolate(tcpNode.getPosition()/1000.0, parameters.followLerp);
parameters.targetTCP.rotation = tcpNode.getOrientationQuat();
parameters.targetTCPOrientation = ofVec4f(parameters.targetTCP.rotation.x(), parameters.targetTCP.rotation.y(), parameters.targetTCP.rotation.z(), parameters.targetTCP.rotation.w());
}
if (followPath && !followRigidBody && !paths.pause){
paths.getNextPose();
ofMatrix4x4 m44 = recordedPath[paths.paths[0]->ptIndex].matrix;
tcpNode.setPosition(m44.getTranslation().x*1000, m44.getTranslation().y*1000, m44.getTranslation().z*1000);
// rotate quaternion so that robot aligns to rb's z-axis instead of x-axis
ofQuaternion m44Orient = m44.getRotate();
m44Orient *= m44Orient.conj();
m44Orient.makeRotate(90, 0, 1, 0);
m44Orient *= m44.getRotate();
tcpNode.setOrientation(m44Orient);
parameters.targetTCP.position.interpolate(tcpNode.getPosition()/1000.0, parameters.followLerp);
parameters.targetTCP.rotation = tcpNode.getOrientationQuat();
parameters.targetTCPOrientation = ofVec4f(parameters.targetTCP.rotation.x(), parameters.targetTCP.rotation.y(), parameters.targetTCP.rotation.z(), parameters.targetTCP.rotation.w());
}
}
//--------------------------------------------------------------
ofConePrimitive::ofConePrimitive( float radius, float height, int radiusSegments, int heightSegments, int capSegments, ofPrimitiveMode mode ) {
texCoords = ofVec4f(0,0,1,1);
set( radius, height, radiusSegments, heightSegments, capSegments, mode );
}
//--------------------------------------------------------------
void SnapshotRamses::setup(const std::string& folder, int frameIndex)
{
clear();
// Load the HDF5 data.
std::vector<float> posX;
std::vector<float> posY;
std::vector<float> posZ;
std::vector<float> cellSize;
std::vector<float> density;
load(folder + "x/seq_" + ofToString(frameIndex) + "_x.h5", posX);
load(folder + "y/seq_" + ofToString(frameIndex) + "_y.h5", posY);
load(folder + "z/seq_" + ofToString(frameIndex) + "_z.h5", posZ);
load(folder + "dx/seq_" + ofToString(frameIndex) + "_dx.h5", cellSize);
load(folder + "density/seq_" + ofToString(frameIndex) + "_density.h5", density);
m_numCells = posX.size();
// Set the ranges for all data.
for (int i = 0; i < posX.size(); ++i)
{
m_coordRange.add(glm::vec3(posX[i], posY[i], posZ[i]));
m_sizeRange.add(cellSize[i]);
m_densityRange.add(density[i]);
}
// Expand coord range taking cell size into account.
m_coordRange.add(m_coordRange.getMin() - m_sizeRange.getMax());
m_coordRange.add(m_coordRange.getMax() + m_sizeRange.getMax());
// Find the dimension with the max span, and set all spans to be the same (since we're rendering a cube).
glm::vec3 coordSpan = m_coordRange.getSpan();
float maxSpan = MAX(coordSpan.x, MAX(coordSpan.y, coordSpan.z));
glm::vec3 spanOffset(maxSpan * 0.5);
glm::vec3 coordMid = m_coordRange.getCenter();
m_coordRange.add(coordMid - spanOffset);
m_coordRange.add(coordMid + spanOffset);
// Set up the VBO.
m_vboMesh = ofMesh::box(1, 1, 1, 1, 1, 1);
m_vboMesh.setUsage(GL_STATIC_DRAW);
// Upload per-instance data to the VBO.
m_vboMesh.getVbo().setAttributeData(DENSITY_ATTRIBUTE, density.data(), 1, density.size(), GL_STATIC_DRAW, 0);
m_vboMesh.getVbo().setAttributeDivisor(DENSITY_ATTRIBUTE, 1);
// Upload per-instance transform data to the TBO.
std::vector<ofVec4f> transforms;
transforms.resize(posX.size());
for (size_t i = 0; i < transforms.size(); ++i)
{
transforms[i] = ofVec4f(posX[i], posY[i], posZ[i], cellSize[i]);
}
m_bufferObject.allocate();
m_bufferObject.bind(GL_TEXTURE_BUFFER);
m_bufferObject.setData(transforms, GL_STREAM_DRAW);
m_bufferTexture.allocateAsBufferTexture(m_bufferObject, GL_RGBA32F);
m_bLoaded = true;
}
//--------------------------------------------------------------
ofBoxPrimitive::ofBoxPrimitive( float width, float height, float depth, int resWidth, int resHeight, int resDepth ) {
texCoords = ofVec4f(0,0,1,1);
set(width, height, depth, resWidth, resHeight, resDepth );
}
ofVec4f ofxToVec4f(string str) {
vector <string> v = ofSplitString(str,",");
if (v.size()!=4) return ofVec4f(0,0,0,0);
else return ofVec4f(ofToFloat(v[0]),ofToFloat(v[1]),ofToFloat(v[2]),ofToFloat(v[3]));
}
// PLANE PRIMITIVE //
//--------------------------------------------------------------
ofPlanePrimitive::ofPlanePrimitive() {
texCoords = ofVec4f(0,0,1,1);
set( 200, 100, 6, 3);
}
//----------------------------------------------------------
ofSpherePrimitive::ofSpherePrimitive( float _radius, int res, ofPrimitiveMode mode ) {
radius = _radius;
texCoords = ofVec4f(0,0,1,1);
setResolution( res );
}
// SPHERE PRIMITIVE //
//----------------------------------------------------------
ofSpherePrimitive::ofSpherePrimitive() {
texCoords = ofVec4f(0,0,1,1);
radius = 20;
setResolution( 16 );
}
void TerrainFunctions::getObjectsForVertex( TerrainVertex *vertex, Chunk *chunk)
{
int r = rand();
float veg = (vegatationPerlin->Get(vertex->position.x/1500.1, vertex->position.z/1500.1)+1)*0.5;
float veg2 =( vegatationPerlin->Get(vertex->position.x/150.1, vertex->position.z/150.1)+1 )*0.2;
if(vertex->hil<0.8)
{
vertex->color.set(0.37f,0.27f,0.17f);
}else
{
//vertex->color.set(0.51f,0.66f,0.15f);
vertex->color.set(0.44f,0.78f,0.08f);
}
if (veg2<0.2 && vertex->hil>0.7)
{
float factor = (veg2*5) ;
vertex->color.x *= factor ;
vertex->color.y *=factor;
vertex->color.z *=factor ;
factor =1-factor;
vertex->color.x += factor*0.7f;
vertex->color.y +=factor *0.8f;
vertex->color.z +=factor*0 ;
}
float vegFactor =1- (veg-0.48f)*8.0f;
if ( vegFactor <0.5f) vegFactor =0.5f;
if ( vegFactor >0.8) vegFactor =0.8f;
vegFactor+=veg2 ;
vertex->color.x*=vegFactor ;
vertex->color.y *=vegFactor ; //*(1.0f-vegFactor );
vertex->color.z *=vegFactor ; //*(1.0f-vegFactor );
if(vertex->position.y<100 ){
if ( veg>0.5)
{
if (r%120==3){
ofMatrix4x4 objMatrix;
//objMatrix.makeLookAtViewMatrix(vertex->normal,ofVec3f(0,0,0), ofVec3f(0,0,1));
objMatrix.rotate(-90,1,0,0);
objMatrix.rotate((float)rand()/RAND_MAX*360,0,1,0);
float s = (float)rand()/RAND_MAX *0.4 +0.8;
objMatrix.postMultScale(ofVec3f(s,s,s));
objMatrix.postMultTranslate(vertex->position);
vertex->color.x =0;
vertex->color.y =0;
vertex->color.z =0.0;
chunk->detail1Objects[0].objectMatrices.push_back(objMatrix);
chunk->detail2Objects[4].objectMatrices.push_back(objMatrix);
chunk->detail3Objects[0].objectMatrices.push_back(objMatrix);
return;
}else if ((r%4==3 && veg2<0.1)||r%100==3 ){
ofMatrix4x4 objMatrix;
//objMatrix.makeLookAtViewMatrix(vertex->normal,ofVec3f(0,0,0), ofVec3f(0,0,1));
objMatrix.rotate(-90,1,0,0);
objMatrix.rotate((float)rand()/RAND_MAX*360,0,1,0);
float s = (float)rand()/RAND_MAX *0.4 +0.8;
objMatrix.postMultScale(ofVec3f(s,s,s));
objMatrix.postMultTranslate(vertex->position);
vertex->color.x *=0.8;
vertex->color.y *=0.8;
vertex->color.z *=0.8;
chunk->detail1Objects[3].objectMatrices.push_back(objMatrix);
chunk->detail2Objects[3].objectMatrices.push_back(objMatrix);
//chunk->detail3Objects[0].objectMatrices.push_back(objMatrix);
return;
}
}
else
{
if (r%2==1 ){
ofMatrix4x4 objMatrix;
objMatrix.makeLookAtViewMatrix(ofVec3f(0,0.0,0),vertex->normal, ofVec3f(0,0.0,-1));
objMatrix.rotate( 180.0f,1,0,0);
objMatrix.rotate( (float)rand()/RAND_MAX*360,0,1,0);
objMatrix.postMultTranslate(vertex->position +ofVec3f((float)rand()/RAND_MAX,0,(float)rand()/RAND_MAX)*2);
chunk->detail1Objects[1].objectMatrices.push_back(objMatrix);
//chunk->detail2Objects[1].objectMatrices.push_back(objMatrix);
return;
}
}
if (r%50==2 &&vertex->hil>0.8 ){
ofMatrix4x4 objMatrix;
objMatrix.makeLookAtViewMatrix(ofVec3f(0,0.0,0),vertex->normal, ofVec3f(0,0.0,-1));
objMatrix.rotate( 180.0f,1,0,0);
objMatrix.rotate( (float)rand()/RAND_MAX*360,0,1,0);
objMatrix.postMultTranslate(vertex->position);
chunk->detail1Objects[1].objectMatrices.push_back(objMatrix);
//chunk->detail2Objects[1].objectMatrices.push_back(objMatrix);
return;
}
}
if ( veg>0.5 && vertex->position.y<100 )
{
// r = rand();
if (r%150==4)
{
int pIndex = rand()%chunk->pLights.size();
ofMatrix4x4 objMatrix;
ofVec3f center =vertex->position +ofVec4f(0,2* (float)rand()/RAND_MAX+3,0);
objMatrix.makeTranslationMatrix( center);
chunk->pLights[pIndex ].objectMatrices.push_back(objMatrix);
chunk->pLights[pIndex ].objectCenters.push_back(center);
return;
}
}
if (r%700==2)
{
ofMatrix4x4 objMatrix;
//objMatrix.makeRotationMatrix(-90,ofVec3f(1,0,0) );
objMatrix.makeLookAtViewMatrix(ofVec3f(0,0,0),vertex->normal, ofVec3f(0,0,-1));
objMatrix.rotate( (float)rand()/RAND_MAX *360.0f,0,1,0);
float s = (float)rand()/RAND_MAX *0.5 +1.5;
objMatrix.postMultScale(ofVec3f(s,s,s));
objMatrix.postMultTranslate(vertex->position);
chunk->detail1Objects[2].objectMatrices.push_back(objMatrix);
chunk->detail2Objects[2].objectMatrices.push_back(objMatrix);
}
}
