TinyRendererVisualShapeConverterInternalData() :m_upAxis(2), m_swWidth(START_WIDTH), m_swHeight(START_HEIGHT), m_rgbColorBuffer(START_WIDTH,START_HEIGHT,TGAImage::RGB) { m_depthBuffer.resize(m_swWidth*m_swHeight); m_segmentationMaskBuffer.resize(m_swWidth*m_swHeight,-1); }
TinyRendererGUIHelper( int swWidth, int swHeight) : m_upAxis(1), m_swWidth(swWidth), m_swHeight(swHeight), m_rgbColorBuffer(swWidth,swHeight,TGAImage::RGB), m_colObjUniqueIndex(0) { m_depthBuffer.resize(swWidth*swHeight); }
ParticleInternalData() : m_clPositionBuffer(0), m_velocitiesGPU(0), m_simParamGPU(0), m_updatePositionsKernel(0), m_updatePositionsKernel2(0), m_updateAabbsKernel(0), m_collideParticlesKernel(0) { m_simParamCPU.resize(1); }
SW_And_OpenGLGuiHelper(CommonGraphicsApp* glApp, bool useOpenGL2, int swWidth, int swHeight, GLPrimitiveRenderer* primRenderer) : OpenGLGuiHelper(glApp, useOpenGL2), m_swWidth(swWidth), m_swHeight(swHeight), m_rgbColorBuffer(swWidth, swHeight, TGAImage::RGB), m_primRenderer(primRenderer) { m_depthBuffer.resize(swWidth * swHeight); CommonRenderInterface* render = getRenderInterface(); m_image = new unsigned char[m_swWidth * m_swHeight * 4]; m_textureHandle = render->registerTexture(m_image, m_swWidth, m_swHeight); }
TinyRendererSetupInternalData(int width, int height) :m_roll(0), m_pitch(0), m_yaw(0), m_width(width), m_height(height), m_rgbColorBuffer(width,height,TGAImage::RGB), m_textureHandle(0), m_animateRenderer(0) { m_depthBuffer.resize(m_width*m_height); }
inline int b3GpuPgsConstraintSolver::sortConstraintByBatch3(b3BatchConstraint* cs, int numConstraints, int simdWidth, int staticIdx, int numBodies) { //int sz = sizeof(b3BatchConstraint); B3_PROFILE("sortConstraintByBatch3"); static int maxSwaps = 0; int numSwaps = 0; curUsed.resize(2 * simdWidth); static int maxNumConstraints = 0; if (maxNumConstraints < numConstraints) { maxNumConstraints = numConstraints; //printf("maxNumConstraints = %d\n",maxNumConstraints ); } int numUsedArray = numBodies / 32 + 1; bodyUsed.resize(numUsedArray); for (int q = 0; q < numUsedArray; q++) bodyUsed[q] = 0; int curBodyUsed = 0; int numIter = 0; #if defined(_DEBUG) for (int i = 0; i < numConstraints; i++) cs[i].m_batchId = -1; #endif int numValidConstraints = 0; // int unprocessedConstraintIndex = 0; int batchIdx = 0; { B3_PROFILE("cpu batch innerloop"); while (numValidConstraints < numConstraints) { numIter++; int nCurrentBatch = 0; // clear flag for (int i = 0; i < curBodyUsed; i++) bodyUsed[curUsed[i] / 32] = 0; curBodyUsed = 0; for (int i = numValidConstraints; i < numConstraints; i++) { int idx = i; b3Assert(idx < numConstraints); // check if it can go int bodyAS = cs[idx].m_bodyAPtrAndSignBit; int bodyBS = cs[idx].m_bodyBPtrAndSignBit; int bodyA = abs(bodyAS); int bodyB = abs(bodyBS); bool aIsStatic = (bodyAS < 0) || bodyAS == staticIdx; bool bIsStatic = (bodyBS < 0) || bodyBS == staticIdx; int aUnavailable = 0; int bUnavailable = 0; if (!aIsStatic) { aUnavailable = bodyUsed[bodyA / 32] & (1 << (bodyA & 31)); } if (!aUnavailable) if (!bIsStatic) { bUnavailable = bodyUsed[bodyB / 32] & (1 << (bodyB & 31)); } if (aUnavailable == 0 && bUnavailable == 0) // ok { if (!aIsStatic) { bodyUsed[bodyA / 32] |= (1 << (bodyA & 31)); curUsed[curBodyUsed++] = bodyA; } if (!bIsStatic) { bodyUsed[bodyB / 32] |= (1 << (bodyB & 31)); curUsed[curBodyUsed++] = bodyB; } cs[idx].m_batchId = batchIdx; if (i != numValidConstraints) { b3Swap(cs[i], cs[numValidConstraints]); numSwaps++; } numValidConstraints++; { nCurrentBatch++; if (nCurrentBatch == simdWidth) { nCurrentBatch = 0; for (int i = 0; i < curBodyUsed; i++) bodyUsed[curUsed[i] / 32] = 0; curBodyUsed = 0; } } } } m_gpuData->m_batchSizes.push_back(nCurrentBatch); batchIdx++; } } #if defined(_DEBUG) // debugPrintf( "nBatches: %d\n", batchIdx ); for (int i = 0; i < numConstraints; i++) { b3Assert(cs[i].m_batchId != -1); } #endif if (maxSwaps < numSwaps) { maxSwaps = numSwaps; //printf("maxSwaps = %d\n", maxSwaps); } return batchIdx; }
b3Scalar b3GpuPgsConstraintSolver::solveGroupCacheFriendlySetup(b3OpenCLArray<b3RigidBodyData>* gpuBodies, b3OpenCLArray<b3InertiaData>* gpuInertias, int numBodies, b3OpenCLArray<b3GpuGenericConstraint>* gpuConstraints, int numConstraints, const b3ContactSolverInfo& infoGlobal) { B3_PROFILE("GPU solveGroupCacheFriendlySetup"); batchConstraints.resize(numConstraints); m_gpuData->m_gpuBatchConstraints->resize(numConstraints); m_staticIdx = -1; m_maxOverrideNumSolverIterations = 0; /* m_gpuData->m_gpuBodies->resize(numBodies); m_gpuData->m_gpuBodies->copyFromHostPointer(bodies,numBodies); b3OpenCLArray<b3InertiaData> gpuInertias(m_gpuData->m_context,m_gpuData->m_queue); gpuInertias.resize(numBodies); gpuInertias.copyFromHostPointer(inertias,numBodies); */ m_gpuData->m_gpuSolverBodies->resize(numBodies); m_tmpSolverBodyPool.resize(numBodies); { if (useGpuInitSolverBodies) { B3_PROFILE("m_initSolverBodiesKernel"); b3LauncherCL launcher(m_gpuData->m_queue, m_gpuData->m_initSolverBodiesKernel, "m_initSolverBodiesKernel"); launcher.setBuffer(m_gpuData->m_gpuSolverBodies->getBufferCL()); launcher.setBuffer(gpuBodies->getBufferCL()); launcher.setConst(numBodies); launcher.launch1D(numBodies); clFinish(m_gpuData->m_queue); // m_gpuData->m_gpuSolverBodies->copyToHost(m_tmpSolverBodyPool); } else { gpuBodies->copyToHost(m_gpuData->m_cpuBodies); for (int i = 0; i < numBodies; i++) { b3RigidBodyData& body = m_gpuData->m_cpuBodies[i]; b3GpuSolverBody& solverBody = m_tmpSolverBodyPool[i]; initSolverBody(i, &solverBody, &body); solverBody.m_originalBodyIndex = i; } m_gpuData->m_gpuSolverBodies->copyFromHost(m_tmpSolverBodyPool); } } // int totalBodies = 0; int totalNumRows = 0; //b3RigidBody* rb0=0,*rb1=0; //if (1) { { // int i; m_tmpConstraintSizesPool.resizeNoInitialize(numConstraints); // b3OpenCLArray<b3GpuGenericConstraint> gpuConstraints(m_gpuData->m_context,m_gpuData->m_queue); if (useGpuInfo1) { B3_PROFILE("info1 and init batchConstraint"); m_gpuData->m_gpuConstraintInfo1->resize(numConstraints); if (1) { B3_PROFILE("getInfo1Kernel"); b3LauncherCL launcher(m_gpuData->m_queue, m_gpuData->m_getInfo1Kernel, "m_getInfo1Kernel"); launcher.setBuffer(m_gpuData->m_gpuConstraintInfo1->getBufferCL()); launcher.setBuffer(gpuConstraints->getBufferCL()); launcher.setConst(numConstraints); launcher.launch1D(numConstraints); clFinish(m_gpuData->m_queue); } if (m_gpuData->m_batchSizes.size() == 0) { B3_PROFILE("initBatchConstraintsKernel"); m_gpuData->m_gpuConstraintRowOffsets->resize(numConstraints); unsigned int total = 0; m_gpuData->m_prefixScan->execute(*m_gpuData->m_gpuConstraintInfo1, *m_gpuData->m_gpuConstraintRowOffsets, numConstraints, &total); unsigned int lastElem = m_gpuData->m_gpuConstraintInfo1->at(numConstraints - 1); totalNumRows = total + lastElem; { B3_PROFILE("init batch constraints"); b3LauncherCL launcher(m_gpuData->m_queue, m_gpuData->m_initBatchConstraintsKernel, "m_initBatchConstraintsKernel"); launcher.setBuffer(m_gpuData->m_gpuConstraintInfo1->getBufferCL()); launcher.setBuffer(m_gpuData->m_gpuConstraintRowOffsets->getBufferCL()); launcher.setBuffer(m_gpuData->m_gpuBatchConstraints->getBufferCL()); launcher.setBuffer(gpuConstraints->getBufferCL()); launcher.setBuffer(gpuBodies->getBufferCL()); launcher.setConst(numConstraints); launcher.launch1D(numConstraints); clFinish(m_gpuData->m_queue); } //assume the batching happens on CPU, so copy the data m_gpuData->m_gpuBatchConstraints->copyToHost(batchConstraints); } } else { totalNumRows = 0; gpuConstraints->copyToHost(m_gpuData->m_cpuConstraints); //calculate the total number of contraint rows for (int i = 0; i < numConstraints; i++) { unsigned int& info1 = m_tmpConstraintSizesPool[i]; // unsigned int info1; if (m_gpuData->m_cpuConstraints[i].isEnabled()) { m_gpuData->m_cpuConstraints[i].getInfo1(&info1, &m_gpuData->m_cpuBodies[0]); } else { info1 = 0; } totalNumRows += info1; } m_gpuData->m_gpuBatchConstraints->copyFromHost(batchConstraints); m_gpuData->m_gpuConstraintInfo1->copyFromHost(m_tmpConstraintSizesPool); } m_tmpSolverNonContactConstraintPool.resizeNoInitialize(totalNumRows); m_gpuData->m_gpuConstraintRows->resize(totalNumRows); // b3GpuConstraintArray verify; if (useGpuInfo2) { { B3_PROFILE("getInfo2Kernel"); b3LauncherCL launcher(m_gpuData->m_queue, m_gpuData->m_getInfo2Kernel, "m_getInfo2Kernel"); launcher.setBuffer(m_gpuData->m_gpuConstraintRows->getBufferCL()); launcher.setBuffer(m_gpuData->m_gpuConstraintInfo1->getBufferCL()); launcher.setBuffer(m_gpuData->m_gpuConstraintRowOffsets->getBufferCL()); launcher.setBuffer(gpuConstraints->getBufferCL()); launcher.setBuffer(m_gpuData->m_gpuBatchConstraints->getBufferCL()); launcher.setBuffer(gpuBodies->getBufferCL()); launcher.setBuffer(gpuInertias->getBufferCL()); launcher.setBuffer(m_gpuData->m_gpuSolverBodies->getBufferCL()); launcher.setConst(infoGlobal.m_timeStep); launcher.setConst(infoGlobal.m_erp); launcher.setConst(infoGlobal.m_globalCfm); launcher.setConst(infoGlobal.m_damping); launcher.setConst(infoGlobal.m_numIterations); launcher.setConst(numConstraints); launcher.launch1D(numConstraints); clFinish(m_gpuData->m_queue); if (m_gpuData->m_batchSizes.size() == 0) m_gpuData->m_gpuBatchConstraints->copyToHost(batchConstraints); //m_gpuData->m_gpuConstraintRows->copyToHost(verify); //m_gpuData->m_gpuConstraintRows->copyToHost(m_tmpSolverNonContactConstraintPool); } } else { gpuInertias->copyToHost(m_gpuData->m_cpuInertias); ///setup the b3SolverConstraints for (int i = 0; i < numConstraints; i++) { const int& info1 = m_tmpConstraintSizesPool[i]; if (info1) { int constraintIndex = batchConstraints[i].m_originalConstraintIndex; int constraintRowOffset = m_gpuData->m_cpuConstraintRowOffsets[constraintIndex]; b3GpuSolverConstraint* currentConstraintRow = &m_tmpSolverNonContactConstraintPool[constraintRowOffset]; b3GpuGenericConstraint& constraint = m_gpuData->m_cpuConstraints[i]; b3RigidBodyData& rbA = m_gpuData->m_cpuBodies[constraint.getRigidBodyA()]; //b3RigidBody& rbA = constraint.getRigidBodyA(); // b3RigidBody& rbB = constraint.getRigidBodyB(); b3RigidBodyData& rbB = m_gpuData->m_cpuBodies[constraint.getRigidBodyB()]; int solverBodyIdA = constraint.getRigidBodyA(); //getOrInitSolverBody(constraint.getRigidBodyA(),bodies,inertias); int solverBodyIdB = constraint.getRigidBodyB(); //getOrInitSolverBody(constraint.getRigidBodyB(),bodies,inertias); b3GpuSolverBody* bodyAPtr = &m_tmpSolverBodyPool[solverBodyIdA]; b3GpuSolverBody* bodyBPtr = &m_tmpSolverBodyPool[solverBodyIdB]; if (rbA.m_invMass) { batchConstraints[i].m_bodyAPtrAndSignBit = solverBodyIdA; } else { if (!solverBodyIdA) m_staticIdx = 0; batchConstraints[i].m_bodyAPtrAndSignBit = -solverBodyIdA; } if (rbB.m_invMass) { batchConstraints[i].m_bodyBPtrAndSignBit = solverBodyIdB; } else { if (!solverBodyIdB) m_staticIdx = 0; batchConstraints[i].m_bodyBPtrAndSignBit = -solverBodyIdB; } int overrideNumSolverIterations = 0; //constraint->getOverrideNumSolverIterations() > 0 ? constraint->getOverrideNumSolverIterations() : infoGlobal.m_numIterations; if (overrideNumSolverIterations > m_maxOverrideNumSolverIterations) m_maxOverrideNumSolverIterations = overrideNumSolverIterations; int j; for (j = 0; j < info1; j++) { memset(¤tConstraintRow[j], 0, sizeof(b3GpuSolverConstraint)); currentConstraintRow[j].m_angularComponentA.setValue(0, 0, 0); currentConstraintRow[j].m_angularComponentB.setValue(0, 0, 0); currentConstraintRow[j].m_appliedImpulse = 0.f; currentConstraintRow[j].m_appliedPushImpulse = 0.f; currentConstraintRow[j].m_cfm = 0.f; currentConstraintRow[j].m_contactNormal.setValue(0, 0, 0); currentConstraintRow[j].m_friction = 0.f; currentConstraintRow[j].m_frictionIndex = 0; currentConstraintRow[j].m_jacDiagABInv = 0.f; currentConstraintRow[j].m_lowerLimit = 0.f; currentConstraintRow[j].m_upperLimit = 0.f; currentConstraintRow[j].m_originalContactPoint = 0; currentConstraintRow[j].m_overrideNumSolverIterations = 0; currentConstraintRow[j].m_relpos1CrossNormal.setValue(0, 0, 0); currentConstraintRow[j].m_relpos2CrossNormal.setValue(0, 0, 0); currentConstraintRow[j].m_rhs = 0.f; currentConstraintRow[j].m_rhsPenetration = 0.f; currentConstraintRow[j].m_solverBodyIdA = 0; currentConstraintRow[j].m_solverBodyIdB = 0; currentConstraintRow[j].m_lowerLimit = -B3_INFINITY; currentConstraintRow[j].m_upperLimit = B3_INFINITY; currentConstraintRow[j].m_appliedImpulse = 0.f; currentConstraintRow[j].m_appliedPushImpulse = 0.f; currentConstraintRow[j].m_solverBodyIdA = solverBodyIdA; currentConstraintRow[j].m_solverBodyIdB = solverBodyIdB; currentConstraintRow[j].m_overrideNumSolverIterations = overrideNumSolverIterations; } bodyAPtr->internalGetDeltaLinearVelocity().setValue(0.f, 0.f, 0.f); bodyAPtr->internalGetDeltaAngularVelocity().setValue(0.f, 0.f, 0.f); bodyAPtr->internalGetPushVelocity().setValue(0.f, 0.f, 0.f); bodyAPtr->internalGetTurnVelocity().setValue(0.f, 0.f, 0.f); bodyBPtr->internalGetDeltaLinearVelocity().setValue(0.f, 0.f, 0.f); bodyBPtr->internalGetDeltaAngularVelocity().setValue(0.f, 0.f, 0.f); bodyBPtr->internalGetPushVelocity().setValue(0.f, 0.f, 0.f); bodyBPtr->internalGetTurnVelocity().setValue(0.f, 0.f, 0.f); b3GpuConstraintInfo2 info2; info2.fps = 1.f / infoGlobal.m_timeStep; info2.erp = infoGlobal.m_erp; info2.m_J1linearAxis = currentConstraintRow->m_contactNormal; info2.m_J1angularAxis = currentConstraintRow->m_relpos1CrossNormal; info2.m_J2linearAxis = 0; info2.m_J2angularAxis = currentConstraintRow->m_relpos2CrossNormal; info2.rowskip = sizeof(b3GpuSolverConstraint) / sizeof(b3Scalar); //check this ///the size of b3GpuSolverConstraint needs be a multiple of b3Scalar b3Assert(info2.rowskip * sizeof(b3Scalar) == sizeof(b3GpuSolverConstraint)); info2.m_constraintError = ¤tConstraintRow->m_rhs; currentConstraintRow->m_cfm = infoGlobal.m_globalCfm; info2.m_damping = infoGlobal.m_damping; info2.cfm = ¤tConstraintRow->m_cfm; info2.m_lowerLimit = ¤tConstraintRow->m_lowerLimit; info2.m_upperLimit = ¤tConstraintRow->m_upperLimit; info2.m_numIterations = infoGlobal.m_numIterations; m_gpuData->m_cpuConstraints[i].getInfo2(&info2, &m_gpuData->m_cpuBodies[0]); ///finalize the constraint setup for (j = 0; j < info1; j++) { b3GpuSolverConstraint& solverConstraint = currentConstraintRow[j]; if (solverConstraint.m_upperLimit >= m_gpuData->m_cpuConstraints[i].getBreakingImpulseThreshold()) { solverConstraint.m_upperLimit = m_gpuData->m_cpuConstraints[i].getBreakingImpulseThreshold(); } if (solverConstraint.m_lowerLimit <= -m_gpuData->m_cpuConstraints[i].getBreakingImpulseThreshold()) { solverConstraint.m_lowerLimit = -m_gpuData->m_cpuConstraints[i].getBreakingImpulseThreshold(); } // solverConstraint.m_originalContactPoint = constraint; b3Matrix3x3& invInertiaWorldA = m_gpuData->m_cpuInertias[constraint.getRigidBodyA()].m_invInertiaWorld; { //b3Vector3 angularFactorA(1,1,1); const b3Vector3& ftorqueAxis1 = solverConstraint.m_relpos1CrossNormal; solverConstraint.m_angularComponentA = invInertiaWorldA * ftorqueAxis1; //*angularFactorA; } b3Matrix3x3& invInertiaWorldB = m_gpuData->m_cpuInertias[constraint.getRigidBodyB()].m_invInertiaWorld; { const b3Vector3& ftorqueAxis2 = solverConstraint.m_relpos2CrossNormal; solverConstraint.m_angularComponentB = invInertiaWorldB * ftorqueAxis2; //*constraint.getRigidBodyB().getAngularFactor(); } { //it is ok to use solverConstraint.m_contactNormal instead of -solverConstraint.m_contactNormal //because it gets multiplied iMJlB b3Vector3 iMJlA = solverConstraint.m_contactNormal * rbA.m_invMass; b3Vector3 iMJaA = invInertiaWorldA * solverConstraint.m_relpos1CrossNormal; b3Vector3 iMJlB = solverConstraint.m_contactNormal * rbB.m_invMass; //sign of normal? b3Vector3 iMJaB = invInertiaWorldB * solverConstraint.m_relpos2CrossNormal; b3Scalar sum = iMJlA.dot(solverConstraint.m_contactNormal); sum += iMJaA.dot(solverConstraint.m_relpos1CrossNormal); sum += iMJlB.dot(solverConstraint.m_contactNormal); sum += iMJaB.dot(solverConstraint.m_relpos2CrossNormal); b3Scalar fsum = b3Fabs(sum); b3Assert(fsum > B3_EPSILON); solverConstraint.m_jacDiagABInv = fsum > B3_EPSILON ? b3Scalar(1.) / sum : 0.f; } ///fix rhs ///todo: add force/torque accelerators { b3Scalar rel_vel; b3Scalar vel1Dotn = solverConstraint.m_contactNormal.dot(rbA.m_linVel) + solverConstraint.m_relpos1CrossNormal.dot(rbA.m_angVel); b3Scalar vel2Dotn = -solverConstraint.m_contactNormal.dot(rbB.m_linVel) + solverConstraint.m_relpos2CrossNormal.dot(rbB.m_angVel); rel_vel = vel1Dotn + vel2Dotn; b3Scalar restitution = 0.f; b3Scalar positionalError = solverConstraint.m_rhs; //already filled in by getConstraintInfo2 b3Scalar velocityError = restitution - rel_vel * info2.m_damping; b3Scalar penetrationImpulse = positionalError * solverConstraint.m_jacDiagABInv; b3Scalar velocityImpulse = velocityError * solverConstraint.m_jacDiagABInv; solverConstraint.m_rhs = penetrationImpulse + velocityImpulse; solverConstraint.m_appliedImpulse = 0.f; } } } } m_gpuData->m_gpuConstraintRows->copyFromHost(m_tmpSolverNonContactConstraintPool); m_gpuData->m_gpuConstraintInfo1->copyFromHost(m_tmpConstraintSizesPool); if (m_gpuData->m_batchSizes.size() == 0) m_gpuData->m_gpuBatchConstraints->copyFromHost(batchConstraints); else m_gpuData->m_gpuBatchConstraints->copyToHost(batchConstraints); m_gpuData->m_gpuSolverBodies->copyFromHost(m_tmpSolverBodyPool); } //end useGpuInfo2 } #ifdef B3_SUPPORT_CONTACT_CONSTRAINTS { int i; for (i = 0; i < numManifolds; i++) { b3Contact4& manifold = manifoldPtr[i]; convertContact(bodies, inertias, &manifold, infoGlobal); } } #endif //B3_SUPPORT_CONTACT_CONSTRAINTS } // b3ContactSolverInfo info = infoGlobal; // int numNonContactPool = m_tmpSolverNonContactConstraintPool.size(); // int numConstraintPool = m_tmpSolverContactConstraintPool.size(); // int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.size(); return 0.f; }
bool checkData() { bool hasStatus = false; int serviceResult = enet_host_service(m_client, &m_event, 0); if (serviceResult > 0) { switch (m_event.type) { case ENET_EVENT_TYPE_CONNECT: printf("A new client connected from %x:%u.\n", m_event.peer->address.host, m_event.peer->address.port); m_event.peer->data = (void*)"New User"; break; case ENET_EVENT_TYPE_RECEIVE: { if (gVerboseNetworkMessagesClient) { printf("A packet of length %lu containing '%s' was " "received from %s on channel %u.\n", m_event.packet->dataLength, (char*)m_event.packet->data, (char*)m_event.peer->data, m_event.channelID); } int packetSizeInBytes = b3DeserializeInt(m_event.packet->data); if (packetSizeInBytes == m_event.packet->dataLength) { SharedMemoryStatus* statPtr = (SharedMemoryStatus*)&m_event.packet->data[4]; if (statPtr->m_type == CMD_STEP_FORWARD_SIMULATION_COMPLETED) { SharedMemoryStatus dummy; dummy.m_type = CMD_STEP_FORWARD_SIMULATION_COMPLETED; m_lastStatus = dummy; m_stream.resize(0); } else { m_lastStatus = *statPtr; int streamOffsetInBytes = 4 + sizeof(SharedMemoryStatus); int numStreamBytes = packetSizeInBytes - streamOffsetInBytes; m_stream.resize(numStreamBytes); for (int i = 0; i < numStreamBytes; i++) { m_stream[i] = m_event.packet->data[i + streamOffsetInBytes]; } } } else { printf("unknown status message received\n"); } enet_packet_destroy(m_event.packet); hasStatus = true; break; } case ENET_EVENT_TYPE_DISCONNECT: { printf("%s disconnected.\n", (char*)m_event.peer->data); break; } default: { printf("unknown event type: %d.\n", m_event.type); } } } else if (serviceResult > 0) { puts("Error with servicing the client"); } return hasStatus; }
inline int b3GpuPgsContactSolver::sortConstraintByBatch2( b3Contact4* cs, int numConstraints, int simdWidth , int staticIdx, int numBodies) { B3_PROFILE("sortConstraintByBatch2"); bodyUsed2.resize(2*simdWidth); for (int q=0;q<2*simdWidth;q++) bodyUsed2[q]=0; int curBodyUsed = 0; int numIter = 0; m_data->m_sortData.resize(numConstraints); m_data->m_idxBuffer.resize(numConstraints); m_data->m_old.resize(numConstraints); unsigned int* idxSrc = &m_data->m_idxBuffer[0]; #if defined(_DEBUG) for(int i=0; i<numConstraints; i++) cs[i].getBatchIdx() = -1; #endif for(int i=0; i<numConstraints; i++) idxSrc[i] = i; int numValidConstraints = 0; int unprocessedConstraintIndex = 0; int batchIdx = 0; { B3_PROFILE("cpu batch innerloop"); while( numValidConstraints < numConstraints) { numIter++; int nCurrentBatch = 0; // clear flag for(int i=0; i<curBodyUsed; i++) bodyUsed2[i] = 0; curBodyUsed = 0; for(int i=numValidConstraints; i<numConstraints; i++) { int idx = idxSrc[i]; b3Assert( idx < numConstraints ); // check if it can go int bodyAS = cs[idx].m_bodyAPtrAndSignBit; int bodyBS = cs[idx].m_bodyBPtrAndSignBit; int bodyA = abs(bodyAS); int bodyB = abs(bodyBS); bool aIsStatic = (bodyAS<0) || bodyAS==staticIdx; bool bIsStatic = (bodyBS<0) || bodyBS==staticIdx; int aUnavailable = 0; int bUnavailable = 0; if (!aIsStatic) { for (int j=0;j<curBodyUsed;j++) { if (bodyA == bodyUsed2[j]) { aUnavailable=1; break; } } } if (!aUnavailable) if (!bIsStatic) { for (int j=0;j<curBodyUsed;j++) { if (bodyB == bodyUsed2[j]) { bUnavailable=1; break; } } } if( aUnavailable==0 && bUnavailable==0 ) // ok { if (!aIsStatic) { bodyUsed2[curBodyUsed++] = bodyA; } if (!bIsStatic) { bodyUsed2[curBodyUsed++] = bodyB; } cs[idx].getBatchIdx() = batchIdx; m_data->m_sortData[idx].m_key = batchIdx; m_data->m_sortData[idx].m_value = idx; if (i!=numValidConstraints) { b3Swap(idxSrc[i], idxSrc[numValidConstraints]); } numValidConstraints++; { nCurrentBatch++; if( nCurrentBatch == simdWidth ) { nCurrentBatch = 0; for(int i=0; i<curBodyUsed; i++) bodyUsed2[i] = 0; curBodyUsed = 0; } } } } batchIdx ++; } } { B3_PROFILE("quickSort"); //m_data->m_sortData.quickSort(sortfnc); } { B3_PROFILE("reorder"); // reorder memcpy( &m_data->m_old[0], cs, sizeof(b3Contact4)*numConstraints); for(int i=0; i<numConstraints; i++) { b3Assert(m_data->m_sortData[idxSrc[i]].m_value == idxSrc[i]); int idx = m_data->m_sortData[idxSrc[i]].m_value; cs[i] = m_data->m_old[idx]; } } #if defined(_DEBUG) // debugPrintf( "nBatches: %d\n", batchIdx ); for(int i=0; i<numConstraints; i++) { b3Assert( cs[i].getBatchIdx() != -1 ); } #endif return batchIdx; }
inline int b3GpuPgsContactSolver::sortConstraintByBatch( b3Contact4* cs, int n, int simdWidth , int staticIdx, int numBodies) { B3_PROFILE("sortConstraintByBatch"); int numIter = 0; sortData.resize(n); idxBuffer.resize(n); old.resize(n); unsigned int* idxSrc = &idxBuffer[0]; unsigned int* idxDst = &idxBuffer[0]; int nIdxSrc, nIdxDst; const int N_FLG = 256; const int FLG_MASK = N_FLG-1; unsigned int flg[N_FLG/32]; #if defined(_DEBUG) for(int i=0; i<n; i++) cs[i].getBatchIdx() = -1; #endif for(int i=0; i<n; i++) idxSrc[i] = i; nIdxSrc = n; int batchIdx = 0; { B3_PROFILE("cpu batch innerloop"); while( nIdxSrc ) { numIter++; nIdxDst = 0; int nCurrentBatch = 0; // clear flag for(int i=0; i<N_FLG/32; i++) flg[i] = 0; for(int i=0; i<nIdxSrc; i++) { int idx = idxSrc[i]; b3Assert( idx < n ); // check if it can go int bodyAS = cs[idx].m_bodyAPtrAndSignBit; int bodyBS = cs[idx].m_bodyBPtrAndSignBit; int bodyA = abs(bodyAS); int bodyB = abs(bodyBS); int aIdx = bodyA & FLG_MASK; int bIdx = bodyB & FLG_MASK; unsigned int aUnavailable = flg[ aIdx/32 ] & (1<<(aIdx&31)); unsigned int bUnavailable = flg[ bIdx/32 ] & (1<<(bIdx&31)); bool aIsStatic = (bodyAS<0) || bodyAS==staticIdx; bool bIsStatic = (bodyBS<0) || bodyBS==staticIdx; //use inv_mass! aUnavailable = !aIsStatic? aUnavailable:0;// bUnavailable = !bIsStatic? bUnavailable:0; if( aUnavailable==0 && bUnavailable==0 ) // ok { if (!aIsStatic) flg[ aIdx/32 ] |= (1<<(aIdx&31)); if (!bIsStatic) flg[ bIdx/32 ] |= (1<<(bIdx&31)); cs[idx].getBatchIdx() = batchIdx; sortData[idx].m_key = batchIdx; sortData[idx].m_value = idx; { nCurrentBatch++; if( nCurrentBatch == simdWidth ) { nCurrentBatch = 0; for(int i=0; i<N_FLG/32; i++) flg[i] = 0; } } } else { idxDst[nIdxDst++] = idx; } } b3Swap( idxSrc, idxDst ); b3Swap( nIdxSrc, nIdxDst ); batchIdx ++; } } { B3_PROFILE("quickSort"); sortData.quickSort(sortfnc); } { B3_PROFILE("reorder"); // reorder memcpy( &old[0], cs, sizeof(b3Contact4)*n); for(int i=0; i<n; i++) { int idx = sortData[i].m_value; cs[i] = old[idx]; } } #if defined(_DEBUG) // debugPrintf( "nBatches: %d\n", batchIdx ); for(int i=0; i<n; i++) { b3Assert( cs[i].getBatchIdx() != -1 ); } #endif return batchIdx; }
EGLRendererVisualShapeConverterInternalData() : m_upAxis(2), m_swWidth(START_WIDTH), m_swHeight(START_HEIGHT), m_rgbColorBuffer(START_WIDTH, START_HEIGHT, TGAImage::RGB), m_lightDirection(btVector3(-5, 200, -40)), m_hasLightDirection(false), m_lightColor(btVector3(1.0, 1.0, 1.0)), m_hasLightColor(false), m_lightDistance(2.0), m_hasLightDistance(false), m_lightAmbientCoeff(0.6), m_hasLightAmbientCoeff(false), m_lightDiffuseCoeff(0.35), m_hasLightDiffuseCoeff(false), m_lightSpecularCoeff(0.05), m_hasLightSpecularCoeff(false), m_hasShadow(false), m_flags(0) { m_depthBuffer.resize(m_swWidth * m_swHeight); m_shadowBuffer.resize(m_swWidth * m_swHeight); m_segmentationMaskBuffer.resize(m_swWidth * m_swHeight, -1); // OpenGL window bool allowRetina = true; m_window = new DefaultOpenGLWindow(); m_window->setAllowRetina(allowRetina); b3gWindowConstructionInfo ci; ci.m_title = "Title"; ci.m_width = m_swWidth; ci.m_height = m_swHeight; ci.m_renderDevice = 0; m_window->createWindow(ci); m_window->setWindowTitle(ci.m_title); b3Assert(glGetError() == GL_NO_ERROR); { printGLString("Version", GL_VERSION); printGLString("Vendor", GL_VENDOR); printGLString("Renderer", GL_RENDERER); } glClearColor(.7f, .7f, .8f, 1.f); m_window->startRendering(); b3Assert(glGetError() == GL_NO_ERROR); glGetError(); //don't remove this call, it is needed for Ubuntu b3Assert(glGetError() == GL_NO_ERROR); int maxNumObjectCapacity = 128 * 1024; int maxShapeCapacityInBytes = 128 * 1024 * 1024; m_instancingRenderer = new GLInstancingRenderer(maxNumObjectCapacity, maxShapeCapacityInBytes); b3Assert(glGetError() == GL_NO_ERROR); m_instancingRenderer->init(); b3Assert(glGetError() == GL_NO_ERROR); m_instancingRenderer->resize(m_swWidth, m_swHeight); m_instancingRenderer->InitShaders(); b3Assert(glGetError() == GL_NO_ERROR); m_instancingRenderer->setActiveCamera(&m_camera); b3Assert(glGetError() == GL_NO_ERROR); m_instancingRenderer->updateCamera(); b3Assert(glGetError() == GL_NO_ERROR); m_instancingRenderer->setLightPosition(m_lightDirection); m_window->endRendering(); }