diff --git a/src/dtkPhysMassSpring.cpp b/src/dtkPhysMassSpring.cpp index 487ecd9..1715492 100644 --- a/src/dtkPhysMassSpring.cpp +++ b/src/dtkPhysMassSpring.cpp @@ -32,883 +32,928 @@ using namespace std; namespace dtk { -dtkPhysMassSpring::dtkPhysMassSpring(double defaultMass, double defaultStiff, - double defaultDamp, - double defaultPointDamp, - double defaultPointResistence, - dtkDouble3 defaultGravityAccel) { - mDefaultMass = defaultMass; - mDefaultStiff = defaultStiff; - mDefaultDamp = defaultDamp; - mDefaultPointDamp = defaultPointDamp; - mDefaultPointResistence = defaultPointResistence; - mDefaultGravityAccel = defaultGravityAccel; + dtkPhysMassSpring::dtkPhysMassSpring(double defaultMass, double defaultStiff, + double defaultDamp, + double defaultPointDamp, + double defaultPointResistence, + dtkDouble3 defaultGravityAccel) { + mDefaultMass = defaultMass; + mDefaultStiff = defaultStiff; + mDefaultDamp = defaultDamp; + mDefaultPointDamp = defaultPointDamp; + mDefaultPointResistence = defaultPointResistence; + mDefaultGravityAccel = defaultGravityAccel; #ifdef DTK_CL - mUseMultiThread = false; - source_ = ""; - mMPPosData = NULL; - mMPVelData = NULL; - mMPAccelData = NULL; - mMPForceAccumData = NULL; - mMPGravityData = NULL; - mMPMassData = NULL; - mMPPosBufferData = NULL; - mMPVelBufferData = NULL; - mMPAccelBufferData = NULL; - mMPForceDecoratorData = NULL; - mMPActiveData = NULL; - - mSpringVertexIDData = NULL; - mSpringOriLengthData = NULL; - mSpringStiffData = NULL; - mSpringDampData = NULL; + mUseMultiThread = false; + source_ = ""; + mMPPosData = NULL; + mMPVelData = NULL; + mMPAccelData = NULL; + mMPForceAccumData = NULL; + mMPGravityData = NULL; + mMPMassData = NULL; + mMPPosBufferData = NULL; + mMPVelBufferData = NULL; + mMPAccelBufferData = NULL; + mMPForceDecoratorData = NULL; + mMPActiveData = NULL; + + mSpringVertexIDData = NULL; + mSpringOriLengthData = NULL; + mSpringStiffData = NULL; + mSpringDampData = NULL; #endif - mUnderControl = false; -} + mUnderControl = false; + } -dtkPhysMassSpring::~dtkPhysMassSpring() { - for (dtkID i = 0; i < mMassPoints.size(); i++) { - delete mMassPoints[i]; + dtkPhysMassSpring::dtkPhysMassSpring(double defaultMass, double defaultStiff, + double defaultDamp, + double defaultPointDamp, + double defaultPointResistence, + double defaultTimeStep, + dtkDouble3 defaultGravityAccel) { + mDefaultMass = defaultMass; + mDefaultStiff = defaultStiff; + mDefaultDamp = defaultDamp; + mDefaultPointDamp = defaultPointDamp; + mDefaultPointResistence = defaultPointResistence; + mDefaultTimeStep = defaultTimeStep; + mDefaultGravityAccel = defaultGravityAccel; +#ifdef DTK_CL + mUseMultiThread = false; + source_ = ""; + mMPPosData = NULL; + mMPVelData = NULL; + mMPAccelData = NULL; + mMPForceAccumData = NULL; + mMPGravityData = NULL; + mMPMassData = NULL; + mMPPosBufferData = NULL; + mMPVelBufferData = NULL; + mMPAccelBufferData = NULL; + mMPForceDecoratorData = NULL; + mMPActiveData = NULL; + + mSpringVertexIDData = NULL; + mSpringOriLengthData = NULL; + mSpringStiffData = NULL; + mSpringDampData = NULL; +#endif + mUnderControl = false; } - for (dtkID i = 0; i < mSprings.size(); i++) { - delete mSprings[i]; + dtkPhysMassSpring::~dtkPhysMassSpring() { + for (dtkID i = 0; i < mMassPoints.size(); i++) { + delete mMassPoints[i]; + } + + for (dtkID i = 0; i < mSprings.size(); i++) { + delete mSprings[i]; + } } -} -void dtkPhysMassSpring::SetPoints(dtkPoints::Ptr points) { - dtkAssert(points.get() != NULL, NULL_POINTER); - mPts = points; -} + void dtkPhysMassSpring::SetPoints(dtkPoints::Ptr points) { + dtkAssert(points.get() != NULL, NULL_POINTER); + mPts = points; + } -dtkPoints::Ptr dtkPhysMassSpring::GetPoints() { return mPts; } + dtkPoints::Ptr dtkPhysMassSpring::GetPoints() { return mPts; } -const GK::Point3 &dtkPhysMassSpring::GetPoint(dtkID id) const { - dtkAssert(mPts.get() != NULL, NULL_POINTER); - return mPts->GetPoint(id); -} + const GK::Point3& dtkPhysMassSpring::GetPoint(dtkID id) const { + dtkAssert(mPts.get() != NULL, NULL_POINTER); + return mPts->GetPoint(id); + } -void dtkPhysMassSpring::SetPoint(dtkID id, const GK::Point3 &coord) { - dtkAssert(mPts.get() != NULL, ILLEGAL_STATE); - dtkAssert(mPts->SetPoint(id, coord), OUT_OF_RANGE); -} + void dtkPhysMassSpring::SetPoint(dtkID id, const GK::Point3& coord) { + dtkAssert(mPts.get() != NULL, ILLEGAL_STATE); + dtkAssert(mPts->SetPoint(id, coord), OUT_OF_RANGE); + } -dtkID dtkPhysMassSpring::AddMassPoint(dtkID id, const double &mass, - const dtkT3 &vel, - double pointDamp, double pointResistence, - dtkDouble3 defaultGravityAccel) { - mMassPoints.push_back(new dtkPhysMassPoint( + dtkID dtkPhysMassSpring::AddMassPoint(dtkID id, const double& mass, + const dtkT3& vel, + double pointDamp, double pointResistence, + dtkDouble3 defaultGravityAccel) { + mMassPoints.push_back(new dtkPhysMassPoint( id, mPts, mass, vel, pointDamp, pointResistence, defaultGravityAccel)); #ifdef DTK_PHYSMASSSPRINGIMPL_DEBUG - mAltitudeSpringForces.push_back(dtkT3(0, 0, 0)); + mAltitudeSpringForces.push_back(dtkT3(0, 0, 0)); #endif - return mMassPoints.size() - 1; -} + return mMassPoints.size() - 1; + } -dtkID dtkPhysMassSpring::AddSpring(dtkID p1, dtkID p2, const double &stiff, - const double &damp) { - if (p1 > p2) - swap(p1, p2); - if (mEdgeMap.find(dtkID2(p1, p2)) == mEdgeMap.end()) { - dtkPhysSpring *newSpring = + dtkID dtkPhysMassSpring::AddSpring(dtkID p1, dtkID p2, const double& stiff, + const double& damp, const double& rest_length_factor) { + if (p1 > p2) + swap(p1, p2); + if (mEdgeMap.find(dtkID2(p1, p2)) == mEdgeMap.end()) { + dtkPhysSpring* newSpring = new dtkPhysSpring(mMassPoints[p1], mMassPoints[p2], stiff, damp); - mSprings.push_back(newSpring); - mEdgeMap[dtkID2(p1, p2)] = newSpring; + newSpring->SetRestLength(newSpring->GetOriLength() * rest_length_factor); + mSprings.push_back(newSpring); + mEdgeMap[dtkID2(p1, p2)] = newSpring; + } + return mSprings.size() - 1; } - return mSprings.size() - 1; -} -bool dtkPhysMassSpring::Update_s(double timeslice, ItrMethod method, - bool limitDeformation) { - if (timeslice == 0) - return true; - switch (method) { - case Euler: - PreUpdate(timeslice, method); - UpdateStrings(timeslice, method, 0, limitDeformation); - UpdateMassPoints(timeslice, method, 0); - PostUpdate(method); - break; - case Mid: - for (dtkID i = 0; i < 2; i++) { - PreUpdate(timeslice, method, i); - UpdateStrings(timeslice, method, i, limitDeformation); - UpdateMassPoints(timeslice, method, i); - PostUpdate(method, i); - } - break; - case RK4: - for (dtkID i = 0; i < 4; i++) { - PreUpdate(timeslice, method, i); - UpdateStrings(timeslice, method, i, limitDeformation); - UpdateMassPoints(timeslice, method, i); - PostUpdate(method, i); - } - break; - case Heun: - for (dtkID i = 0; i < 2; i++) { - PreUpdate(timeslice, method, i); + bool dtkPhysMassSpring::Update_s(double timeslice, ItrMethod method, + bool limitDeformation) { + if (timeslice == 0) + return true; + switch (method) { + case Euler: + PreUpdate(timeslice, method); + UpdateStrings(timeslice, method, 0, limitDeformation); + UpdateMassPoints(timeslice, method, 0); + PostUpdate(method); + break; + case Mid: + for (dtkID i = 0; i < 2; i++) { + PreUpdate(timeslice, method, i); + UpdateStrings(timeslice, method, i, limitDeformation); + UpdateMassPoints(timeslice, method, i); + PostUpdate(method, i); + } + break; + case RK4: + for (dtkID i = 0; i < 4; i++) { + PreUpdate(timeslice, method, i); + UpdateStrings(timeslice, method, i, limitDeformation); + UpdateMassPoints(timeslice, method, i); + PostUpdate(method, i); + } + break; + case Heun: + for (dtkID i = 0; i < 2; i++) { + PreUpdate(timeslice, method, i); #ifdef DTK_PHYSMASSSPRINGIMPL_DEBUG - if (i == 0) { - mAltitudeSpringForces.clear(); - for (dtkID j = 0; j < mMassPoints.size(); j++) - mAltitudeSpringForces.push_back(mMassPoints[j]->GetForceAccum()); + if (i == 0) { + mAltitudeSpringForces.clear(); + for (dtkID j = 0; j < mMassPoints.size(); j++) + mAltitudeSpringForces.push_back(mMassPoints[j]->GetForceAccum()); + } +#endif + UpdateStrings(timeslice, method, i, limitDeformation); + UpdateMassPoints(timeslice, method, i); + PostUpdate(method, i); } + case Collision: + for (dtkID i = 0; i < 2; i++) { + PreUpdate(timeslice, method, i); +#ifdef DTK_PHYSMASSSPRINGIMPL_DEBUG + if (i == 0) { + mAltitudeSpringForces.clear(); + for (dtkID j = 0; j < mMassPoints.size(); j++) + mAltitudeSpringForces.push_back(mMassPoints[j]->GetForceAccum()); + } #endif - UpdateStrings(timeslice, method, i, limitDeformation); - UpdateMassPoints(timeslice, method, i); - PostUpdate(method, i); + UpdateStrings(timeslice, method, i, limitDeformation); + UpdateMassPoints(timeslice, method, i); + PostUpdate(method, i); + } + default: { + // not handle } - case Collision: - for (dtkID i = 0; i < 2; i++) { - PreUpdate(timeslice, method, i); + } + + return true; + } + + bool dtkPhysMassSpring::Update_iteration(double timeslice, ItrMethod method, + dtkID iteration, + bool limitDeformation) { + if (timeslice == 0) + return true; + PreUpdate(timeslice, method, iteration); #ifdef DTK_PHYSMASSSPRINGIMPL_DEBUG - if (i == 0) { - mAltitudeSpringForces.clear(); - for (dtkID j = 0; j < mMassPoints.size(); j++) - mAltitudeSpringForces.push_back(mMassPoints[j]->GetForceAccum()); - } -#endif - UpdateStrings(timeslice, method, i, limitDeformation); - UpdateMassPoints(timeslice, method, i); - PostUpdate(method, i); + if (iteration == 0) { + mAltitudeSpringForces.clear(); + for (dtkID j = 0; j < mMassPoints.size(); j++) + mAltitudeSpringForces.push_back(mMassPoints[j]->GetForceAccum()); } - default: { - // not handle +#endif + UpdateStrings(timeslice, method, iteration, limitDeformation); + UpdateMassPoints(timeslice, method, iteration); + PostUpdate(method, iteration); + return true; } + + bool dtkPhysMassSpring::Update(double timeslice, ItrMethod method, + bool limitDeformation) { +#ifdef DTK_CL + if (mUseMultiThread) + return Update_mt(timeslice, method, limitDeformation); + else +#endif + return Update_s(timeslice, method, limitDeformation); } - return true; -} + bool dtkPhysMassSpring::UpdateStrings(double timeslice, ItrMethod method, + dtkID iteration, bool limitDeformation) { + for (dtkID i = 0; i < mSprings.size(); i++) + mSprings[i]->Update(timeslice, method, iteration, limitDeformation); -bool dtkPhysMassSpring::Update_iteration(double timeslice, ItrMethod method, - dtkID iteration, - bool limitDeformation) { - if (timeslice == 0) return true; - PreUpdate(timeslice, method, iteration); -#ifdef DTK_PHYSMASSSPRINGIMPL_DEBUG - if (iteration == 0) { - mAltitudeSpringForces.clear(); - for (dtkID j = 0; j < mMassPoints.size(); j++) - mAltitudeSpringForces.push_back(mMassPoints[j]->GetForceAccum()); } -#endif - UpdateStrings(timeslice, method, iteration, limitDeformation); - UpdateMassPoints(timeslice, method, iteration); - PostUpdate(method, iteration); - return true; -} - -bool dtkPhysMassSpring::Update(double timeslice, ItrMethod method, - bool limitDeformation) { -#ifdef DTK_CL - if (mUseMultiThread) - return Update_mt(timeslice, method, limitDeformation); - else -#endif - return Update_s(timeslice, method, limitDeformation); -} -bool dtkPhysMassSpring::UpdateStrings(double timeslice, ItrMethod method, - dtkID iteration, bool limitDeformation) { - for (dtkID i = 0; i < mSprings.size(); i++) - mSprings[i]->Update(timeslice, method, iteration, limitDeformation); + bool dtkPhysMassSpring::UpdateMassPoints(double timeslice, ItrMethod method, + dtkID iteration) { + for (dtkID i = 0; i < mMassPoints.size(); i++) + mMassPoints[i]->Update(timeslice, method, iteration); - return true; -} + return true; + } -bool dtkPhysMassSpring::UpdateMassPoints(double timeslice, ItrMethod method, - dtkID iteration) { - for (dtkID i = 0; i < mMassPoints.size(); i++) - mMassPoints[i]->Update(timeslice, method, iteration); + bool dtkPhysMassSpring::ApplyImpulse(double timeslice) { + for (dtkID i = 0; i < mMassPoints.size(); i++) + mMassPoints[i]->ApplyImpulse(); - return true; -} + return true; + } -bool dtkPhysMassSpring::ApplyImpulse(double timeslice) { - for (dtkID i = 0; i < mMassPoints.size(); i++) - mMassPoints[i]->ApplyImpulse(); + bool dtkPhysMassSpring::PreUpdate(double timeslice, ItrMethod method, + dtkID iteration) { + return false; + } - return true; -} + bool dtkPhysMassSpring::PostUpdate(ItrMethod method, dtkID iteration) { + return false; + } -bool dtkPhysMassSpring::PreUpdate(double timeslice, ItrMethod method, - dtkID iteration) { - return false; -} + void dtkPhysMassSpring::SetSpringStiffness(int id, double newStiff) { + if (id >= ((int)mSprings.size()) || id < 0) + return; + mSprings[id]->SetStiffness(newStiff); + } -bool dtkPhysMassSpring::PostUpdate(ItrMethod method, dtkID iteration) { - return false; -} + void dtkPhysMassSpring::SetSpringDamp(int id, double newDamp) { + if (id >= ((int)mSprings.size()) || id < 0) + return; + mSprings[id]->SetDamp(newDamp); + } -void dtkPhysMassSpring::SetSpringStiffness(int id, double newStiff) { - if (id >= ((int)mSprings.size()) || id < 0) - return; - mSprings[id]->SetStiffness(newStiff); -} + void dtkPhysMassSpring::SetPointMass(int id, double newMass) { + if (id >= mMassPoints.size() || id < 0) + return; + mMassPoints[id]->SetMass(newMass); + } -void dtkPhysMassSpring::SetSpringDamp(int id, double newDamp) { - if (id >= ((int)mSprings.size()) || id < 0) - return; - mSprings[id]->SetDamp(newDamp); -} + void dtkPhysMassSpring::SetTimeStep(double timeStep) { + mDefaultTimeStep = timeStep; + } -void dtkPhysMassSpring::SetPointMass(int id, double newMass) { - if (id >= mMassPoints.size() || id < 0) - return; - mMassPoints[id]->SetMass(newMass); -} + double dtkPhysMassSpring::GetTimeStep() { + return mDefaultTimeStep; + } #ifdef DTK_CL -bool dtkPhysMassSpring::Update_mt(double timeslice, ItrMethod method, - bool limitDeformation) { - if (timeslice == 0) - return true; - for (dtkID i = 0; i < 2; i++) { - PreUpdate(timeslice, method, i); + bool dtkPhysMassSpring::Update_mt(double timeslice, ItrMethod method, + bool limitDeformation) { + if (timeslice == 0) + return true; + for (dtkID i = 0; i < 2; i++) { + PreUpdate(timeslice, method, i); #ifdef DTK_PHYSMASSSPRINGIMPL_DEBUG - if (i == 0) { - mAltitudeSpringForces.clear(); - for (dtkID j = 0; j < mMassPoints.size(); j++) - mAltitudeSpringForces.push_back(mMassPoints[j]->GetForceAccum()); - } + if (i == 0) { + mAltitudeSpringForces.clear(); + for (dtkID j = 0; j < mMassPoints.size(); j++) + mAltitudeSpringForces.push_back(mMassPoints[j]->GetForceAccum()); + } #endif - UpdateCopyToCLArray(); - RunCLKernels(timeslice, i); - UpdateCopyBack(); + UpdateCopyToCLArray(); + RunCLKernels(timeslice, i); + UpdateCopyBack(); - PostUpdate(method, i); + PostUpdate(method, i); + } + return true; } - return true; -} -template int dtkPhysMassSpring::SetupCL() { - cl_int status; - clGetPlatformIDs(1, &platform, NULL); - clGetDeviceIDs(platform, CL_DEVICE_TYPE_GPU, 1, &device, NULL); - context = clCreateContext(NULL, 1, &device, NULL, NULL, NULL); - queue = clCreateCommandQueue(context, device, 0, &status); - if (status != CL_SUCCESS) { - cout << "create command error!" << endl; - } - const char *source = source_.c_str(); - size_t sourceSize[] = {strlen(source_.c_str())}; - program = clCreateProgramWithSource(context, 1, &source, sourceSize, &status); - if (status != CL_SUCCESS) { - cout << "create program with source error!" << endl; - } - const char *option = "-g -O0"; - status = clBuildProgram(program, 1, &device, option, NULL, NULL); - if (status != CL_SUCCESS) { - cout << "build program error!" << endl; - if (status == CL_INVALID_PROGRAM) - cout << "invalid program" << endl; - if (status == CL_INVALID_VALUE) - cout << "invalid value" << endl; - if (status == CL_INVALID_DEVICE) - cout << "invalid device" << endl; - if (status == CL_INVALID_BUILD_OPTIONS) - cout << "invalid build options" << endl; - if (status == CL_INVALID_OPERATION) - cout << "invalid operation" << endl; - if (status == CL_COMPILER_NOT_AVAILABLE) - cout << "compiler not available" << endl; - if (status == CL_BUILD_PROGRAM_FAILURE) - cout << "build program failure" << endl; - } - kernelSpring = clCreateKernel(program, "dtkPhysSpringUpdate", NULL); - kernelMP = clCreateKernel(program, "dtkPhysPointUpdate", NULL); - - mSpringOriLengthBuffer = clCreateBuffer( + template int dtkPhysMassSpring::SetupCL() { + cl_int status; + clGetPlatformIDs(1, &platform, NULL); + clGetDeviceIDs(platform, CL_DEVICE_TYPE_GPU, 1, &device, NULL); + context = clCreateContext(NULL, 1, &device, NULL, NULL, NULL); + queue = clCreateCommandQueue(context, device, 0, &status); + if (status != CL_SUCCESS) { + cout << "create command error!" << endl; + } + const char* source = source_.c_str(); + size_t sourceSize[] = { strlen(source_.c_str()) }; + program = clCreateProgramWithSource(context, 1, &source, sourceSize, &status); + if (status != CL_SUCCESS) { + cout << "create program with source error!" << endl; + } + const char* option = "-g -O0"; + status = clBuildProgram(program, 1, &device, option, NULL, NULL); + if (status != CL_SUCCESS) { + cout << "build program error!" << endl; + if (status == CL_INVALID_PROGRAM) + cout << "invalid program" << endl; + if (status == CL_INVALID_VALUE) + cout << "invalid value" << endl; + if (status == CL_INVALID_DEVICE) + cout << "invalid device" << endl; + if (status == CL_INVALID_BUILD_OPTIONS) + cout << "invalid build options" << endl; + if (status == CL_INVALID_OPERATION) + cout << "invalid operation" << endl; + if (status == CL_COMPILER_NOT_AVAILABLE) + cout << "compiler not available" << endl; + if (status == CL_BUILD_PROGRAM_FAILURE) + cout << "build program failure" << endl; + } + kernelSpring = clCreateKernel(program, "dtkPhysSpringUpdate", NULL); + kernelMP = clCreateKernel(program, "dtkPhysPointUpdate", NULL); + + mSpringOriLengthBuffer = clCreateBuffer( context, CL_MEM_READ_WRITE | CL_MEM_COPY_HOST_PTR, this->GetNumberOfSprings() * sizeof(cl_float), mSpringOriLengthData, 0); - mSpringStiffBuffer = clCreateBuffer( + mSpringStiffBuffer = clCreateBuffer( context, CL_MEM_READ_WRITE | CL_MEM_COPY_HOST_PTR, this->GetNumberOfSprings() * sizeof(cl_float), mSpringStiffData, 0); - mSpringDampBuffer = clCreateBuffer( + mSpringDampBuffer = clCreateBuffer( context, CL_MEM_READ_WRITE | CL_MEM_COPY_HOST_PTR, this->GetNumberOfSprings() * sizeof(cl_float), mSpringDampData, 0); - mSpringVertexIDBuffer = + mSpringVertexIDBuffer = clCreateBuffer(context, CL_MEM_READ_WRITE | CL_MEM_COPY_HOST_PTR, this->GetNumberOfSprings() * 2.0 * sizeof(cl_int), mSpringVertexIDData, 0); - mMPPosBuffer = clCreateBuffer( + mMPPosBuffer = clCreateBuffer( context, CL_MEM_READ_WRITE | CL_MEM_COPY_HOST_PTR, this->GetNumberOfMassPoints() * 3.0 * sizeof(cl_float), mMPPosData, 0); - mMPVelBuffer = clCreateBuffer( + mMPVelBuffer = clCreateBuffer( context, CL_MEM_READ_WRITE | CL_MEM_COPY_HOST_PTR, this->GetNumberOfMassPoints() * 3 * sizeof(cl_float), mMPVelData, 0); - mMPAccelBuffer = clCreateBuffer( + mMPAccelBuffer = clCreateBuffer( context, CL_MEM_READ_WRITE | CL_MEM_COPY_HOST_PTR, this->GetNumberOfMassPoints() * 3 * sizeof(cl_float), mMPAccelData, 0); - mMPForceAccumBuffer = + mMPForceAccumBuffer = clCreateBuffer(context, CL_MEM_READ_WRITE | CL_MEM_COPY_HOST_PTR, this->GetNumberOfMassPoints() * 3 * sizeof(cl_float), mMPForceAccumData, 0); - mMPGravityBuffer = clCreateBuffer( + mMPGravityBuffer = clCreateBuffer( context, CL_MEM_READ_WRITE | CL_MEM_COPY_HOST_PTR, this->GetNumberOfMassPoints() * 3 * sizeof(cl_float), mMPGravityData, 0); - mMPMassBuffer = clCreateBuffer( + mMPMassBuffer = clCreateBuffer( context, CL_MEM_READ_WRITE | CL_MEM_COPY_HOST_PTR, this->GetNumberOfMassPoints() * sizeof(cl_float), mMPMassData, &status); - this->CheckCLStatus(status, "massbuffer create buffer error!"); - mMPPosBufferBuffer = + this->CheckCLStatus(status, "massbuffer create buffer error!"); + mMPPosBufferBuffer = clCreateBuffer(context, CL_MEM_READ_WRITE | CL_MEM_COPY_HOST_PTR, this->GetNumberOfMassPoints() * 9 * sizeof(cl_float), mMPPosBufferData, 0); - mMPVelBufferBuffer = + mMPVelBufferBuffer = clCreateBuffer(context, CL_MEM_READ_WRITE | CL_MEM_COPY_HOST_PTR, this->GetNumberOfMassPoints() * 9 * sizeof(cl_float), mMPVelBufferData, 0); - mMPAccelBufferBuffer = + mMPAccelBufferBuffer = clCreateBuffer(context, CL_MEM_READ_WRITE | CL_MEM_COPY_HOST_PTR, this->GetNumberOfMassPoints() * 9 * sizeof(cl_float), mMPAccelBufferData, 0); - mMPForceDecoratorBuffer = + mMPForceDecoratorBuffer = clCreateBuffer(context, CL_MEM_READ_WRITE | CL_MEM_COPY_HOST_PTR, this->GetNumberOfMassPoints() * 3 * sizeof(cl_float), mMPForceDecoratorData, 0); - mMPActiveBuffer = clCreateBuffer( + mMPActiveBuffer = clCreateBuffer( context, CL_MEM_READ_WRITE | CL_MEM_COPY_HOST_PTR, this->GetNumberOfMassPoints() * sizeof(cl_bool), mMPActiveData, &status); - mMPAdjacentVertexIDsBuffer = + mMPAdjacentVertexIDsBuffer = clCreateBuffer(context, CL_MEM_READ_WRITE | CL_MEM_COPY_HOST_PTR, this->GetNumberOfSprings() * sizeof(cl_int) * 2, mMPAdjacentVertexIDsData, &status); - mMPNumberOfAdjacentVertexBuffer = + mMPNumberOfAdjacentVertexBuffer = clCreateBuffer(context, CL_MEM_READ_WRITE | CL_MEM_COPY_HOST_PTR, this->GetNumberOfMassPoints() * sizeof(cl_int), mMPNumberOfAdjacentVertexData, &status); - this->CheckCLStatus(status, "numofadjvertexbuffer create buffer error!"); - mMPStartAdjacentVertexIDBuffer = + this->CheckCLStatus(status, "numofadjvertexbuffer create buffer error!"); + mMPStartAdjacentVertexIDBuffer = clCreateBuffer(context, CL_MEM_READ_WRITE | CL_MEM_COPY_HOST_PTR, this->GetNumberOfSprings() * sizeof(cl_int), mMPStartAdjacentVertexIDData, &status); - mMPAdjacentSpringIDsBuffer = + mMPAdjacentSpringIDsBuffer = clCreateBuffer(context, CL_MEM_READ_WRITE | CL_MEM_COPY_HOST_PTR, this->GetNumberOfSprings() * sizeof(cl_int) * 2, mMPAdjacentSpringIDsData, &status); - global_work_size = NWITEMS; - mUseMultiThread = true; -} - -template -int dtkPhysMassSpring::RunCLKernels(double timeslice, dtkID iteration) { - mTimesliceData = static_cast(timeslice); - mIterationData = iteration; - cl_int status; + global_work_size = NWITEMS; + mUseMultiThread = true; + } - status = - clSetKernelArg(kernelMP, 0, sizeof(cl_int), (void *)&mNumberOfMassPoints); - // this->CheckCLStatus(status, "set kernel arg error! MP 0"); - status = clSetKernelArg(kernelMP, 1, sizeof(cl_mem), (void *)&mMPPosBuffer); - // this->CheckCLStatus(status, "set kernel arg error! MP 1"); - status = clSetKernelArg(kernelMP, 2, sizeof(cl_mem), (void *)&mMPVelBuffer); - // this->CheckCLStatus(status, "set kernel arg error! MP 2"); - status = clSetKernelArg(kernelMP, 3, sizeof(cl_mem), (void *)&mMPAccelBuffer); - // this->CheckCLStatus(status, "set kernel arg error! MP 3"); - status = - clSetKernelArg(kernelMP, 4, sizeof(cl_mem), (void *)&mMPForceAccumBuffer); - // this->CheckCLStatus(status, "set kernel arg error! MP 4"); - status = - clSetKernelArg(kernelMP, 5, sizeof(cl_mem), (void *)&mMPGravityBuffer); - // this->CheckCLStatus(status, "set kernel arg error! MP 5"); - status = clSetKernelArg(kernelMP, 6, sizeof(cl_mem), (void *)&mMPMassBuffer); - // this->CheckCLStatus(status, "set kernel arg error! MP 6"); - status = - clSetKernelArg(kernelMP, 7, sizeof(cl_mem), (void *)&mMPPosBufferBuffer); - // this->CheckCLStatus(status, "set kernel arg error! MP 7"); - status = - clSetKernelArg(kernelMP, 8, sizeof(cl_mem), (void *)&mMPVelBufferBuffer); - // this->CheckCLStatus(status, "set kernel arg error! MP 8"); - status = clSetKernelArg(kernelMP, 9, sizeof(cl_mem), - (void *)&mMPAccelBufferBuffer); - // this->CheckCLStatus(status, "set kernel arg error! MP 9"); - status = clSetKernelArg(kernelMP, 10, sizeof(cl_mem), - (void *)&mMPForceDecoratorBuffer); - // this->CheckCLStatus(status, "set kernel arg error! MP 10"); - status = - clSetKernelArg(kernelMP, 11, sizeof(cl_mem), (void *)&mMPActiveBuffer); - // this->CheckCLStatus(status, "set kernel arg error! MP 11"); - status = clSetKernelArg(kernelMP, 12, sizeof(cl_int), &mIterationData); - // this->CheckCLStatus(status, "set kernel arg error! MP 12"); - status = clSetKernelArg(kernelMP, 13, sizeof(cl_float), &mTimesliceData); - // this->CheckCLStatus(status, "set kernel arg error! MP 13"); - status = clSetKernelArg(kernelMP, 14, sizeof(cl_mem), - (void *)&mMPAdjacentVertexIDsBuffer); - status = clSetKernelArg(kernelMP, 15, sizeof(cl_mem), - (void *)&mMPNumberOfAdjacentVertexBuffer); - status = clSetKernelArg(kernelMP, 16, sizeof(cl_mem), - (void *)&mMPStartAdjacentVertexIDBuffer); - status = clSetKernelArg(kernelMP, 17, sizeof(cl_mem), - (void *)&mMPAdjacentSpringIDsBuffer); - status = clSetKernelArg(kernelMP, 18, sizeof(cl_mem), - (void *)&mSpringOriLengthBuffer); - status = - clSetKernelArg(kernelMP, 19, sizeof(cl_mem), (void *)&mSpringStiffBuffer); - status = - clSetKernelArg(kernelMP, 20, sizeof(cl_mem), (void *)&mSpringDampBuffer); - - work_size = this->GetNumberOfMassPoints(); - size_t local_work_size = 256; - work_size = (work_size / local_work_size + 1) * local_work_size; - clEnqueueNDRangeKernel(queue, kernelMP, 1, NULL, &work_size, &local_work_size, - 0, NULL, NULL); - // clFinish( queue ); - - clEnqueueReadBuffer(queue, mMPPosBuffer, CL_TRUE, 0, - this->GetNumberOfMassPoints() * 3 * sizeof(cl_float), - mMPPosData, 0, NULL, NULL); // - clEnqueueReadBuffer(queue, mMPVelBuffer, CL_TRUE, 0, - this->GetNumberOfMassPoints() * 3 * sizeof(cl_float), - mMPVelData, 0, NULL, NULL); // - clEnqueueReadBuffer(queue, mMPPosBufferBuffer, CL_TRUE, 0, - this->GetNumberOfMassPoints() * 9 * sizeof(cl_float), - mMPPosBufferData, 0, NULL, NULL); // - clEnqueueReadBuffer(queue, mMPVelBufferBuffer, CL_TRUE, 0, - this->GetNumberOfMassPoints() * 9 * sizeof(cl_float), - mMPVelBufferData, 0, NULL, NULL); // - clFinish(queue); - - /* - clEnqueueReadBuffer(queue, mMPAccelBuffer, CL_TRUE, 0, - this->GetNumberOfMassPoints() * 3.0 * sizeof(cl_float), mMPAccelData, - 0, NULL, NULL ); clEnqueueReadBuffer(queue, mMPForceAccumBuffer, CL_TRUE, 0, - this->GetNumberOfMassPoints() * 3.0 * sizeof(cl_float), - mMPForceAccumData, 0, NULL, NULL ); // clEnqueueReadBuffer(queue, - mMPGravityBuffer, CL_TRUE, 0, this->GetNumberOfMassPoints() * 3.0 * - sizeof(cl_float), mMPGravityData, 0, NULL, NULL ); // - clEnqueueReadBuffer(queue, mMPMassBuffer, CL_TRUE, 0, - this->GetNumberOfMassPoints() * sizeof(cl_float), mMPMassData, 0, - NULL, NULL ); // clEnqueueReadBuffer(queue, mMPAccelBufferBuffer, CL_TRUE, 0, - this->GetNumberOfMassPoints() * 9.0 * sizeof(cl_float), - mMPAccelBufferData, 0, NULL, NULL ); // clEnqueueReadBuffer(queue, - mMPForceDecoratorBuffer, CL_TRUE, 0, this->GetNumberOfMassPoints() * 3.0 * - sizeof(cl_float), mMPForceDecoratorData, 0, NULL, NULL ); // - clEnqueueReadBuffer(queue, mMPActiveBuffer, CL_TRUE, 0, - this->GetNumberOfMassPoints() * sizeof(cl_bool), mMPActiveData, 0, - NULL, NULL ); // clEnqueueReadBuffer(queue, mSpringVertexIDBuffer, CL_TRUE, 0, - this->GetNumberOfSprings() * 2.0 * sizeof(cl_int), - mSpringVertexIDData, 0, NULL, NULL ); // clEnqueueReadBuffer(queue, - mSpringOriLengthBuffer, CL_TRUE, 0, this->GetNumberOfSprings() * - sizeof(cl_float), mSpringOriLengthData, 0, NULL, NULL ); // - clEnqueueReadBuffer(queue, mSpringStiffBuffer, CL_TRUE, 0, - this->GetNumberOfSprings() * sizeof(cl_float), mSpringStiffData, 0, - NULL, NULL ); // clEnqueueReadBuffer(queue, mSpringDampBuffer, CL_TRUE, 0, - this->GetNumberOfSprings() * sizeof(cl_float), mSpringDampData, 0, - NULL, NULL ); // - */ - /* - cout << "RunKernel Ouptput:" << endl; - cout << mSpringOriLengthData[0] << endl; - cout << mSpringStiffData[0] << endl; - cout << mSpringDampData[0] << endl; - cout << mSpringVertexIDData[0] << endl; - cout << mMPPosData[0] << endl; - cout << mMPVelData[0] << endl; - cout << mMPForceAccumData[0] << endl; - cout << mMPGravityData[0] << endl; - cout << mMPMassData[0] << endl; - cout << mMPPosBufferData[0] << endl; - cout << mMPVelBufferData[0] << endl; - cout << mMPAccelBufferData[0] << endl; - cout << mMPForceDecoratorData[0] << endl; - cout << mMPActiveData[0] << endl; - cout << mMPAccelData[0] << endl; - cout << "/RunKernel Ouptput" << endl; - */ + template + int dtkPhysMassSpring::RunCLKernels(double timeslice, dtkID iteration) { + mTimesliceData = static_cast(timeslice); + mIterationData = iteration; + cl_int status; + + status = + clSetKernelArg(kernelMP, 0, sizeof(cl_int), (void*)&mNumberOfMassPoints); + // this->CheckCLStatus(status, "set kernel arg error! MP 0"); + status = clSetKernelArg(kernelMP, 1, sizeof(cl_mem), (void*)&mMPPosBuffer); + // this->CheckCLStatus(status, "set kernel arg error! MP 1"); + status = clSetKernelArg(kernelMP, 2, sizeof(cl_mem), (void*)&mMPVelBuffer); + // this->CheckCLStatus(status, "set kernel arg error! MP 2"); + status = clSetKernelArg(kernelMP, 3, sizeof(cl_mem), (void*)&mMPAccelBuffer); + // this->CheckCLStatus(status, "set kernel arg error! MP 3"); + status = + clSetKernelArg(kernelMP, 4, sizeof(cl_mem), (void*)&mMPForceAccumBuffer); + // this->CheckCLStatus(status, "set kernel arg error! MP 4"); + status = + clSetKernelArg(kernelMP, 5, sizeof(cl_mem), (void*)&mMPGravityBuffer); + // this->CheckCLStatus(status, "set kernel arg error! MP 5"); + status = clSetKernelArg(kernelMP, 6, sizeof(cl_mem), (void*)&mMPMassBuffer); + // this->CheckCLStatus(status, "set kernel arg error! MP 6"); + status = + clSetKernelArg(kernelMP, 7, sizeof(cl_mem), (void*)&mMPPosBufferBuffer); + // this->CheckCLStatus(status, "set kernel arg error! MP 7"); + status = + clSetKernelArg(kernelMP, 8, sizeof(cl_mem), (void*)&mMPVelBufferBuffer); + // this->CheckCLStatus(status, "set kernel arg error! MP 8"); + status = clSetKernelArg(kernelMP, 9, sizeof(cl_mem), + (void*)&mMPAccelBufferBuffer); + // this->CheckCLStatus(status, "set kernel arg error! MP 9"); + status = clSetKernelArg(kernelMP, 10, sizeof(cl_mem), + (void*)&mMPForceDecoratorBuffer); + // this->CheckCLStatus(status, "set kernel arg error! MP 10"); + status = + clSetKernelArg(kernelMP, 11, sizeof(cl_mem), (void*)&mMPActiveBuffer); + // this->CheckCLStatus(status, "set kernel arg error! MP 11"); + status = clSetKernelArg(kernelMP, 12, sizeof(cl_int), &mIterationData); + // this->CheckCLStatus(status, "set kernel arg error! MP 12"); + status = clSetKernelArg(kernelMP, 13, sizeof(cl_float), &mTimesliceData); + // this->CheckCLStatus(status, "set kernel arg error! MP 13"); + status = clSetKernelArg(kernelMP, 14, sizeof(cl_mem), + (void*)&mMPAdjacentVertexIDsBuffer); + status = clSetKernelArg(kernelMP, 15, sizeof(cl_mem), + (void*)&mMPNumberOfAdjacentVertexBuffer); + status = clSetKernelArg(kernelMP, 16, sizeof(cl_mem), + (void*)&mMPStartAdjacentVertexIDBuffer); + status = clSetKernelArg(kernelMP, 17, sizeof(cl_mem), + (void*)&mMPAdjacentSpringIDsBuffer); + status = clSetKernelArg(kernelMP, 18, sizeof(cl_mem), + (void*)&mSpringOriLengthBuffer); + status = + clSetKernelArg(kernelMP, 19, sizeof(cl_mem), (void*)&mSpringStiffBuffer); + status = + clSetKernelArg(kernelMP, 20, sizeof(cl_mem), (void*)&mSpringDampBuffer); + + work_size = this->GetNumberOfMassPoints(); + size_t local_work_size = 256; + work_size = (work_size / local_work_size + 1) * local_work_size; + clEnqueueNDRangeKernel(queue, kernelMP, 1, NULL, &work_size, &local_work_size, + 0, NULL, NULL); + // clFinish( queue ); + + clEnqueueReadBuffer(queue, mMPPosBuffer, CL_TRUE, 0, + this->GetNumberOfMassPoints() * 3 * sizeof(cl_float), + mMPPosData, 0, NULL, NULL); // + clEnqueueReadBuffer(queue, mMPVelBuffer, CL_TRUE, 0, + this->GetNumberOfMassPoints() * 3 * sizeof(cl_float), + mMPVelData, 0, NULL, NULL); // + clEnqueueReadBuffer(queue, mMPPosBufferBuffer, CL_TRUE, 0, + this->GetNumberOfMassPoints() * 9 * sizeof(cl_float), + mMPPosBufferData, 0, NULL, NULL); // + clEnqueueReadBuffer(queue, mMPVelBufferBuffer, CL_TRUE, 0, + this->GetNumberOfMassPoints() * 9 * sizeof(cl_float), + mMPVelBufferData, 0, NULL, NULL); // + clFinish(queue); + + /* + clEnqueueReadBuffer(queue, mMPAccelBuffer, CL_TRUE, 0, + this->GetNumberOfMassPoints() * 3.0 * sizeof(cl_float), mMPAccelData, + 0, NULL, NULL ); clEnqueueReadBuffer(queue, mMPForceAccumBuffer, CL_TRUE, 0, + this->GetNumberOfMassPoints() * 3.0 * sizeof(cl_float), + mMPForceAccumData, 0, NULL, NULL ); // clEnqueueReadBuffer(queue, + mMPGravityBuffer, CL_TRUE, 0, this->GetNumberOfMassPoints() * 3.0 * + sizeof(cl_float), mMPGravityData, 0, NULL, NULL ); // + clEnqueueReadBuffer(queue, mMPMassBuffer, CL_TRUE, 0, + this->GetNumberOfMassPoints() * sizeof(cl_float), mMPMassData, 0, + NULL, NULL ); // clEnqueueReadBuffer(queue, mMPAccelBufferBuffer, CL_TRUE, 0, + this->GetNumberOfMassPoints() * 9.0 * sizeof(cl_float), + mMPAccelBufferData, 0, NULL, NULL ); // clEnqueueReadBuffer(queue, + mMPForceDecoratorBuffer, CL_TRUE, 0, this->GetNumberOfMassPoints() * 3.0 * + sizeof(cl_float), mMPForceDecoratorData, 0, NULL, NULL ); // + clEnqueueReadBuffer(queue, mMPActiveBuffer, CL_TRUE, 0, + this->GetNumberOfMassPoints() * sizeof(cl_bool), mMPActiveData, 0, + NULL, NULL ); // clEnqueueReadBuffer(queue, mSpringVertexIDBuffer, CL_TRUE, 0, + this->GetNumberOfSprings() * 2.0 * sizeof(cl_int), + mSpringVertexIDData, 0, NULL, NULL ); // clEnqueueReadBuffer(queue, + mSpringOriLengthBuffer, CL_TRUE, 0, this->GetNumberOfSprings() * + sizeof(cl_float), mSpringOriLengthData, 0, NULL, NULL ); // + clEnqueueReadBuffer(queue, mSpringStiffBuffer, CL_TRUE, 0, + this->GetNumberOfSprings() * sizeof(cl_float), mSpringStiffData, 0, + NULL, NULL ); // clEnqueueReadBuffer(queue, mSpringDampBuffer, CL_TRUE, 0, + this->GetNumberOfSprings() * sizeof(cl_float), mSpringDampData, 0, + NULL, NULL ); // + */ + /* + cout << "RunKernel Ouptput:" << endl; + cout << mSpringOriLengthData[0] << endl; + cout << mSpringStiffData[0] << endl; + cout << mSpringDampData[0] << endl; + cout << mSpringVertexIDData[0] << endl; + cout << mMPPosData[0] << endl; + cout << mMPVelData[0] << endl; + cout << mMPForceAccumData[0] << endl; + cout << mMPGravityData[0] << endl; + cout << mMPMassData[0] << endl; + cout << mMPPosBufferData[0] << endl; + cout << mMPVelBufferData[0] << endl; + cout << mMPAccelBufferData[0] << endl; + cout << mMPForceDecoratorData[0] << endl; + cout << mMPActiveData[0] << endl; + cout << mMPAccelData[0] << endl; + cout << "/RunKernel Ouptput" << endl; + */ - return 0; -} + return 0; + } -bool dtkPhysMassSpring::Open(const char *fileName) { - size_t size; - char *str; + bool dtkPhysMassSpring::Open(const char* fileName) { + size_t size; + char* str; - fstream f(fileName); //, (fstream::in | fstream::binary)); - if (!f) - return false; - else // if(f.is_open()) - { - size_t sizeFile; - f.seekg(0, fstream::end); - size = sizeFile = (size_t)f.tellg(); - f.seekg(0, fstream::beg); - - str = new char[size + 1]; - if (!str) { - f.close(); + fstream f(fileName); //, (fstream::in | fstream::binary)); + if (!f) return false; - } + else // if(f.is_open()) + { + size_t sizeFile; + f.seekg(0, fstream::end); + size = sizeFile = (size_t)f.tellg(); + f.seekg(0, fstream::beg); + + str = new char[size + 1]; + if (!str) { + f.close(); + return false; + } - f.read(str, sizeFile); - f.close(); - str[size] = '\0'; + f.read(str, sizeFile); + f.close(); + str[size] = '\0'; - source_ = str; - delete[] str; - return true; + source_ = str; + delete[] str; + return true; + } } -} - -void dtkPhysMassSpring::InitialCLArray() { - size_t sizeInBytes = this->GetNumberOfMassPoints() * sizeof(cl_float); - mMPPosData = (cl_float *)malloc(sizeInBytes * 3.0); - mMPVelData = (cl_float *)malloc(sizeInBytes * 3.0); - mMPAccelData = (cl_float *)malloc(sizeInBytes * 3.0); - mMPForceAccumData = (cl_float *)malloc(sizeInBytes * 3.0); - mMPForceDecoratorData = (cl_float *)malloc(sizeInBytes * 3.0); - mMPGravityData = (cl_float *)malloc(sizeInBytes * 3.0); - mMPMassData = (cl_float *)malloc(sizeInBytes); - mMPPosBufferData = (cl_float *)malloc(sizeInBytes * 9.0); - mMPVelBufferData = (cl_float *)malloc(sizeInBytes * 9.0); - mMPAccelBufferData = (cl_float *)malloc(sizeInBytes * 9.0); - - sizeInBytes = this->GetNumberOfMassPoints() * sizeof(cl_bool); - mMPActiveData = (cl_bool *)malloc(sizeInBytes); - - sizeInBytes = this->GetNumberOfSprings() * sizeof(cl_int); - mSpringVertexIDData = (cl_int *)malloc(sizeInBytes * 2.0); - - sizeInBytes = this->GetNumberOfSprings() * sizeof(cl_float); - mSpringOriLengthData = (cl_float *)malloc(sizeInBytes); - mSpringStiffData = (cl_float *)malloc(sizeInBytes); - mSpringDampData = (cl_float *)malloc(sizeInBytes); - - sizeInBytes = this->GetNumberOfSprings() * 2.0 * sizeof(cl_int); - mMPAdjacentVertexIDsData = (cl_int *)malloc(sizeInBytes); - mMPAdjacentSpringIDsData = (cl_int *)malloc(sizeInBytes); - sizeInBytes = this->GetNumberOfMassPoints() * sizeof(cl_int); - mMPNumberOfAdjacentVertexData = (cl_int *)malloc(sizeInBytes); - mMPStartAdjacentVertexIDData = (cl_int *)malloc(sizeInBytes); - - vector> tmpAdjacentVertexIDs; - vector> tmpAdjacentSpringIDs; - tmpAdjacentVertexIDs.resize(this->GetNumberOfMassPoints()); - tmpAdjacentSpringIDs.resize(this->GetNumberOfMassPoints()); - - for (dtkID i = 0; i < mSprings.size(); i++) { - int id1, id2; - id1 = mSpringVertexIDData[i * 2] = + + void dtkPhysMassSpring::InitialCLArray() { + size_t sizeInBytes = this->GetNumberOfMassPoints() * sizeof(cl_float); + mMPPosData = (cl_float*)malloc(sizeInBytes * 3.0); + mMPVelData = (cl_float*)malloc(sizeInBytes * 3.0); + mMPAccelData = (cl_float*)malloc(sizeInBytes * 3.0); + mMPForceAccumData = (cl_float*)malloc(sizeInBytes * 3.0); + mMPForceDecoratorData = (cl_float*)malloc(sizeInBytes * 3.0); + mMPGravityData = (cl_float*)malloc(sizeInBytes * 3.0); + mMPMassData = (cl_float*)malloc(sizeInBytes); + mMPPosBufferData = (cl_float*)malloc(sizeInBytes * 9.0); + mMPVelBufferData = (cl_float*)malloc(sizeInBytes * 9.0); + mMPAccelBufferData = (cl_float*)malloc(sizeInBytes * 9.0); + + sizeInBytes = this->GetNumberOfMassPoints() * sizeof(cl_bool); + mMPActiveData = (cl_bool*)malloc(sizeInBytes); + + sizeInBytes = this->GetNumberOfSprings() * sizeof(cl_int); + mSpringVertexIDData = (cl_int*)malloc(sizeInBytes * 2.0); + + sizeInBytes = this->GetNumberOfSprings() * sizeof(cl_float); + mSpringOriLengthData = (cl_float*)malloc(sizeInBytes); + mSpringStiffData = (cl_float*)malloc(sizeInBytes); + mSpringDampData = (cl_float*)malloc(sizeInBytes); + + sizeInBytes = this->GetNumberOfSprings() * 2.0 * sizeof(cl_int); + mMPAdjacentVertexIDsData = (cl_int*)malloc(sizeInBytes); + mMPAdjacentSpringIDsData = (cl_int*)malloc(sizeInBytes); + sizeInBytes = this->GetNumberOfMassPoints() * sizeof(cl_int); + mMPNumberOfAdjacentVertexData = (cl_int*)malloc(sizeInBytes); + mMPStartAdjacentVertexIDData = (cl_int*)malloc(sizeInBytes); + + vector> tmpAdjacentVertexIDs; + vector> tmpAdjacentSpringIDs; + tmpAdjacentVertexIDs.resize(this->GetNumberOfMassPoints()); + tmpAdjacentSpringIDs.resize(this->GetNumberOfMassPoints()); + + for (dtkID i = 0; i < mSprings.size(); i++) { + int id1, id2; + id1 = mSpringVertexIDData[i * 2] = mSprings[i]->GetFirstVertex()->GetPointID(); - id2 = mSpringVertexIDData[i * 2 + 1] = + id2 = mSpringVertexIDData[i * 2 + 1] = mSprings[i]->GetSecondVertex()->GetPointID(); - tmpAdjacentVertexIDs[id1].push_back( + tmpAdjacentVertexIDs[id1].push_back( mSprings[i]->GetSecondVertex()->GetPointID()); - tmpAdjacentVertexIDs[id2].push_back( + tmpAdjacentVertexIDs[id2].push_back( mSprings[i]->GetFirstVertex()->GetPointID()); - tmpAdjacentSpringIDs[id1].push_back(i); - tmpAdjacentSpringIDs[id2].push_back(i); - - mSpringOriLengthData[i] = mSprings[i]->GetOriLength(); - mSpringStiffData[i] = mSprings[i]->GetStiffness(); - mSpringDampData[i] = mSprings[i]->GetDamp(); - } - - int startADJ = 0; - int startSADJ = 0; - for (dtkID i = 0; i < mMassPoints.size(); i++) { - dtkT3 pos = mMassPoints[i]->GetPosition(); - dtkT3 vel = mMassPoints[i]->GetVel(); - dtkT3 accel = mMassPoints[i]->GetAccel(); - dtkT3 forceAccum = mMassPoints[i]->GetForceAccum(); - dtkT3 gravity = mMassPoints[i]->GetGravity(); - dtkT3 forceDecorator = mMassPoints[i]->GetForceDecorator(); - std::vector> posBuffer; - posBuffer.resize(3); - posBuffer = mMassPoints[i]->GetPosBuffer(); - std::vector> velBuffer; - velBuffer.resize(3); - velBuffer = mMassPoints[i]->GetVelBuffer(); - std::vector> accelBuffer; - accelBuffer.resize(3); - accelBuffer = mMassPoints[i]->GetAccelBuffer(); - bool activity = mMassPoints[i]->GetActive(); - mMPMassData[i] = mMassPoints[i]->GetMass(); - mMPActiveData[i] = activity; - for (int j = 0; j < 3; j++) { - mMPPosData[i * 3 + j] = pos[j]; - mMPVelData[i * 3 + j] = vel[j]; - mMPAccelData[i * 3 + j] = accel[j]; - mMPForceAccumData[i * 3 + j] = forceAccum[j]; - mMPGravityData[i * 3 + j] = gravity[j]; - mMPForceDecoratorData[i * 3 + j] = forceDecorator[j]; + tmpAdjacentSpringIDs[id1].push_back(i); + tmpAdjacentSpringIDs[id2].push_back(i); + + mSpringOriLengthData[i] = mSprings[i]->GetOriLength(); + mSpringStiffData[i] = mSprings[i]->GetStiffness(); + mSpringDampData[i] = mSprings[i]->GetDamp(); } - for (int j = 0; j < 3; j++) { - for (int m = 0; m < 3; m++) { - mMPPosBufferData[i * 9 + j * 3 + m] = posBuffer[j][m]; - mMPVelBufferData[i * 9 + j * 3 + m] = velBuffer[j][m]; - mMPAccelBufferData[i * 9 + j * 3 + m] = accelBuffer[j][m]; + + int startADJ = 0; + int startSADJ = 0; + for (dtkID i = 0; i < mMassPoints.size(); i++) { + dtkT3 pos = mMassPoints[i]->GetPosition(); + dtkT3 vel = mMassPoints[i]->GetVel(); + dtkT3 accel = mMassPoints[i]->GetAccel(); + dtkT3 forceAccum = mMassPoints[i]->GetForceAccum(); + dtkT3 gravity = mMassPoints[i]->GetGravity(); + dtkT3 forceDecorator = mMassPoints[i]->GetForceDecorator(); + std::vector> posBuffer; + posBuffer.resize(3); + posBuffer = mMassPoints[i]->GetPosBuffer(); + std::vector> velBuffer; + velBuffer.resize(3); + velBuffer = mMassPoints[i]->GetVelBuffer(); + std::vector> accelBuffer; + accelBuffer.resize(3); + accelBuffer = mMassPoints[i]->GetAccelBuffer(); + bool activity = mMassPoints[i]->GetActive(); + mMPMassData[i] = mMassPoints[i]->GetMass(); + mMPActiveData[i] = activity; + for (int j = 0; j < 3; j++) { + mMPPosData[i * 3 + j] = pos[j]; + mMPVelData[i * 3 + j] = vel[j]; + mMPAccelData[i * 3 + j] = accel[j]; + mMPForceAccumData[i * 3 + j] = forceAccum[j]; + mMPGravityData[i * 3 + j] = gravity[j]; + mMPForceDecoratorData[i * 3 + j] = forceDecorator[j]; + } + for (int j = 0; j < 3; j++) { + for (int m = 0; m < 3; m++) { + mMPPosBufferData[i * 9 + j * 3 + m] = posBuffer[j][m]; + mMPVelBufferData[i * 9 + j * 3 + m] = velBuffer[j][m]; + mMPAccelBufferData[i * 9 + j * 3 + m] = accelBuffer[j][m]; + } } + + mMPStartAdjacentVertexIDData[i] = startADJ; + mMPNumberOfAdjacentVertexData[i] = tmpAdjacentVertexIDs[i].size(); + for (int j = 0; j < tmpAdjacentVertexIDs[i].size(); j++) + mMPAdjacentVertexIDsData[startADJ++] = tmpAdjacentVertexIDs[i][j]; + for (int j = 0; j < tmpAdjacentSpringIDs[i].size(); j++) + mMPAdjacentSpringIDsData[startSADJ++] = tmpAdjacentSpringIDs[i][j]; } - mMPStartAdjacentVertexIDData[i] = startADJ; - mMPNumberOfAdjacentVertexData[i] = tmpAdjacentVertexIDs[i].size(); - for (int j = 0; j < tmpAdjacentVertexIDs[i].size(); j++) - mMPAdjacentVertexIDsData[startADJ++] = tmpAdjacentVertexIDs[i][j]; - for (int j = 0; j < tmpAdjacentSpringIDs[i].size(); j++) - mMPAdjacentSpringIDsData[startSADJ++] = tmpAdjacentSpringIDs[i][j]; - } - - assert(startADJ == this->GetNumberOfSprings() * 2); - assert(startSADJ == this->GetNumberOfSprings() * 2); - - mNumberOfMassPoints = this->GetNumberOfMassPoints(); - /* - mSpringOriLengthData[0] = 1; - mSpringStiffData[0] = 2; - mSpringDampData[0] = 3; - mSpringVertexIDData[0] = 4; - mMPPosData[0] = 5; - mMPVelData[0] = 6; - mMPForceAccumData[0] = 7; - mMPGravityData[0] = 8; - mMPMassData[0] = 9; - mMPPosBufferData[0] = 10; - mMPVelBufferData[0] = 11; - mMPAccelBufferData[0] = 12; - mMPForceDecoratorData[0] = 13; - mMPActiveData[0] = true; - mMPAccelData[0] = 15; - */ -} - -void dtkPhysMassSpring::UpdateCopyToCLArray() { - for (dtkID i = 0; i < mMassPoints.size(); i++) { - // dtkT3 vel = mMassPoints[i]->GetVel(); - // dtkT3 accel = mMassPoints[i]->GetAccel(); - // dtkT3 pos = mMassPoints[i]->GetPosition(); - dtkT3 forceAccum = mMassPoints[i]->GetForceAccum(); + assert(startADJ == this->GetNumberOfSprings() * 2); + assert(startSADJ == this->GetNumberOfSprings() * 2); + + mNumberOfMassPoints = this->GetNumberOfMassPoints(); /* - dtkT3 forceDecorator = mMassPoints[i]->GetForceDecorator(); - vector< dtkT3 > posBuffer = mMassPoints[i]->GetPosBuffer(); - vector< dtkT3 > velBuffer = mMassPoints[i]->GetVelBuffer(); - vector< dtkT3 > accelBuffer = mMassPoints[i]->GetAccelBuffer(); + mSpringOriLengthData[0] = 1; + mSpringStiffData[0] = 2; + mSpringDampData[0] = 3; + mSpringVertexIDData[0] = 4; + mMPPosData[0] = 5; + mMPVelData[0] = 6; + mMPForceAccumData[0] = 7; + mMPGravityData[0] = 8; + mMPMassData[0] = 9; + mMPPosBufferData[0] = 10; + mMPVelBufferData[0] = 11; + mMPAccelBufferData[0] = 12; + mMPForceDecoratorData[0] = 13; + mMPActiveData[0] = true; + mMPAccelData[0] = 15; */ - bool activity = mMassPoints[i]->IsActive(); - mMPActiveData[i] = activity; - for (int j = 0; j < 3; j++) { - // mMPVelData[i * 3 + j] = vel[j]; - // mMPAccelData[i * 3 + j] = accel[j]; - // mMPPosData[i * 3 + j] = pos[j]; - mMPForceAccumData[i * 3 + j] = static_cast(forceAccum[j]); + } + + void dtkPhysMassSpring::UpdateCopyToCLArray() { + for (dtkID i = 0; i < mMassPoints.size(); i++) { + // dtkT3 vel = mMassPoints[i]->GetVel(); + // dtkT3 accel = mMassPoints[i]->GetAccel(); + // dtkT3 pos = mMassPoints[i]->GetPosition(); + dtkT3 forceAccum = mMassPoints[i]->GetForceAccum(); + /* + dtkT3 forceDecorator = mMassPoints[i]->GetForceDecorator(); + vector< dtkT3 > posBuffer = mMassPoints[i]->GetPosBuffer(); + vector< dtkT3 > velBuffer = mMassPoints[i]->GetVelBuffer(); + vector< dtkT3 > accelBuffer = mMassPoints[i]->GetAccelBuffer(); + */ + bool activity = mMassPoints[i]->IsActive(); + mMPActiveData[i] = activity; + for (int j = 0; j < 3; j++) { + // mMPVelData[i * 3 + j] = vel[j]; + // mMPAccelData[i * 3 + j] = accel[j]; + // mMPPosData[i * 3 + j] = pos[j]; + mMPForceAccumData[i * 3 + j] = static_cast(forceAccum[j]); + } + /* + for(int j = 0;j < 4;j++) + { + for(int m = 0;m < 3;m++) + { + mMPPosBufferData[i * 12 + j * 3 + m] = posBuffer[j][m]; + mMPVelBufferData[i * 12 + j * 3 + m] = velBuffer[j][m]; + mMPAccelBufferData[i * 12 + j * 3 + m] = accelBuffer[j][m]; + } + } + */ } /* - for(int j = 0;j < 4;j++) - { - for(int m = 0;m < 3;m++) - { - mMPPosBufferData[i * 12 + j * 3 + m] = posBuffer[j][m]; - mMPVelBufferData[i * 12 + j * 3 + m] = velBuffer[j][m]; - mMPAccelBufferData[i * 12 + j * 3 + m] = accelBuffer[j][m]; - } + cl_int status; + status = clEnqueueWriteBuffer(queue, + mMPPosBuffer, + CL_TRUE, + 0, + sizeof(cl_float) * this->GetNumberOfMassPoints() * 3.0, + mMPPosData, + 0, NULL, NULL); + + status = clEnqueueWriteBuffer(queue, + mMPActiveBuffer, + CL_TRUE, + 0, + sizeof(cl_bool) * this->GetNumberOfMassPoints(), + mMPActiveData, + 0, NULL, &writeEvt); + this->CheckCLStatus( status, "write to spring vertex buffer error!"); + status = WaitForEventAndRelease(&writeEvt); + this->CheckCLStatus( status, "wait for evt and release error!"); + */ + clEnqueueWriteBuffer(queue, mMPForceAccumBuffer, CL_TRUE, 0, + sizeof(cl_float) * this->GetNumberOfMassPoints() * 3, + mMPForceAccumData, 0, NULL, NULL); + // clFinish(queue); + } + + void dtkPhysMassSpring::UpdateCopyBack() { + for (dtkID i = 0; i < mMassPoints.size(); i++) { + dtkT3 vel(mMPVelData[i * 3], mMPVelData[i * 3 + 1], + mMPVelData[i * 3 + 2]); + dtkT3 pos(mMPPosData[i * 3], mMPPosData[i * 3 + 1], + mMPPosData[i * 3 + 2]); + mMassPoints[i]->SetPosition(pos); + mMassPoints[i]->SetVel(vel); + dtkT3 velBuffer, posBuffer; + for (int m = 0; m < 3; m++) { + velBuffer[m] = mMPVelBufferData[i * 9 + m]; + posBuffer[m] = mMPPosBufferData[i * 9 + m]; + } + mMassPoints[i]->SetVel(velBuffer, 0); + mMassPoints[i]->SetPosition(posBuffer, 0); + mMassPoints[i]->SetForceAccum(dtkT3(0, 0, 0)); } - */ } - /* -cl_int status; -status = clEnqueueWriteBuffer(queue, - mMPPosBuffer, - CL_TRUE, - 0, - sizeof(cl_float) * this->GetNumberOfMassPoints() * 3.0, - mMPPosData, - 0, NULL, NULL); - -status = clEnqueueWriteBuffer(queue, - mMPActiveBuffer, - CL_TRUE, - 0, - sizeof(cl_bool) * this->GetNumberOfMassPoints(), - mMPActiveData, - 0, NULL, &writeEvt); -this->CheckCLStatus( status, "write to spring vertex buffer error!"); -status = WaitForEventAndRelease(&writeEvt); -this->CheckCLStatus( status, "wait for evt and release error!"); -*/ - clEnqueueWriteBuffer(queue, mMPForceAccumBuffer, CL_TRUE, 0, - sizeof(cl_float) * this->GetNumberOfMassPoints() * 3, - mMPForceAccumData, 0, NULL, NULL); - // clFinish(queue); -} - -void dtkPhysMassSpring::UpdateCopyBack() { - for (dtkID i = 0; i < mMassPoints.size(); i++) { - dtkT3 vel(mMPVelData[i * 3], mMPVelData[i * 3 + 1], - mMPVelData[i * 3 + 2]); - dtkT3 pos(mMPPosData[i * 3], mMPPosData[i * 3 + 1], - mMPPosData[i * 3 + 2]); - mMassPoints[i]->SetPosition(pos); - mMassPoints[i]->SetVel(vel); - dtkT3 velBuffer, posBuffer; - for (int m = 0; m < 3; m++) { - velBuffer[m] = mMPVelBufferData[i * 9 + m]; - posBuffer[m] = mMPPosBufferData[i * 9 + m]; + + void dtkPhysMassSpring::CheckCLStatus(cl_int status, string message) { + if (status != CL_SUCCESS) + cout << message << endl; + return; + } + + int dtkPhysMassSpring::WaitForEventAndRelease(cl_event* event) { + cl_int status = CL_SUCCESS; + cl_int eventStatus = CL_QUEUED; + while (eventStatus != CL_COMPLETE) { + status = clGetEventInfo(*event, CL_EVENT_COMMAND_EXECUTION_STATUS, + sizeof(cl_int), &eventStatus, NULL); + CheckCLStatus(status, "clGetEventInfo error!"); } - mMassPoints[i]->SetVel(velBuffer, 0); - mMassPoints[i]->SetPosition(posBuffer, 0); - mMassPoints[i]->SetForceAccum(dtkT3(0, 0, 0)); - } -} - -void dtkPhysMassSpring::CheckCLStatus(cl_int status, string message) { - if (status != CL_SUCCESS) - cout << message << endl; - return; -} - -int dtkPhysMassSpring::WaitForEventAndRelease(cl_event *event) { - cl_int status = CL_SUCCESS; - cl_int eventStatus = CL_QUEUED; - while (eventStatus != CL_COMPLETE) { - status = clGetEventInfo(*event, CL_EVENT_COMMAND_EXECUTION_STATUS, - sizeof(cl_int), &eventStatus, NULL); - CheckCLStatus(status, "clGetEventInfo error!"); - } - status = clReleaseEvent(*event); - CheckCLStatus(status, "clReleaseEvent error!"); - return CL_SUCCESS; -} - -void dtkPhysMassSpring::UseMultiThread(const char *fileName) { - this->Open(fileName); - this->InitialCLArray(); - this->SetupCL(); -} + status = clReleaseEvent(*event); + CheckCLStatus(status, "clReleaseEvent error!"); + return CL_SUCCESS; + } + + void dtkPhysMassSpring::UseMultiThread(const char* fileName) { + this->Open(fileName); + this->InitialCLArray(); + this->SetupCL(); + } #endif -size_t dtkPhysMassSpring::FindTwins(Ptr ms, double distance) { - size_t count = 0; + size_t dtkPhysMassSpring::FindTwins(Ptr ms, double distance) { + size_t count = 0; - for (dtkID i = 0; i < this->GetNumberOfMassPoints(); i++) { - dtkPhysMassPoint *point1 = this->GetMassPoint(i); - if (point1->HasTwin()) - continue; + for (dtkID i = 0; i < this->GetNumberOfMassPoints(); i++) { + dtkPhysMassPoint* point1 = this->GetMassPoint(i); + if (point1->HasTwin()) + continue; - double minDistance = dtkDoubleMax; - dtkT3 pos1 = point1->GetPosition(); + double minDistance = dtkDoubleMax; + dtkT3 pos1 = point1->GetPosition(); - dtkPhysMassPoint *point2; - dtkT3 pos2; - dtkID minDistanceID = 0; + dtkPhysMassPoint* point2; + dtkT3 pos2; + dtkID minDistanceID = 0; - for (dtkID j = 0; j < ms->GetNumberOfMassPoints(); j++) { - point2 = ms->GetMassPoint(j); - if (point2->HasTwin()) - continue; - double tempDistance; - pos2 = point2->GetPosition(); - tempDistance = length(pos1 - pos2); + for (dtkID j = 0; j < ms->GetNumberOfMassPoints(); j++) { + point2 = ms->GetMassPoint(j); + if (point2->HasTwin()) + continue; + double tempDistance; + pos2 = point2->GetPosition(); + tempDistance = length(pos1 - pos2); - if (tempDistance < minDistance) { - minDistance = tempDistance; - minDistanceID = j; + if (tempDistance < minDistance) { + minDistance = tempDistance; + minDistanceID = j; + } + } + if (minDistance < distance) { + point1->AddTwin(ms->GetMassPoint(minDistanceID)); + ms->GetMassPoint(minDistanceID)->AddTwin(point1); + count++; } } - if (minDistance < distance) { - point1->AddTwin(ms->GetMassPoint(minDistanceID)); - ms->GetMassPoint(minDistanceID)->AddTwin(point1); - count++; - } - } - return count; -} - -void dtkPhysMassSpring::ConvertImpulseToForce(double timeslice) { - mImpulseForce = dtkT3(0, 0, 0); - for (dtkID i = 0; i < this->GetNumberOfMassPoints(); i++) { - dtkPhysMassPoint *point = this->GetMassPoint(i); - mImpulseForce = mImpulseForce + point->GetAndClearImpulse(); + return count; } - mImpulseForce = mImpulseForce * (1.0 / timeslice); -} -void dtkPhysMassSpring::RegisterLabel(dtkID label) { - mLabels.push_back(label); - mTransportForces[label] = dtkT3(0, 0, 0); -} + void dtkPhysMassSpring::ConvertImpulseToForce(double timeslice) { + mImpulseForce = dtkT3(0, 0, 0); + for (dtkID i = 0; i < this->GetNumberOfMassPoints(); i++) { + dtkPhysMassPoint* point = this->GetMassPoint(i); + mImpulseForce = mImpulseForce + point->GetAndClearImpulse(); + } + mImpulseForce = mImpulseForce * (1.0 / timeslice); + } -void dtkPhysMassSpring::TransportForce(double timeslice) { - for (std::map>::iterator itr = mTransportForces.begin(); - itr != mTransportForces.end(); itr++) { - mTransportForces[itr->first] = dtkT3(0, 0, 0); + void dtkPhysMassSpring::RegisterLabel(dtkID label) { + mLabels.push_back(label); + mTransportForces[label] = dtkT3(0, 0, 0); } - for (dtkID i = 0; i < this->GetNumberOfMassPoints(); i++) { - dtkPhysMassPoint *point = this->GetMassPoint(i); - if (!point->IsActive()) { - dtkID label = point->GetLabel(); - if (label != 0) { - mTransportForces[label] = + + void dtkPhysMassSpring::TransportForce(double timeslice) { + for (std::map>::iterator itr = mTransportForces.begin(); + itr != mTransportForces.end(); itr++) { + mTransportForces[itr->first] = dtkT3(0, 0, 0); + } + for (dtkID i = 0; i < this->GetNumberOfMassPoints(); i++) { + dtkPhysMassPoint* point = this->GetMassPoint(i); + if (!point->IsActive()) { + dtkID label = point->GetLabel(); + if (label != 0) { + mTransportForces[label] = mTransportForces[label] + point->GetForceAccum(); + } } } } -} -void dtkPhysMassSpring::SetTriangleMesh( + void dtkPhysMassSpring::SetTriangleMesh( dtkStaticTriangleMesh::Ptr newTriangleMesh) { - this->SetPoints(newTriangleMesh->GetPoints()); - for (dtkID i = 0; i < newTriangleMesh->GetNumberOfPoints(); i++) { - this->AddMassPoint(i, mDefaultMass, dtkT3(0, 0, 0), - mDefaultPointDamp, mDefaultPointResistence, - mDefaultGravityAccel); - } - const std::vector &ec = newTriangleMesh->GetECTable(); - set edges; - for (dtkID i = 0; i < ec.size(); i++) { - if (ec[i][0] > ec[i][1]) - edges.insert(dtkID2(ec[i][1], ec[i][0])); - else - edges.insert(dtkID2(ec[i][0], ec[i][1])); - - if (ec[i][0] > ec[i][2]) - edges.insert(dtkID2(ec[i][2], ec[i][0])); - else - edges.insert(dtkID2(ec[i][0], ec[i][2])); + this->SetPoints(newTriangleMesh->GetPoints()); + for (dtkID i = 0; i < newTriangleMesh->GetNumberOfPoints(); i++) { + this->AddMassPoint(i, mDefaultMass, dtkT3(0, 0, 0), + mDefaultPointDamp, mDefaultPointResistence, + mDefaultGravityAccel); + } + const std::vector& ec = newTriangleMesh->GetECTable(); + set edges; + for (dtkID i = 0; i < ec.size(); i++) { + if (ec[i][0] > ec[i][1]) + edges.insert(dtkID2(ec[i][1], ec[i][0])); + else + edges.insert(dtkID2(ec[i][0], ec[i][1])); + + if (ec[i][0] > ec[i][2]) + edges.insert(dtkID2(ec[i][2], ec[i][0])); + else + edges.insert(dtkID2(ec[i][0], ec[i][2])); + + if (ec[i][2] > ec[i][1]) + edges.insert(dtkID2(ec[i][1], ec[i][2])); + else + edges.insert(dtkID2(ec[i][2], ec[i][1])); + } - if (ec[i][2] > ec[i][1]) - edges.insert(dtkID2(ec[i][1], ec[i][2])); - else - edges.insert(dtkID2(ec[i][2], ec[i][1])); + for (set::iterator itr = edges.begin(); itr != edges.end(); itr++) { + this->AddSpring(itr->a, itr->b, mDefaultStiff, mDefaultDamp); + } } - for (set::iterator itr = edges.begin(); itr != edges.end(); itr++) { - this->AddSpring(itr->a, itr->b, mDefaultStiff, mDefaultDamp); + dtkPhysSpring* dtkPhysMassSpring::GetSpringByPoints(dtkID2 edge) { + map::iterator it; + it = mEdgeMap.find(edge); + if (it != mEdgeMap.end()) + return it->second; + else + return 0; } -} -dtkPhysSpring *dtkPhysMassSpring::GetSpringByPoints(dtkID2 edge) { - map::iterator it; - it = mEdgeMap.find(edge); - if (it != mEdgeMap.end()) - return it->second; - else - return 0; -} - -void dtkPhysMassSpring::DeleteSpring(dtkID id1, dtkID id2) { - if (id1 > id2) - swap(id1, id2); - dtkPhysSpring *spring = mEdgeMap[dtkID2(id1, id2)]; - for (dtkID i = 0; i < mSprings.size(); i++) { - if (mSprings[i] == spring) { - mSprings.erase(mSprings.begin() + i); - break; + void dtkPhysMassSpring::DeleteSpring(dtkID id1, dtkID id2) { + if (id1 > id2) + swap(id1, id2); + dtkPhysSpring* spring = mEdgeMap[dtkID2(id1, id2)]; + for (dtkID i = 0; i < mSprings.size(); i++) { + if (mSprings[i] == spring) { + mSprings.erase(mSprings.begin() + i); + break; + } } + delete spring; } - delete spring; -} -void dtkPhysMassSpring::AbandonTwins() { - for (dtkID i = 0; i < GetNumberOfMassPoints(); i++) { - mMassPoints[i]->AbandonTwins(); + void dtkPhysMassSpring::AbandonTwins() { + for (dtkID i = 0; i < GetNumberOfMassPoints(); i++) { + mMassPoints[i]->AbandonTwins(); + } } -} } // namespace dtk diff --git a/src/include/dtkPhysMassSpring.h b/src/include/dtkPhysMassSpring.h index 8ba47f5..ab74eca 100644 --- a/src/include/dtkPhysMassSpring.h +++ b/src/include/dtkPhysMassSpring.h @@ -35,256 +35,278 @@ using namespace std; #define NWITEMS 256 namespace dtk { -class dtkPhysMassSpring : public boost::noncopyable { -public: - typedef std::shared_ptr Ptr; - - static Ptr - New(double defaultMass = 2, double defaultK = 2000, double defaultDamp = 5880, - double defaultPointDamp = 1.0, double defaultPointResistence = 2.5, - dtkDouble3 defaultGravityAccel = dtkDouble3( - 0, 0, 0)) // K = k0 * cross section area, damp is additional damp - { - return Ptr(new dtkPhysMassSpring(defaultMass, defaultK, defaultDamp, - defaultPointDamp, defaultPointResistence, - defaultGravityAccel)); - } - -public: - virtual ~dtkPhysMassSpring(); - dtkPhysMassSpring(double defaultMass, double defaultK, double defaultDamp, - double defaultPointDamp, - double defaultPointResistence = 2.5, - dtkDouble3 defaultGravityAccel = dtkDouble3(0, 0, 0)); - void SetPoints(dtkPoints::Ptr points); - dtkPoints::Ptr GetPoints(); - - const GK::Point3 &GetPoint(dtkID id) const; - void SetPoint(dtkID id, const GK::Point3 &coord); - - dtkPhysMassPoint *GetMassPoint(dtkID id) { return mMassPoints[id]; }; - dtkID GetNumberOfMassPoints() { return mMassPoints.size(); } - dtkID AddMassPoint(dtkID id, const double &mass = 1.0, - const dtkT3 &vel = dtkT3(0, 0, 0), - double pointDamp = 1.0, double pointResistence = 2.5, - dtkDouble3 defaultGravityAccel = dtkDouble3(0, 0, 0)); - - dtkPhysSpring *GetSpring(dtkID id) { return mSprings[id]; }; - dtkID GetNumberOfSprings() { return (dtkID)mSprings.size(); } - dtkID AddSpring(dtkID p1, dtkID p2, const double &stiff = 0, - const double &damp = 0); - void DeleteSpring(dtkID p1, dtkID p2); - - void SetSpringStiffness(int id, double newK); - void SetSpringDamp(int id, double newDamp); - void SetPointMass(int id, double newMass); - - /** - * @brief 更新弹簧及质点状态 - * @param[in] timeslice : 更新时间间隔 - * @param[in] method : 迭代更新算法 - * @param[in] limitDeformation : 弹簧弹性限度 - * @note 更新弹簧及质点状态 - * @return - * true update successfully \n - * false update failure \n - */ - bool Update(double timeslice, ItrMethod method = Euler, - bool limitDeformation = false); - - /** - * @brief 单线程更新弹簧及质点状态 - * @param[in] timeslice : 更新时间间隔 - * @param[in] method : 迭代更新算法 - * @param[in] limitDeformation : 弹簧弹性限度 - * @note 单线程更新弹簧及质点状态 - * @return - * true update successfully \n - * false update failure \n - */ - bool Update_s(double timeslice, ItrMethod method = Euler, + class dtkPhysMassSpring : public boost::noncopyable { + public: + typedef std::shared_ptr Ptr; + + static Ptr + New(double defaultMass = 2, double defaultK = 2000, double defaultDamp = 5880, + double defaultPointDamp = 1.0, double defaultPointResistence = 2.5, + dtkDouble3 defaultGravityAccel = dtkDouble3( + 0, 0, 0)) // K = k0 * cross section area, damp is additional damp + { + return Ptr(new dtkPhysMassSpring(defaultMass, defaultK, defaultDamp, + defaultPointDamp, defaultPointResistence, + defaultGravityAccel)); + } + + static Ptr + New(double defaultMass = 2, double defaultK = 2000, double defaultDamp = 5880, + double defaultPointDamp = 1.0, double defaultPointResistence = 2.5, double defaultTimeStep = 0.01, + dtkDouble3 defaultGravityAccel = dtkDouble3( + 0, 0, 0)) // K = k0 * cross section area, damp is additional damp + { + return Ptr(new dtkPhysMassSpring(defaultMass, defaultK, defaultDamp, + defaultPointDamp, defaultPointResistence, defaultTimeStep, + defaultGravityAccel)); + } + + public: + virtual ~dtkPhysMassSpring(); + dtkPhysMassSpring(double defaultMass, double defaultK, double defaultDamp, + double defaultPointDamp, + double defaultPointResistence = 2.5, + dtkDouble3 defaultGravityAccel = dtkDouble3(0, 0, 0)); + dtkPhysMassSpring(double defaultMass, double defaultK, double defaultDamp, + double defaultPointDamp, + double defaultPointResistence = 2.5, + double defaultTimeStep = 0.01, + dtkDouble3 defaultGravityAccel = dtkDouble3(0, 0, 0)); + void SetPoints(dtkPoints::Ptr points); + dtkPoints::Ptr GetPoints(); + + const GK::Point3& GetPoint(dtkID id) const; + void SetPoint(dtkID id, const GK::Point3& coord); + + dtkPhysMassPoint* GetMassPoint(dtkID id) { return mMassPoints[id]; }; + dtkID GetNumberOfMassPoints() { return mMassPoints.size(); } + dtkID AddMassPoint(dtkID id, const double& mass = 1.0, + const dtkT3& vel = dtkT3(0, 0, 0), + double pointDamp = 1.0, double pointResistence = 2.5, + dtkDouble3 defaultGravityAccel = dtkDouble3(0, 0, 0)); + + dtkPhysSpring* GetSpring(dtkID id) { return mSprings[id]; }; + dtkID GetNumberOfSprings() { return (dtkID)mSprings.size(); } + dtkID AddSpring(dtkID p1, dtkID p2, const double& stiff = 0, + const double& damp = 0, const double& rest_length_factor = 1.0); + void DeleteSpring(dtkID p1, dtkID p2); + + void SetSpringStiffness(int id, double newK); + void SetSpringDamp(int id, double newDamp); + void SetPointMass(int id, double newMass); + void SetTimeStep(double timeStep); + double GetTimeStep(); + double GetDefaultStiffness() const { return mDefaultStiff; } + double GetDefaultDamp() const { return mDefaultDamp; } + dtk::dtkDouble3 GetDefaultGravityAccel() const { return mDefaultGravityAccel; } + + /** + * @brief 更新弹簧及质点状态 + * @param[in] timeslice : 更新时间间隔 + * @param[in] method : 迭代更新算法 + * @param[in] limitDeformation : 弹簧弹性限度 + * @note 更新弹簧及质点状态 + * @return + * true update successfully \n + * false update failure \n + */ + bool Update(double timeslice, ItrMethod method = Euler, bool limitDeformation = false); - virtual bool PreUpdate(double timeslice, ItrMethod method = Euler, - dtkID iteration = 0); - virtual bool PostUpdate(ItrMethod method = Euler, dtkID iteration = 0); - // 更新迭代 - bool Update_iteration(double timeslice, ItrMethod method = Euler, - dtkID iteration = 0, bool limitDeformation = false); + /** + * @brief 单线程更新弹簧及质点状态 + * @param[in] timeslice : 更新时间间隔 + * @param[in] method : 迭代更新算法 + * @param[in] limitDeformation : 弹簧弹性限度 + * @note 单线程更新弹簧及质点状态 + * @return + * true update successfully \n + * false update failure \n + */ + bool Update_s(double timeslice, ItrMethod method = Euler, + bool limitDeformation = false); + virtual bool PreUpdate(double timeslice, ItrMethod method = Euler, + dtkID iteration = 0); + virtual bool PostUpdate(ItrMethod method = Euler, dtkID iteration = 0); - // 应用冲量 - bool ApplyImpulse(double timeslice); + // 更新迭代 + bool Update_iteration(double timeslice, ItrMethod method = Euler, + dtkID iteration = 0, bool limitDeformation = false); - /** - * @brief 更新弹簧状态，点位置，速度等 - * @param[in] timeslice : 更新时间间隔 - * @param[in] method : 迭代更新算法 - * @param[in] limitDeformation : 弹簧弹性限度 - * @note 更新弹簧状态，点位置，速度等 - * @return - * true update successfully \n - * false update failure \n - */ + // 应用冲量 + bool ApplyImpulse(double timeslice); - bool UpdateStrings(double timeslice, ItrMethod method = Euler, - dtkID iteration = 0, bool limitDeformation = false); + /** + * @brief 更新弹簧状态，点位置，速度等 + * @param[in] timeslice : 更新时间间隔 + * @param[in] method : 迭代更新算法 + * @param[in] limitDeformation : 弹簧弹性限度 + * @note 更新弹簧状态，点位置，速度等 + * @return + * true update successfully \n + * false update failure \n + */ - /** - * @brief 更新质点状态，点位置，速度等 - * @param[in] timeslice : 更新时间间隔 - * @param[in] method : 迭代更新算法 - * @param[in] limitDeformation : 弹簧弹性限度 - * @note 更新质点状态，点位置，速度等 - * @return - * true update successfully \n - * false update failure \n - */ + bool UpdateStrings(double timeslice, ItrMethod method = Euler, + dtkID iteration = 0, bool limitDeformation = false); - bool UpdateMassPoints(double timeslice, ItrMethod method = Euler, - dtkID iteration = 0); + /** + * @brief 更新质点状态，点位置，速度等 + * @param[in] timeslice : 更新时间间隔 + * @param[in] method : 迭代更新算法 + * @param[in] limitDeformation : 弹簧弹性限度 + * @note 更新质点状态，点位置，速度等 + * @return + * true update successfully \n + * false update failure \n + */ - // 从三角网格添加质量弹簧 + bool UpdateMassPoints(double timeslice, ItrMethod method = Euler, + dtkID iteration = 0); - void SetTriangleMesh(dtkStaticTriangleMesh::Ptr newTriangleMesh); + // 从三角网格添加质量弹簧 - // 寻找周围距离小于distance的twins点. + void SetTriangleMesh(dtkStaticTriangleMesh::Ptr newTriangleMesh); - size_t FindTwins(Ptr, double distance); - void AbandonTwins(); + // 寻找周围距离小于distance的twins点. - void RegisterLabel(dtkID label); + size_t FindTwins(Ptr, double distance); + void AbandonTwins(); - void TransportForce(double timeslice); + void RegisterLabel(dtkID label); - const dtkT3 &GetTransportForce(dtkID label) { - return mTransportForces[label]; - } + void TransportForce(double timeslice); - void ConvertImpulseToForce(double timeslice); + const dtkT3& GetTransportForce(dtkID label) { + return mTransportForces[label]; + } - const dtkT3 &GetImpulseForce() { return mImpulseForce; } + void ConvertImpulseToForce(double timeslice); - bool IsUnderControl() { return mUnderControl; } + const dtkT3& GetImpulseForce() { return mImpulseForce; } - void SetUnderControl(bool underControl) { mUnderControl = underControl; } + bool IsUnderControl() { return mUnderControl; } - dtkPhysSpring *GetSpringByPoints(dtkID2); + void SetUnderControl(bool underControl) { mUnderControl = underControl; } + + dtkPhysSpring* GetSpringByPoints(dtkID2); #ifdef DTK_CL - /** - * OpenCL related initialisations. - * Set up Context, Device list, Command Queue, Memory buffers - * Build CL kernel program executable - * @return 1 on success and 0 on failure - */ - bool Update_mt(double timeslice, ItrMethod method = Euler, - bool limitDeformation = false); - int SetupCL(); - int RunCLKernels(double timeslice, dtkID iteration); - bool Open(const char *fileName); - void InitialCLArray(); - void UpdateCopyToCLArray(); - void UpdateCopyBack(); - void CheckCLStatus(cl_int, std::string); - int WaitForEventAndRelease(cl_event *); - void UseMultiThread(const char *fileName); + /** + * OpenCL related initialisations. + * Set up Context, Device list, Command Queue, Memory buffers + * Build CL kernel program executable + * @return 1 on success and 0 on failure + */ + bool Update_mt(double timeslice, ItrMethod method = Euler, + bool limitDeformation = false); + int SetupCL(); + int RunCLKernels(double timeslice, dtkID iteration); + bool Open(const char* fileName); + void InitialCLArray(); + void UpdateCopyToCLArray(); + void UpdateCopyBack(); + void CheckCLStatus(cl_int, std::string); + int WaitForEventAndRelease(cl_event*); + void UseMultiThread(const char* fileName); #endif #ifdef DTK_DEBUG -public: - std::vector> mAltitudeSpringForces; - std::vector> mTotalSpringForces; + public: + std::vector> mAltitudeSpringForces; + std::vector> mTotalSpringForces; #endif -protected: - dtkPoints::Ptr mPts; /**< 点集 */ - std::vector mMassPoints; /**< 质点集 */ - std::vector mSprings; /**< 弹簧 */ + protected: + dtkPoints::Ptr mPts; /**< 点集 */ + std::vector mMassPoints; /**< 质点集 */ + std::vector mSprings; /**< 弹簧 */ - // 边集 - std::map + // 边集 + std::map mEdgeMap; /**< map from spring specified by dtkID2 to spring id */ - double mDefaultMass; /**< 质量 */ - double mDefaultStiff; /**< 弹簧刚性系数，弹性系数 */ - double mDefaultDamp; /**< 弹簧阻尼 */ - double mDefaultPointDamp; /**< 点阻尼 */ - double mDefaultPointResistence; /**< 阻力 */ - dtkDouble3 mDefaultGravityAccel; /**< 重力加速度 */ + double mDefaultMass; /**< 质量 */ + double mDefaultStiff; /**< 弹簧刚性系数，弹性系数 */ + double mDefaultDamp; /**< 弹簧阻尼 */ + double mDefaultPointDamp; /**< 点阻尼 */ + double mDefaultPointResistence; /**< 阻力 */ + double mDefaultTimeStep; /**< 时间步长 */ + dtkDouble3 mDefaultGravityAccel; /**< 重力加速度 */ - bool mUnderControl; /**< 弹簧受控 */ + bool mUnderControl; /**< 弹簧受控 */ - std::vector mLabels; // 标记点集, 可给予Transport力 + std::vector mLabels; // 标记点集, 可给予Transport力 - std::map> mTransportForces; // - dtkT3 mImpulseForce; // 瞬时力 + std::map> mTransportForces; // + dtkT3 mImpulseForce; // 瞬时力 #ifdef DTK_CL - bool mUseMultiThread; - // OpenCL - cl_float mTimesliceData; - cl_int mIterationData; - cl_int mNumberOfMassPoints; - - cl_int *mSpringVertexIDData; - cl_mem mSpringVertexIDBuffer; - cl_float *mSpringOriLengthData; - cl_mem mSpringOriLengthBuffer; - cl_float *mSpringStiffData; - cl_mem mSpringStiffBuffer; - cl_float *mSpringDampData; - cl_mem mSpringDampBuffer; - - cl_float *mMPPosData; - cl_mem mMPPosBuffer; - cl_float *mMPVelData; - cl_mem mMPVelBuffer; - cl_float *mMPAccelData; - cl_mem mMPAccelBuffer; - cl_float *mMPForceAccumData; - cl_mem mMPForceAccumBuffer; - cl_float *mMPGravityData; - cl_mem mMPGravityBuffer; - cl_float *mMPMassData; - cl_mem mMPMassBuffer; - cl_float *mMPPosBufferData; - cl_mem mMPPosBufferBuffer; - cl_float *mMPVelBufferData; - cl_mem mMPVelBufferBuffer; - cl_float *mMPAccelBufferData; - cl_mem mMPAccelBufferBuffer; - cl_float *mMPForceDecoratorData; - cl_mem mMPForceDecoratorBuffer; - cl_bool *mMPActiveData; - cl_mem mMPActiveBuffer; - - // new spring calculate - cl_int *mMPAdjacentVertexIDsData; - cl_mem mMPAdjacentVertexIDsBuffer; - cl_int *mMPNumberOfAdjacentVertexData; - cl_mem mMPNumberOfAdjacentVertexBuffer; - cl_int *mMPStartAdjacentVertexIDData; - cl_mem mMPStartAdjacentVertexIDBuffer; - - cl_int *mMPAdjacentSpringIDsData; - cl_mem mMPAdjacentSpringIDsBuffer; - - size_t maxWorkGroupSize; - size_t *maxWorkItemSizes; - - cl_context context; - cl_platform_id platform; - cl_device_id device; - - cl_command_queue queue; - cl_program program; - cl_kernel kernelSpring; - cl_kernel kernelMP; - size_t global_work_size; - size_t work_size; - std::string source_; + bool mUseMultiThread; + // OpenCL + cl_float mTimesliceData; + cl_int mIterationData; + cl_int mNumberOfMassPoints; + + cl_int* mSpringVertexIDData; + cl_mem mSpringVertexIDBuffer; + cl_float* mSpringOriLengthData; + cl_mem mSpringOriLengthBuffer; + cl_float* mSpringStiffData; + cl_mem mSpringStiffBuffer; + cl_float* mSpringDampData; + cl_mem mSpringDampBuffer; + + cl_float* mMPPosData; + cl_mem mMPPosBuffer; + cl_float* mMPVelData; + cl_mem mMPVelBuffer; + cl_float* mMPAccelData; + cl_mem mMPAccelBuffer; + cl_float* mMPForceAccumData; + cl_mem mMPForceAccumBuffer; + cl_float* mMPGravityData; + cl_mem mMPGravityBuffer; + cl_float* mMPMassData; + cl_mem mMPMassBuffer; + cl_float* mMPPosBufferData; + cl_mem mMPPosBufferBuffer; + cl_float* mMPVelBufferData; + cl_mem mMPVelBufferBuffer; + cl_float* mMPAccelBufferData; + cl_mem mMPAccelBufferBuffer; + cl_float* mMPForceDecoratorData; + cl_mem mMPForceDecoratorBuffer; + cl_bool* mMPActiveData; + cl_mem mMPActiveBuffer; + + // new spring calculate + cl_int* mMPAdjacentVertexIDsData; + cl_mem mMPAdjacentVertexIDsBuffer; + cl_int* mMPNumberOfAdjacentVertexData; + cl_mem mMPNumberOfAdjacentVertexBuffer; + cl_int* mMPStartAdjacentVertexIDData; + cl_mem mMPStartAdjacentVertexIDBuffer; + + cl_int* mMPAdjacentSpringIDsData; + cl_mem mMPAdjacentSpringIDsBuffer; + + size_t maxWorkGroupSize; + size_t* maxWorkItemSizes; + + cl_context context; + cl_platform_id platform; + cl_device_id device; + + cl_command_queue queue; + cl_program program; + cl_kernel kernelSpring; + cl_kernel kernelMP; + size_t global_work_size; + size_t work_size; + std::string source_; #endif -}; + }; }; // namespace dtk #endif /* SIMPLEPHYSICSENGINE_DTKPHYSMASSSPRING_H */ diff --git a/src/include/dtkPhysSpring.h b/src/include/dtkPhysSpring.h index 0d03e54..d1735e0 100644 --- a/src/include/dtkPhysSpring.h +++ b/src/include/dtkPhysSpring.h @@ -21,33 +21,36 @@ #include "dtkPhysMassPoint.h" namespace dtk { -class dtkPhysSpring { -public: - dtkPhysSpring(dtkPhysMassPoint *p1, dtkPhysMassPoint *p2, - const double &stiff = 5, const double &damp = 0); - - dtkPhysMassPoint *GetFirstVertex() { return mVerteces[0]; } - dtkPhysMassPoint *GetSecondVertex() { return mVerteces[1]; } - - // 更新力到质点,迭代更新 - bool Update(double timeslice, ItrMethod method = Euler, dtkID iteration = 0, - bool limitDeformation = false); - void SetStiffness(double newStiffness) { mStiffness = newStiffness; } - void SetDamp(double newDamp) { mDamp = newDamp; } - double GetOriLength() { return mOriLength; } - double GetStiffness() { return mStiffness; } - double GetDamp() { return mDamp; } - -public: - virtual ~dtkPhysSpring(); - -private: - dtkPhysMassPoint *mVerteces[2]; /**< 弹簧两个质点 */ - - double mOriLength; /**< 长度 */ - double mStiffness; /**< 刚度 */ - double mDamp; /**< 阻尼 */ -}; + class dtkPhysSpring { + public: + dtkPhysSpring(dtkPhysMassPoint* p1, dtkPhysMassPoint* p2, + const double& stiff = 5, const double& damp = 0); + + dtkPhysMassPoint* GetFirstVertex() { return mVerteces[0]; } + dtkPhysMassPoint* GetSecondVertex() { return mVerteces[1]; } + + // 更新力到质点,迭代更新 + bool Update(double timeslice, ItrMethod method = Euler, dtkID iteration = 0, + bool limitDeformation = false); + void SetStiffness(double newStiffness) { mStiffness = newStiffness; } + void SetDamp(double newDamp) { mDamp = newDamp; } + double GetOriLength() { return mOriLength; } + void SetRestLength(const double& newRestLength) { mRestLength = newRestLength; } + double GetRestLength() { return mRestLength; } + double GetStiffness() { return mStiffness; } + double GetDamp() { return mDamp; } + + public: + virtual ~dtkPhysSpring(); + + private: + dtkPhysMassPoint* mVerteces[2]; /**< 弹簧两个质点 */ + + double mOriLength; /**< 原始长度 */ + double mRestLength; /**< 静息长度 */ + double mStiffness; /**< 刚度 */ + double mDamp; /**< 阻尼 */ + }; } // namespace dtk #endif /* SIMPLEPHYSICSENGINE_DTKPHYSSPRING_H */ diff --git a/src/math/include/dtkSparseMatrix.h b/src/math/include/dtkSparseMatrix.h new file mode 100644 index 0000000..644aaa1 --- /dev/null +++ b/src/math/include/dtkSparseMatrix.h @@ -0,0 +1,129 @@ +/** + * @file dtkSparseMatrix.h + * @brief dtkSparseMatrix 头文件 + * @author Chance.H (chancewithchance@outlook.com) + * @version 1.0 + * @date 2024-7-30 + * @copyright MIT LICENSE + * https://github.com/Simple-XX/SimplePhysicsEngine + */ +#ifndef SIMPLEPHYSICSENGINE_DTKSPARSEMATRIX_H +#define SIMPLEPHYSICSENGINE_DTKSPARSEMATRIX_H +#include +#include +#include + +namespace dtk { + + /** + * @class + * @brief 稀疏矩阵 + * @author <> + * @note + * 实现稀疏矩阵 + */ + template + class dtkSparseMatrix { + public: + typedef unsigned int Index; + typedef T ValueType; + + struct Triplet { + Index row, col; + ValueType value; + + Triplet(Index r, Index c, const ValueType& v) : row(r), col(c), value(v) {} + }; + + dtkSparseMatrix() : rows_(0), cols_(0) {} + + dtkSparseMatrix(Index rows, Index cols) : rows_(rows), cols_(cols) {} + + void resize(Index rows, Index cols) { + rows_ = rows; + cols_ = cols; + triplets_.clear(); + } + + void insert(Index row, Index col, const ValueType& value) { + assert(row < rows_ && col < cols_); + triplets_.emplace_back(row, col, value); + } + + void setFromTriplets(const std::vector& triplets) { + triplets_ = triplets; + } + + void reserve(Index reserveSize) { + triplets_.reserve(reserveSize); + } + + void clear() { + triplets_.clear(); + } + + Index rows() const { return rows_; } + Index cols() const { return cols_; } + Index nonZeros() const { return triplets_.size(); } + + const std::vector& triplets() const { return triplets_; } + + dtkSparseMatrix transpose() const { + dtkSparseMatrix result(cols_, rows_); + for (const auto& triplet : triplets_) { + result.insert(triplet.col, triplet.row, triplet.value); + } + return result; + } + + template + std::vector operator*(const VectorT& rhs) const { + assert(rhs.size() == cols_); + std::vector result(rows_, T(0)); + for (const auto& triplet : triplets_) { + result[triplet.row] += triplet.value * rhs[triplet.col]; + } + return result; + } + + dtkSparseMatrix operator+(const dtkSparseMatrix& rhs) const { + assert(rows_ == rhs.rows_ && cols_ == rhs.cols_); + dtkSparseMatrix result(rows_, cols_); + result.triplets_ = triplets_; + for (const auto& triplet : rhs.triplets_) { + auto it = std::find_if(result.triplets_.begin(), result.triplets_.end(), + [&](const Triplet& t) { return t.row == triplet.row && t.col == triplet.col; }); + if (it != result.triplets_.end()) { + it->value += triplet.value; + } + else { + result.triplets_.emplace_back(triplet.row, triplet.col, triplet.value); + } + } + return result; + } + + dtkSparseMatrix operator-(const dtkSparseMatrix& rhs) const { + assert(rows_ == rhs.rows_ && cols_ == rhs.cols_); + dtkSparseMatrix result(rows_, cols_); + result.triplets_ = triplets_; + for (const auto& triplet : rhs.triplets_) { + auto it = std::find_if(result.triplets_.begin(), result.triplets_.end(), + [&](const Triplet& t) { return t.row == triplet.row && t.col == triplet.col; }); + if (it != result.triplets_.end()) { + it->value -= triplet.value; + } + else { + result.triplets_.emplace_back(triplet.row, triplet.col, -triplet.value); + } + } + return result; + } + + private: + Index rows_, cols_; + std::vector triplets_; + }; + +} // namespace dtk +#endif /* SIMPLEPHYSICSENGINE_DTKSPARSEMATRIX_H */ \ No newline at end of file diff --git a/src/math/include/dtkVector.h b/src/math/include/dtkVector.h new file mode 100644 index 0000000..d760896 --- /dev/null +++ b/src/math/include/dtkVector.h @@ -0,0 +1,265 @@ +/** + * @file dtkVector.h + * @brief dtkVector 头文件 + * @author Chance.H (chancewithchance@outlook.com) + * @version 1.0 + * @date 2024-7-29 + * @copyright MIT LICENSE + * https://github.com/Simple-XX/SimplePhysicsEngine + */ +#ifndef SIMPLEPHYSICSENGINE_DTKVECTOR_H +#define SIMPLEPHYSICSENGINE_DTKVECTOR_H +#include +#include + +#include + +namespace dtk { + /** + * @class + * @brief 向量 + * @author <> + * @note + * 实现任意维的向量 + */ + template > struct dtkVector { + // STL-friendly typedefs + + typedef typename VectorT::iterator iterator; + typedef typename VectorT::const_iterator const_iterator; + typedef typename VectorT::size_type size_type; + typedef long difference_type; + typedef T& reference; + typedef const T& const_reference; + typedef T value_type; + typedef T* pointer; + typedef const T* const_pointer; + typedef typename VectorT::reverse_iterator reverse_iterator; + typedef typename VectorT::const_reverse_iterator const_reverse_iterator; + + // the actual representation + + unsigned int n; + VectorT v; + + // the interface + + dtkVector(void) : n(0) {} + + dtkVector(int n_) : n(n_), v(n_) { + assert(n_ >= 0); + } + + dtkVector(int n_, VectorT& v_) : n(n_), v(v_) { + assert(n_ >= 0); + } + + dtkVector(int n_, const T& value) + : n(n_), v(n_, value) { + assert(n_ >= 0); + } + + dtkVector(int n_, const T& value, size_type max_n_) + : n(n_), v(n_, value, max_n_) { + assert(n_ >= 0); + } + + void init(const T* values) { + for (unsigned int i = 0; i < n; i++) + v[i] = values[i]; + } + + template + dtkVector(dtkVector& other) + : n(other.n), v(other.v) {} + + ~dtkVector(void) { +#ifndef NDEBUG + n = 0; +#endif + } + + const T& operator()(unsigned int i) const { + assert(i >= 0 && i < n); + return v[i]; + } + + T& operator()(unsigned int i) { + assert(i >= 0 && i < n); + return v[i]; + } + + const T& operator[](unsigned int i) const { + assert(i >= 0 && i < n); + return v[i]; + } + + T& operator[](unsigned int i) { + assert(i >= 0 && i < n); + return v[i]; + } + + bool operator==(const dtkVector& x) const { + return n == x.n && v == x.v; + } + + bool operator!=(const dtkVector& x) const { + return n != x.n || v != x.v; + } + + bool operator<(const dtkVector& x) const { + if (n < x.n) + return true; + else if (n > x.n) + return false; + return v < x.v; + } + + bool operator>(const dtkVector& x) const { + if (n > x.n) + return true; + else if (n < x.n) + return false; + return v > x.v; + } + + bool operator<=(const dtkVector& x) const { + if (n < x.n) + return true; + else if (n > x.n) + return false; + return v <= x.v; + } + + bool operator>=(const dtkVector& x) const { + if (n > x.n) + return true; + else if (n < x.n) + return false; + return v >= x.v; + } + + void assign(const T& value) { v.assign(value); } + + void assign(int n_, const T& value) { + v.assign(n_, value); + n = n_; + } + + void assign(int n_, const T* copydata) { + v.assign(n_, copydata); + n = n_; + } + + const T& at(int i) const { + assert(i >= 0 && i < n); + return v[i]; + } + + T& at(int i) { + assert(i >= 0 && i < n); + return v[i]; + } + + const T& back(void) const { + assert(v.size()); + return v.back(); + } + + T& back(void) { + assert(v.size()); + return v.back(); + } + + const_iterator begin(void) const { return v.begin(); } + + iterator begin(void) { return v.begin(); } + + size_type capacity(void) const { return v.capacity(); } + + void clear(void) { + v.clear(); + n = 0; + } + + bool empty(void) const { return v.empty(); } + + const_iterator end(void) const { return v.end(); } + + iterator end(void) { return v.end(); } + + void fill(int n_, const T& value) { + v.fill(n_, value); + n = n_; + } + + const T& front(void) const { + assert(v.size()); + return v.front(); + } + + T& front(void) { + assert(v.size()); + return v.front(); + } + + size_type max_size(void) const { return v.max_size(); } + + reverse_iterator rbegin(void) { return reverse_iterator(end()); } + + const_reverse_iterator rbegin(void) const { + return const_reverse_iterator(end()); + } + + reverse_iterator rend(void) { return reverse_iterator(begin()); } + + const_reverse_iterator rend(void) const { + return const_reverse_iterator(begin()); + } + + void reserve(int reserve_n) { + v.reserve(reserve_n); + } + + void resize(int n_) { + assert(n_ >= 0); + v.resize(n_); + n = n_; + } + + void resize(int n_, const T& value) { + assert(n_ >= 0); + v.resize(n_, value); + n = n_; + } + + void set_zero(void) { v.set_zero(); } + + size_type size(void) const { return v.size(); } + + void swap(dtkVector& x) { + std::swap(n, x.n); + v.swap(x.v); + } + + void trim(void) { v.trim(); } + }; + + // some common vectors + typedef dtkVector> dtkVectorDouble; + typedef dtkVector> dtkVectorFloat; + typedef dtkVector> dtkVectorLLong; + typedef dtkVector> dtkVectorULLong; + typedef dtkVector> dtkVectorInt; + typedef dtkVector> dtkVectorUInt; + typedef dtkVector> dtkVectorShort; + typedef dtkVector> dtkVectorUShort; + typedef dtkVector> dtkVectorChar; + typedef dtkVector> dtkVectorUChar; + + typedef glm::vec2 dtkVector2; + typedef glm::vec3 dtkVector3; + typedef glm::vec4 dtkVector4; + +} +#endif // SIMPLEPHYSICSENGINE_DTKVECTOR_H \ No newline at end of file diff --git a/src/math/include/dtkVectorOP.h b/src/math/include/dtkVectorOP.h new file mode 100644 index 0000000..b1fe2c1 --- /dev/null +++ b/src/math/include/dtkVectorOP.h @@ -0,0 +1,142 @@ +/** + * @file dtkVectorOP.h + * @brief dtkVectorOP 头文件 + * @author Chance.H (chancewithchance@outlook.com) + * @version 1.0 + * @date 2024-7-29 + * @copyright MIT LICENSE + * https://github.com/Simple-XX/SimplePhysicsEngine + */ +#ifndef SIMPLEPHYSICSENGINE_DTKVECTOROP_H +#define SIMPLEPHYSICSENGINE_DTKVECTOROP_H +#include +#include + +#include "dtkVector.h" +#include "dtkMatrix.h" + +namespace dtk { + template inline dtkVector transform_identity_vector() { + dtkVector v(4, 0); + v[0] = 1; + v[1] = 1; + v[2] = 1; + v[3] = 1; + return v; + } + + template inline dtkVector rotate_x_vector(dtkVector v, T angle) { + dtkMatrix m = rotate_x_matrix(angle); + return m * v; + } + + template inline dtkVector rotate_y_vector(dtkVector v, T angle) { + dtkMatrix m = rotate_y_matrix(angle); + return m * v; + } + + template inline dtkVector rotate_z_vector(dtkVector v, T angle) { + dtkMatrix m = rotate_z_matrix(angle); + return m * v; + } + + template inline dtkVector translate_vector(dtkVector v, T x, T y, T z) { + dtkMatrix m = translate_matrix(x, y, z); + return m * v; + } + + template inline dtkVector scale_vector(dtkVector v, T x, T y, T z) { + dtkMatrix m = scale_matrix(x, y, z); + return m * v; + } + + template + inline dtkVector operator+(const dtkVector& lhs, const dtkVector& rhs) { + assert(lhs.n == rhs.n); + dtkVector v(lhs.n); + for (unsigned int i = 0; i < v.n; i++) + v[i] = lhs[i] + rhs[i]; + return v; + } + + template + inline dtkVector operator-(const dtkVector& lhs, const dtkVector& rhs) { + assert(lhs.n == rhs.n); + dtkVector v(lhs.n); + for (unsigned int i = 0; i < v.n; i++) + v[i] = lhs[i] - rhs[i]; + return v; + } + + template + inline dtkVector operator*(T lhs, const dtkVector& rhs) { + dtkVector v(rhs.n); + for (unsigned int i = 0; i < v.n; i++) + v[i] = lhs * rhs[i]; + return v; + } + + template + inline dtkVector operator*(const dtkVector& lhs, T rhs) { + dtkVector v(lhs.n); + for (unsigned int i = 0; i < v.n; i++) + v[i] = lhs[i] * rhs; + return v; + } + + template + inline T dot(const dtkVector& lhs, const dtkVector& rhs) { + assert(lhs.n == rhs.n); + T result = 0; + for (unsigned int i = 0; i < lhs.n; i++) + result += lhs[i] * rhs[i]; + return result; + } + + template + inline dtkVector cross(const dtkVector& lhs, const dtkVector& rhs) { + assert(lhs.n == 3 && rhs.n == 3); + dtkVector v(3); + v[0] = lhs[1] * rhs[2] - lhs[2] * rhs[1]; + v[1] = lhs[2] * rhs[0] - lhs[0] * rhs[2]; + v[2] = lhs[0] * rhs[1] - lhs[1] * rhs[0]; + return v; + } + + template + inline dtkVector normalize(const dtkVector& v) { + T length = std::sqrt(dot(v, v)); + assert(length != 0); + return (1 / length) * v; + } + + template inline dtkMatrix outer_product(const dtkVector& lhs, const dtkVector& rhs) { + dtkMatrix m(lhs.n, rhs.n); + for (unsigned int i = 0; i < lhs.n; i++) + for (unsigned int j = 0; j < rhs.n; j++) + m(i, j) = lhs[i] * rhs[j]; + return m; + } + + template + inline dtkVector operator*(const dtkMatrix& lhs, const dtkVector& rhs) { + assert(lhs.nj == rhs.n); + dtkVector v(lhs.ni, 0); + for (unsigned int i = 0; i < lhs.ni; i++) + for (unsigned int j = 0; j < lhs.nj; j++) + v[i] += lhs(i, j) * rhs[j]; + return v; + } + + template + inline dtkVector operator*(const dtkVector& lhs, const dtkMatrix& rhs) { + assert(lhs.n == rhs.ni); + dtkVector v(rhs.nj, 0); + for (unsigned int i = 0; i < rhs.nj; i++) + for (unsigned int j = 0; j < lhs.n; j++) + v[i] += lhs[j] * rhs(j, i); + return v; + } + +} +#endif //SIMPLEPHYSICSENGINE_DTKVECTOROP_H \ No newline at end of file diff --git a/test/system_test/MassSpring3D/CMakeLists.txt b/test/system_test/MassSpring3D/CMakeLists.txt index 3d92c8f..56786ea 100644 --- a/test/system_test/MassSpring3D/CMakeLists.txt +++ b/test/system_test/MassSpring3D/CMakeLists.txt @@ -10,6 +10,7 @@ add_executable(${PROJECT_NAME} ClothSimulation.cpp Shader.cpp Renderer.cpp + dtkPhysMassSpringSolver.cpp ) target_include_directories(${PROJECT_NAME} PRIVATE diff --git a/test/system_test/MassSpring3D/ClothSimulation.cpp b/test/system_test/MassSpring3D/ClothSimulation.cpp index 4ff9afe..5711743 100644 --- a/test/system_test/MassSpring3D/ClothSimulation.cpp +++ b/test/system_test/MassSpring3D/ClothSimulation.cpp @@ -38,27 +38,15 @@ void ClothSimulation::Init() { }; void ClothSimulation::Update(float dt) { - // if (IsVisible() == false || IsPause() == true) return; + if (IsVisible() == false || IsPause() == true) return; + + _solver->solve(_iter_num); + _solver->solve(_iter_num); UpdateRenderTarget(); }; void ClothSimulation::Render() { - // glMatrixMode(GL_PROJECTION); - // glLoadMatrixf(glm::value_ptr(g_ProjectionMatrix)); - // glMatrixMode(GL_MODELVIEW); - // glLoadMatrixf(glm::value_ptr(g_ModelViewMatrix)); - // glColor3f(0.8f, 0.8f, 0.0f); - // const std::vector& points = _cloth_mesh->GetECTable(); - // for (int i = 0; i < points.size(); i++) { - // dtk::dtkID3 p = points[i]; - // glBegin(GL_TRIANGLES); - // glVertex3f(_cloth_mesh->GetPoint(p[0])[0], _cloth_mesh->GetPoint(p[0])[1], _cloth_mesh->GetPoint(p[0])[2]); - // glVertex3f(_cloth_mesh->GetPoint(p[1])[0], _cloth_mesh->GetPoint(p[1])[1], _cloth_mesh->GetPoint(p[1])[2]); - // glVertex3f(_cloth_mesh->GetPoint(p[2])[0], _cloth_mesh->GetPoint(p[2])[1], _cloth_mesh->GetPoint(p[2])[2]); - // glEnd(); - // } - if (!IsVisible()) return; Renderer renderer; renderer.setProgram(g_phongShader); @@ -94,11 +82,14 @@ void ClothSimulation::InitShader() { void ClothSimulation::InitCloth() { _cloth_mesh = dtkFactory::CreateClothMesh(SystemParam::w, SystemParam::n); - g_render_target = new ProgramInput; - - UpdateRenderTarget(); - ClothDrop(); + + g_render_target = new ProgramInput; + UpdateRenderTarget(); // set position data + const std::vector& mEc = _cloth_mesh->GetECTable(); + unsigned int* indexBuffer = (unsigned int*)&mEc[0]; + unsigned int indexBufferSize = mEc.size() * 3; + g_render_target->setIndexData(indexBuffer, indexBufferSize); } void ClothSimulation::InitScene() { @@ -115,26 +106,18 @@ void ClothSimulation::SetParameters() { }; void ClothSimulation::ClothDrop() { - _system = dtkFactory::CreateClothMassSpring(_cloth_mesh, _gravity); + _system = dtkFactory::CreateClothMassSpringSystem(_cloth_mesh); + + _solver = dtkFactory::CreateClothMassSpringSolver(_system); }; void ClothSimulation::UpdateRenderTarget() { dtk::dtkPoints::Ptr mPts = _cloth_mesh->GetPoints(); - const std::vector& mEc = _cloth_mesh->GetECTable(); - float* vertexBuffer = new float[mPts->GetNumberOfPoints() * 3]; - for (int i = 0; i < mPts->GetNumberOfPoints(); i++) { - dtk::GK::Point3 p = mPts->GetPoint(i); - vertexBuffer[i * 3] = p[0]; - vertexBuffer[i * 3 + 1] = p[1]; - vertexBuffer[i * 3 + 2] = p[2]; - } + float* vertexBuffer = _solver->getVertexBuffer(); unsigned int vertexBufferSize = mPts->GetNumberOfPoints() * 3; - unsigned int* indexBuffer = (unsigned int*)&mEc[0]; - unsigned int indexBufferSize = mEc.size() * 3; g_render_target->setPositionData(vertexBuffer, vertexBufferSize); - g_render_target->setIndexData(indexBuffer, indexBufferSize); }; dtk::dtkStaticTriangleMesh::Ptr dtkFactory::CreateClothMesh(float w, int n) { @@ -146,7 +129,6 @@ dtk::dtkStaticTriangleMesh::Ptr dtkFactory::CreateClothMesh(float w, int n) { const glm::vec3 o = glm::vec3(-w / 2.0f, w / 2.0f, 0.0f); // origin const glm::vec3 ux = glm::vec3(1.0f, 0.0f, 0.0f); // unit x direction const glm::vec3 uy = glm::vec3(0.0f, -1.0f, 0.0f); // unit y direction - std::vector handle_table(n * n); // table storing vertex handles for easy grid connectivity establishment dtk::dtkPointsVector::Ptr vertices = dtk::dtkPointsVector::New(); for (int i = 0; i < n; i++) { @@ -155,7 +137,6 @@ dtk::dtkStaticTriangleMesh::Ptr dtkFactory::CreateClothMesh(float w, int n) { dtk::GK::Point3 p3(p.x, p.y, p.z); dtk::dtkID pid = j + i * n; vertices->InsertPoint(pid, p3); // add vertex - // handle_table[pid] = vertices->GetPoint(pid); // store vertex handle } } result->SetPoints(vertices); @@ -183,9 +164,10 @@ dtk::dtkStaticTriangleMesh::Ptr dtkFactory::CreateClothMesh(float w, int n) { return result; } -dtk::dtkPhysMassSpring::Ptr dtkFactory::CreateClothMassSpring(const dtk::dtkStaticTriangleMesh::Ptr& mesh, const dtk::dtkDouble3& gravity) { - dtk::dtkPhysMassSpring::Ptr system = dtk::dtkPhysMassSpring::New(SystemParam::m, SystemParam::k, SystemParam::b, SystemParam::a, SystemParam::r, gravity); - system->SetTriangleMesh(mesh); +dtk::dtkPhysMassSpring::Ptr dtkFactory::CreateClothMassSpringSystem(const dtk::dtkStaticTriangleMesh::Ptr& mesh) { + dtk::dtkDouble3 gravity(0, 0, -SystemParam::g); + dtk::dtkPhysMassSpring::Ptr system = dtk::dtkPhysMassSpring::New(SystemParam::m, SystemParam::k, SystemParam::b, SystemParam::a, SystemParam::r, SystemParam::h, gravity); + // system->SetTriangleMesh(mesh); // n must be odd assert(SystemParam::n % 2 == 1); @@ -193,13 +175,13 @@ dtk::dtkPhysMassSpring::Ptr dtkFactory::CreateClothMassSpring(const dtk::dtkStat // compute n_points and n_springs unsigned int n_points = SystemParam::n * SystemParam::n; unsigned int n_springs = (SystemParam::n - 1) * (5 * SystemParam::n - 2); - + system->SetPoints(mesh->GetPoints()); for (unsigned int id = 0; id < n_points; id++) { system->AddMassPoint(id, SystemParam::m, dtk::dtkT3(0, 0, 0), SystemParam::a, SystemParam::c, gravity); } unsigned int n = SystemParam::n; - unsigned int k = 0; // spring counter + double rest_length_factor = 1.05; for (unsigned int i = 0; i < SystemParam::n; i++) { for (unsigned int j = 0; j < SystemParam::n; j++) { // bottom right corner @@ -209,11 +191,11 @@ dtk::dtkPhysMassSpring::Ptr dtkFactory::CreateClothMassSpring(const dtk::dtkStat if (i == n - 1) { // structural spring - system->AddSpring(n * i + j, n * i + j + 1, SystemParam::k, SystemParam::a); + system->AddSpring(n * i + j, n * i + j + 1, SystemParam::k, SystemParam::a, rest_length_factor); // bending spring if (j % 2 == 0) { - system->AddSpring(n * i + j, n * i + j + 2, SystemParam::k, SystemParam::a); + system->AddSpring(n * i + j, n * i + j + 2, SystemParam::k, SystemParam::a, rest_length_factor); } continue; } @@ -221,34 +203,41 @@ dtk::dtkPhysMassSpring::Ptr dtkFactory::CreateClothMassSpring(const dtk::dtkStat // right edge if (j == n - 1) { // structural spring - system->AddSpring(n * i + j, n * (i + 1) + j, SystemParam::k, SystemParam::a); + system->AddSpring(n * i + j, n * (i + 1) + j, SystemParam::k, SystemParam::a, rest_length_factor); // bending spring if (i % 2 == 0) { - system->AddSpring(n * i + j, n * (i + 2) + j, SystemParam::k, SystemParam::a); + system->AddSpring(n * i + j, n * (i + 2) + j, SystemParam::k, SystemParam::a, rest_length_factor); } continue; } // structural springs - system->AddSpring(n * i + j, n * i + j + 1, SystemParam::k, SystemParam::a); + system->AddSpring(n * i + j, n * i + j + 1, SystemParam::k, SystemParam::a, rest_length_factor); - system->AddSpring(n * i + j, n * (i + 1) + j, SystemParam::k, SystemParam::a); + system->AddSpring(n * i + j, n * (i + 1) + j, SystemParam::k, SystemParam::a, rest_length_factor); // shearing springs - system->AddSpring(n * i + j, n * (i + 1) + j + 1, SystemParam::k, SystemParam::a); + system->AddSpring(n * i + j, n * (i + 1) + j + 1, SystemParam::k, SystemParam::a, rest_length_factor); - system->AddSpring(n * (i + 1) + j, n * i + j + 1, SystemParam::k, SystemParam::a); + system->AddSpring(n * (i + 1) + j, n * i + j + 1, SystemParam::k, SystemParam::a, rest_length_factor); // bending springs if (j % 2 == 0) { - system->AddSpring(n * i + j, n * i + j + 2, SystemParam::k, SystemParam::a); + system->AddSpring(n * i + j, n * i + j + 2, SystemParam::k, SystemParam::a, rest_length_factor); } if (i % 2 == 0) { - system->AddSpring(n * i + j, n * (i + 2) + j, SystemParam::k, SystemParam::a); + system->AddSpring(n * i + j, n * (i + 2) + j, SystemParam::k, SystemParam::a, rest_length_factor); } } } + // TODO: setting gravity as an external force + return system; +} + +dtk::dtkPhysMassSpringSolver::Ptr dtkFactory::CreateClothMassSpringSolver(const dtk::dtkPhysMassSpring::Ptr& system) { + dtk::dtkPhysMassSpringSolver::Ptr solver = dtk::dtkPhysMassSpringSolver::New(system); + return solver; } \ No newline at end of file diff --git a/test/system_test/MassSpring3D/dtkPhysMassSpringSolver.cpp b/test/system_test/MassSpring3D/dtkPhysMassSpringSolver.cpp new file mode 100644 index 0000000..1e8e880 --- /dev/null +++ b/test/system_test/MassSpring3D/dtkPhysMassSpringSolver.cpp @@ -0,0 +1,117 @@ +#include "dtkPhysMassSpringSolver.h" + +dtk::dtkPhysMassSpringSolver::dtkPhysMassSpringSolver() { +} + +dtk::dtkPhysMassSpringSolver::dtkPhysMassSpringSolver(const dtk::dtkPhysMassSpring::Ptr& massSpringSystem) { + _system = massSpringSystem; + _time_step = _system->GetTimeStep(); + + // current state + _current_state.resize(3 * _system->GetNumberOfMassPoints()); + for (dtk::dtkID i = 0; i < _system->GetNumberOfMassPoints(); i++) { + dtk::dtkPhysMassPoint* massPoint = _system->GetMassPoint(i); + _current_state[3 * i] = massPoint->GetPosition()[0]; + _current_state[3 * i + 1] = massPoint->GetPosition()[1]; + _current_state[3 * i + 2] = massPoint->GetPosition()[2]; + } + _prev_state = _current_state; + _spring_directions.resize(3 * _system->GetNumberOfSprings()); + + // M + TripletList MTriplets; + _M.resize(3 * _system->GetNumberOfMassPoints(), 3 * _system->GetNumberOfMassPoints()); + for (dtk::dtkID i = 0; i < _system->GetNumberOfMassPoints(); i++) { + for (dtk::dtkID j = 0; j < 3; j++) { + MTriplets.push_back(Triplet(3 * i + j, 3 * i + j, _system->GetMassPoint(i)->GetMass())); + } + } + _M.setFromTriplets(MTriplets.begin(), MTriplets.end()); + + // L + TripletList LTriplets; + _L.resize(3 * _system->GetNumberOfMassPoints(), 3 * _system->GetNumberOfMassPoints()); + for (dtk::dtkID i = 0;i < _system->GetNumberOfSprings();i++) { + double stiffness = _system->GetSpring(i)->GetStiffness(); + dtk::dtkPhysSpring* spring = _system->GetSpring(i); + dtk::dtkID id1 = spring->GetFirstVertex()->GetPointID();// TODO GetFirstVertex修改成GetFirstPoint(); + dtk::dtkID id2 = spring->GetSecondVertex()->GetPointID(); + for (dtk::dtkID j = 0; j < 3; j++) { + LTriplets.push_back(Triplet(3 * id1 + j, 3 * id1 + j, 1 * stiffness)); + LTriplets.push_back(Triplet(3 * id1 + j, 3 * id2 + j, -1 * stiffness)); + LTriplets.push_back(Triplet(3 * id2 + j, 3 * id1 + j, -1 * stiffness)); + LTriplets.push_back(Triplet(3 * id2 + j, 3 * id2 + j, 1 * stiffness)); + } + } + _L.setFromTriplets(LTriplets.begin(), LTriplets.end()); + + // J + TripletList JTriplets; + _J.resize(3 * _system->GetNumberOfMassPoints(), 3 * _system->GetNumberOfSprings()); + for (dtk::dtkID i = 0;i < _system->GetNumberOfSprings();i++) { + double stiffness = _system->GetSpring(i)->GetStiffness(); + dtk::dtkPhysSpring* spring = _system->GetSpring(i); + dtk::dtkID id1 = spring->GetFirstVertex()->GetPointID(); + dtk::dtkID id2 = spring->GetSecondVertex()->GetPointID(); + for (unsigned int j = 0; j < 3; j++) { + JTriplets.push_back( + Triplet(3 * id1 + j, 3 * i + j, 1 * stiffness)); + JTriplets.push_back( + Triplet(3 * id2 + j, 3 * i + j, -1 * stiffness)); + } + } + _J.setFromTriplets(JTriplets.begin(), JTriplets.end()); + + // pre-factor + double h2 = _system->GetTimeStep() * _system->GetTimeStep(); + SparseMatrix A = _M + h2 * _L; + _system_matrix.compute(A); +} + +void dtk::dtkPhysMassSpringSolver::solve(unsigned int iter_num) { + float damping_factor = _system->GetDefaultDamp(); + + // update inertial term + _inertial_term = _M * ((damping_factor + 1) * (_current_state)-damping_factor * _prev_state); + _prev_state = _current_state; + + // perform steps + for (unsigned int i = 0; i < iter_num; i++) { + localStep(); + globalStep(); + } +} + +void dtk::dtkPhysMassSpringSolver::localStep() { + for (dtk::dtkID id = 0;id < _system->GetNumberOfSprings();id++) { + dtk::dtkPhysSpring* spring = _system->GetSpring(id); + dtk::dtkID id1 = spring->GetFirstVertex()->GetPointID(); + dtk::dtkID id2 = spring->GetSecondVertex()->GetPointID(); + double rest_length = spring->GetRestLength(); + Vector3f p12( + _current_state[3 * id1 + 0] - _current_state[3 * id2 + 0], + _current_state[3 * id1 + 1] - _current_state[3 * id2 + 1], + _current_state[3 * id1 + 2] - _current_state[3 * id2 + 2] + ); + + p12.normalize(); + _spring_directions[3 * id + 0] = rest_length * p12[0]; + _spring_directions[3 * id + 1] = rest_length * p12[1]; + _spring_directions[3 * id + 2] = rest_length * p12[2]; + } +} + +void dtk::dtkPhysMassSpringSolver::globalStep() { + float h2 = _system->GetTimeStep() * _system->GetTimeStep(); // shorthand + + // compute right hand side + // dtk::dtkDouble3 fext(0, 0, 0); + dtk::dtkDouble3 fext = _system->GetDefaultGravityAccel(); // TODO: change to _system->GetImpulseForce() + VectorXf fext_force = VectorXf(Vector3f(fext.x, fext.y, fext.z).replicate(_system->GetNumberOfMassPoints(), 1)); + VectorXf b = _inertial_term + h2 * _J * _spring_directions + h2 * fext_force; + + // std::cout << _spring_directions << std::endl; + + // solve system and update state + _current_state = _system_matrix.solve(b); +} \ No newline at end of file diff --git a/test/system_test/MassSpring3D/include/ClothSimulation.h b/test/system_test/MassSpring3D/include/ClothSimulation.h index 51c98a8..a0677c8 100644 --- a/test/system_test/MassSpring3D/include/ClothSimulation.h +++ b/test/system_test/MassSpring3D/include/ClothSimulation.h @@ -32,10 +32,11 @@ #include "Scene.h" #include "Shader.h" #include "Renderer.h" +#include "dtkPhysMassSpringSolver.h" namespace SystemParam { static const int n = 33; // must be odd, n * n = n_vertices | 61 - static const float w = 5.0f; // width | 2.0f + static const float w = 2.0f; // width | 2.0f static const float h = 0.008f; // time step, smaller for better results | 0.008f = 0.016f/2 static const float r = w / (n - 1) * 1.05f; // spring rest legnth static const float k = 1.0f; // spring stiffness | 1.0f; @@ -53,6 +54,7 @@ class ClothSimulation : public Scene { using ClothMesh = dtk::dtkStaticTriangleMesh::Ptr; using ClothMassSpring = dtk::dtkPhysMassSpring::Ptr; + using ClothMassSpringSolver = dtk::dtkPhysMassSpringSolver::Ptr; const ClothMesh GetClothMesh() const; @@ -94,12 +96,16 @@ class ClothSimulation : public Scene { ClothMesh _cloth_mesh; ClothMassSpring _system; + ClothMassSpringSolver _solver; + + const int _iter_num = 5; }; class dtkFactory { public: static dtk::dtkStaticTriangleMesh::Ptr CreateClothMesh(float w, int n); - static dtk::dtkPhysMassSpring::Ptr CreateClothMassSpring(const dtk::dtkStaticTriangleMesh::Ptr& mesh, const dtk::dtkDouble3& gravity); + static dtk::dtkPhysMassSpring::Ptr CreateClothMassSpringSystem(const dtk::dtkStaticTriangleMesh::Ptr& mesh); + static dtk::dtkPhysMassSpringSolver::Ptr CreateClothMassSpringSolver(const dtk::dtkPhysMassSpring::Ptr& system); }; #endif /* SIMPLEPHYSICSENGINE_CLOTHSIMULATION_H */ diff --git a/test/system_test/MassSpring3D/include/dtkPhysMassSpringSolver.h b/test/system_test/MassSpring3D/include/dtkPhysMassSpringSolver.h new file mode 100644 index 0000000..9510455 --- /dev/null +++ b/test/system_test/MassSpring3D/include/dtkPhysMassSpringSolver.h @@ -0,0 +1,56 @@ +#ifndef SIMPLEPHYSICSENGINE_DTKPHYSMASSSPRINGSOLVER_H +#define SIMPLEPHYSICSENGINE_DTKPHYSMASSSPRINGSOLVER_H +#include "dtkPhysMassPoint.h" +#include "dtkPhysMassSpring.h" + +#include +#include + + +namespace dtk { + class dtkPhysMassSpringSolver { + private: + typedef Eigen::Vector3f Vector3f; + typedef Eigen::VectorXf VectorXf; + typedef Eigen::SparseMatrix SparseMatrix; + typedef Eigen::SimplicialLLT > Cholesky; + typedef Eigen::Map Map; + typedef std::pair Edge; + typedef Eigen::Triplet Triplet; + typedef std::vector TripletList; + public: + typedef std::shared_ptr Ptr; + + static dtkPhysMassSpringSolver::Ptr New() { + return Ptr(new dtkPhysMassSpringSolver()); + } + static dtkPhysMassSpringSolver::Ptr New(const dtkPhysMassSpring::Ptr& massSpring) { + return Ptr(new dtkPhysMassSpringSolver(massSpring)); + } + + void solve(unsigned int iter_num); + float* getVertexBuffer() { return _current_state.data(); }; + private: + dtkPhysMassSpringSolver(); + dtkPhysMassSpringSolver(const dtkPhysMassSpring::Ptr& massSpring); + + void localStep(); + void globalStep(); + + Cholesky _system_matrix; + dtkPhysMassSpring::Ptr _system; + + // M, L, J matrices + SparseMatrix _M;/**< 质量稀疏矩阵 */ + SparseMatrix _L;/**< Laplace稀疏矩阵，表示弹簧质点系统中质点与质点之间的连接关系，以及连接弹簧的刚度 */ + SparseMatrix _J;/**< Jacobian稀疏矩阵，表示弹簧质点系统中每个质点与弹簧的连接关系，以及连接弹簧的刚度 */ + + VectorXf _current_state; // q(n) + VectorXf _prev_state; // q(n-1) + VectorXf _spring_directions; // d, spring directions + VectorXf _inertial_term; /**< 惯性项 = M * y, y = (a + 1) * q(n) - a * q(n - 1), a = damp_factor */ + + float _time_step; + }; +} +#endif // SIMPLEPHYSICSENGINE_DTKPHYSMASSSPRINGSOLVER_H \ No newline at end of file diff --git a/test/system_test/MassSpring3D/main.cpp b/test/system_test/MassSpring3D/main.cpp index 9b39c92..aba6684 100644 --- a/test/system_test/MassSpring3D/main.cpp +++ b/test/system_test/MassSpring3D/main.cpp @@ -74,9 +74,7 @@ void display() { glTranslatef(0.0f, -8.0f, -25.0f); auto now = std::chrono::high_resolution_clock::now(); - auto dt = std::chrono::duration_cast>( - now - last_clock) - .count(); + auto dt = std::chrono::duration_cast>(now - last_clock).count(); last_clock = now; int h = glutGet(GLUT_WINDOW_HEIGHT); @@ -92,7 +90,7 @@ void display() { else draw_text(5, h - 20, "dt: %.2f ms", dt * 1000); - scene.Update(std::min(dt, 0.01)); + scene.Update(std::min(dt, 0.08)); scene.Render(); GLenum err;