static void dQueryCCTLPotentialCollisionTriangles(OBBCollider &Collider,
const sTrimeshCapsuleColliderData &cData, dxTriMesh *TriMesh, dxGeom *Capsule,
OBBCache &BoxCache)
{
// It is a potential issue to explicitly cast to float
// if custom width floating point type is introduced in OPCODE.
// It is necessary to make a typedef and cast to it
// (e.g. typedef float opc_float;)
// However I'm not sure in what header it should be added.
const dVector3 &vCapsulePosition = cData.m_vCapsulePosition;
Point cCenter(/*(float)*/ vCapsulePosition[0], /*(float)*/ vCapsulePosition[1], /*(float)*/ vCapsulePosition[2]);
Point cExtents(/*(float)*/ cData.m_vCapsuleRadius, /*(float)*/ cData.m_vCapsuleRadius,/*(float)*/ cData.m_fCapsuleSize/2);
Matrix3x3 obbRot;
const dMatrix3 &mCapsuleRotation = cData.m_mCapsuleRotation;
obbRot[0][0] = /*(float)*/ mCapsuleRotation[0];
obbRot[1][0] = /*(float)*/ mCapsuleRotation[1];
obbRot[2][0] = /*(float)*/ mCapsuleRotation[2];
obbRot[0][1] = /*(float)*/ mCapsuleRotation[4];
obbRot[1][1] = /*(float)*/ mCapsuleRotation[5];
obbRot[2][1] = /*(float)*/ mCapsuleRotation[6];
obbRot[0][2] = /*(float)*/ mCapsuleRotation[8];
obbRot[1][2] = /*(float)*/ mCapsuleRotation[9];
obbRot[2][2] = /*(float)*/ mCapsuleRotation[10];
OBB obbCapsule(cCenter,cExtents,obbRot);
Matrix4x4 CapsuleMatrix;
MakeMatrix(vCapsulePosition, mCapsuleRotation, CapsuleMatrix);
Matrix4x4 MeshMatrix;
MakeMatrix(cData.m_mTriMeshPos, cData.m_mTriMeshRot, MeshMatrix);
// TC results
if (TriMesh->doBoxTC) {
dxTriMesh::BoxTC* BoxTC = 0;
for (int i = 0; i < TriMesh->BoxTCCache.size(); i++){
if (TriMesh->BoxTCCache[i].Geom == Capsule){
BoxTC = &TriMesh->BoxTCCache[i];
break;
}
}
if (!BoxTC){
TriMesh->BoxTCCache.push(dxTriMesh::BoxTC());
BoxTC = &TriMesh->BoxTCCache[TriMesh->BoxTCCache.size() - 1];
BoxTC->Geom = Capsule;
BoxTC->FatCoeff = 1.0f;
}
// Intersect
Collider.SetTemporalCoherence(true);
Collider.Collide(*BoxTC, obbCapsule, TriMesh->Data->BVTree, null, &MeshMatrix);
}
else {
Collider.SetTemporalCoherence(false);
Collider.Collide(BoxCache, obbCapsule, TriMesh->Data->BVTree, null,&MeshMatrix);
}
}
int dCollideSTL(dxGeom* g1, dxGeom* SphereGeom, int Flags, dContactGeom* Contacts, int Stride){
dIASSERT (Stride >= (int)sizeof(dContactGeom));
dIASSERT (g1->type == dTriMeshClass);
dIASSERT (SphereGeom->type == dSphereClass);
dIASSERT ((Flags & NUMC_MASK) >= 1);
dxTriMesh* TriMesh = (dxTriMesh*)g1;
// Init
const dVector3& TLPosition = *(const dVector3*)dGeomGetPosition(TriMesh);
const dMatrix3& TLRotation = *(const dMatrix3*)dGeomGetRotation(TriMesh);
const unsigned uiTLSKind = TriMesh->getParentSpaceTLSKind();
dIASSERT(uiTLSKind == SphereGeom->getParentSpaceTLSKind()); // The colliding spaces must use matching cleanup method
TrimeshCollidersCache *pccColliderCache = GetTrimeshCollidersCache(uiTLSKind);
SphereCollider& Collider = pccColliderCache->_SphereCollider;
const dVector3& Position = *(const dVector3*)dGeomGetPosition(SphereGeom);
dReal Radius = dGeomSphereGetRadius(SphereGeom);
// Sphere
Sphere Sphere;
dCopyVector3(Sphere.mCenter, Position);
Sphere.mRadius = Radius;
Matrix4x4 amatrix;
// TC results
if (TriMesh->doSphereTC) {
dxTriMesh::SphereTC* sphereTC = 0;
for (int i = 0; i < TriMesh->SphereTCCache.size(); i++){
if (TriMesh->SphereTCCache[i].Geom == SphereGeom){
sphereTC = &TriMesh->SphereTCCache[i];
break;
}
}
if (!sphereTC){
TriMesh->SphereTCCache.push(dxTriMesh::SphereTC());
sphereTC = &TriMesh->SphereTCCache[TriMesh->SphereTCCache.size() - 1];
sphereTC->Geom = SphereGeom;
}
// Intersect
Collider.SetTemporalCoherence(true);
Collider.Collide(*sphereTC, Sphere, TriMesh->Data->BVTree, null,
&MakeMatrix(TLPosition, TLRotation, amatrix));
}
else {
Collider.SetTemporalCoherence(false);
Collider.Collide(pccColliderCache->defaultSphereCache, Sphere, TriMesh->Data->BVTree, null,
&MakeMatrix(TLPosition, TLRotation, amatrix));
}
if (! Collider.GetContactStatus()) {
// no collision occurred
return 0;
}
// get results
int TriCount = Collider.GetNbTouchedPrimitives();
const int* Triangles = (const int*)Collider.GetTouchedPrimitives();
if (TriCount != 0){
if (TriMesh->ArrayCallback != null){
TriMesh->ArrayCallback(TriMesh, SphereGeom, Triangles, TriCount);
}
int OutTriCount = 0;
for (int i = 0; i < TriCount; i++){
if (OutTriCount == (Flags & NUMC_MASK)){
break;
}
const int TriIndex = Triangles[i];
dVector3 dv[3];
if (!Callback(TriMesh, SphereGeom, TriIndex))
continue;
FetchTriangle(TriMesh, TriIndex, TLPosition, TLRotation, dv);
dVector3& v0 = dv[0];
dVector3& v1 = dv[1];
dVector3& v2 = dv[2];
dVector3 vu;
dSubtractVectors3r4(vu, v1, v0);
vu[3] = REAL(0.0);
dVector3 vv;
dSubtractVectors3r4(vv, v2, v0);
vv[3] = REAL(0.0);
// Get plane coefficients
dVector4 Plane;
dCalcVectorCross3(Plane, vu, vv);
// Even though all triangles might be initially valid,
// a triangle may degenerate into a segment after applying
// space transformation.
if (!dSafeNormalize3(Plane)) {
continue;
}
/* If the center of the sphere is within the positive halfspace of the
* triangle's plane, allow a contact to be generated.
* If the center of the sphere made it into the positive halfspace of a
* back-facing triangle, then the physics update and/or velocity needs
* to be adjusted (penetration has occured anyway).
*/
dReal side = dCalcVectorDot3(Plane, Position) - dCalcVectorDot3(Plane, v0);
if(side < REAL(0.0))
{
continue;
}
dReal Depth;
dReal u, v;
if (!GetContactData(Position, Radius, v0, vu, vv, Depth, u, v)){
continue; // Sphere doesn't hit triangle
}
if (Depth < REAL(0.0)){
continue; // Negative depth does not produce a contact
}
dVector3 ContactPos;
dReal w = REAL(1.0) - u - v;
dAddScaledVectors3r4(ContactPos, v1, v2, u, v);
dAddScaledVector3r4(ContactPos, v0, w);
// Depth returned from GetContactData is depth along
// contact point - sphere center direction
// we'll project it to contact normal
dVector3 dir;
dSubtractVectors3r4(dir, Position, ContactPos);
dReal dirProj = dCalcVectorDot3(dir, Plane) / dCalcVectorLength3(dir);
// Since Depth already had a requirement to be non-negative,
// negative direction projections should not be allowed as well,
// as otherwise the multiplication will result in negative contact depth.
if (dirProj < REAL(0.0))
continue; // Zero contact depth could be ignored
dContactGeom* Contact = SAFECONTACT(Flags, Contacts, OutTriCount, Stride);
dCopyVector3r4(Contact->pos, ContactPos);
// Using normal as plane (reversed)
dCopyNegatedVector3r4(Contact->normal, Plane);
Contact->depth = Depth * dirProj;
//Contact->depth = Radius - side; // (mg) penetration depth is distance along normal not shortest distance
// We need to set these unconditionally, as the merging may fail! - Bram
Contact->g1 = TriMesh;
Contact->g2 = SphereGeom;
Contact->side2 = -1;
Contact->side1 = TriIndex;
OutTriCount++;
}
if (OutTriCount > 0)
{
if (TriMesh->SphereContactsMergeOption == MERGE_CONTACTS_FULLY)
{
dContactGeom* Contact = SAFECONTACT(Flags, Contacts, 0, Stride);
Contact->g1 = TriMesh;
Contact->g2 = SphereGeom;
Contact->side2 = -1;
if (OutTriCount > 1 && !(Flags & CONTACTS_UNIMPORTANT))
{
dVector3 pos;
dCopyVector3r4(pos, Contact->pos);
dVector3 normal;
dCopyScaledVector3r4(normal, Contact->normal, Contact->depth);
int TriIndex = Contact->side1;
for (int i = 1; i < OutTriCount; i++)
{
dContactGeom* TempContact = SAFECONTACT(Flags, Contacts, i, Stride);
dAddVector3r4(pos, TempContact->pos);
dAddScaledVector3r4(normal, TempContact->normal, TempContact->depth);
TriIndex = (TriMesh->TriMergeCallback) ? TriMesh->TriMergeCallback(TriMesh, TriIndex, TempContact->side1) : -1;
}
Contact->side1 = TriIndex;
dReal invOutTriCount = dRecip(OutTriCount);
dCopyScaledVector3r4(Contact->pos, pos, invOutTriCount);
if ( !dSafeNormalize3(normal) )
return OutTriCount; // Cannot merge in this pathological case
// Using a merged normal, means that for each intersection, this new normal will be less effective in solving the intersection.
// That is why we need to correct this by increasing the depth for each intersection.
// The maximum of the adjusted depths is our newly merged depth value - Bram.
dReal mergedDepth = REAL(0.0);
dReal minEffectiveness = REAL(0.5);
for ( int i = 0; i < OutTriCount; ++i )
{
dContactGeom* TempContact = SAFECONTACT(Flags, Contacts, i, Stride);
dReal effectiveness = dCalcVectorDot3(normal, TempContact->normal);
if ( effectiveness < dEpsilon )
return OutTriCount; // Cannot merge this pathological case
// Cap our adjustment for the new normal to a factor 2, meaning a 60 deg change in normal.
effectiveness = ( effectiveness < minEffectiveness ) ? minEffectiveness : effectiveness;
dReal adjusted = TempContact->depth / effectiveness;
mergedDepth = ( mergedDepth < adjusted ) ? adjusted : mergedDepth;
}
Contact->depth = mergedDepth;
dCopyVector3r4(Contact->normal, normal);
}
return 1;
}
else if (TriMesh->SphereContactsMergeOption == MERGE_CONTACT_NORMALS)
{
if (OutTriCount != 1 && !(Flags & CONTACTS_UNIMPORTANT))
{
dVector3 Normal;
dContactGeom* FirstContact = SAFECONTACT(Flags, Contacts, 0, Stride);
dCopyScaledVector3r4(Normal, FirstContact->normal, FirstContact->depth);
for (int i = 1; i < OutTriCount; i++)
{
dContactGeom* Contact = SAFECONTACT(Flags, Contacts, i, Stride);
dAddScaledVector3r4(Normal, Contact->normal, Contact->depth);
}
dNormalize3(Normal);
for (int i = 0; i < OutTriCount; i++)
{
dContactGeom* Contact = SAFECONTACT(Flags, Contacts, i, Stride);
dCopyVector3r4(Contact->normal, Normal);
}
}
return OutTriCount;
}
else
{
dIASSERT(TriMesh->SphereContactsMergeOption == DONT_MERGE_CONTACTS);
return OutTriCount;
}
}
else return 0;
}
else return 0;
}
void Interpolate(int n) {
FreeMem();
GetMem(n);
GetMaxMin();
GetNodesAndValues(n);
MakeMatrix(n);
double *ss = new double[n - 2];
SolveSystem(n - 2, aa, bb, ss);
for (int i = 2; i < n; i++)
D(i) = ss[i - 2 - 1];
delete ss;
EM(1, 1) = X(1) * X(1) * X(1);
EM(1, 2) = X(1) * X(1);
EM(1, 3) = X(1);
EM(1, 4) = 1.0;
EM(2, 1) = X(2) * X(2) * X(2);
EM(2, 2) = X(2) * X(2);
EM(2, 3) = X(2);
EM(2, 4) = 1.0;
EM(3, 1) = X(3) * X(3) * X(3);
EM(3, 2) = X(3) * X(3);
EM(3, 3) = X(3);
EM(3, 4) = 1.0;
EM(4, 1) = 3.0 * X(3) * X(3);
EM(4, 2) = 2.0 * X(3);
EM(4, 3) = X(3);
EM(4, 4) = 0.0;
eb[0] = F(1);
eb[1] = F(2);
eb[2] = F(3);
eb[3] = D(3);
SolveSystem(4, em, eb, exs);
EM(1, 1) = X(n - 2) * X(n - 2) * X(n - 2);
EM(1, 2) = X(n - 2) * X(n - 2);
EM(1, 3) = X(n - 2);
EM(1, 4) = 1.0;
EM(2, 1) = X(n - 1) * X(n - 1) * X(n - 1);
EM(2, 2) = X(n - 1) * X(n - 1);
EM(2, 3) = X(n - 1);
EM(2, 4) = 1.0;
EM(3, 1) = X(n) * X(n) * X(n);
EM(3, 2) = X(n) * X(n);
EM(3, 3) = X(n);
EM(3, 4) = 1.0;
EM(4, 1) = 3.0 * X(n - 2) * X(n - 2);
EM(4, 2) = 2.0 * X(n - 2);
EM(4, 3) = X(n - 2);
EM(4, 4) = 0.0;
eb[0] = F(n - 2);
eb[1] = F(n - 1);
eb[2] = F(n);
eb[3] = D(n - 2);
SolveSystem(4, em, eb, exe);
}
int dCollideRTL(dxGeom* g1, dxGeom* RayGeom, int Flags, dContactGeom* Contacts, int Stride){
dIASSERT (Stride >= (int)sizeof(dContactGeom));
dIASSERT (g1->type == dTriMeshClass);
dIASSERT (RayGeom->type == dRayClass);
dIASSERT ((Flags & NUMC_MASK) >= 1);
dxTriMesh* TriMesh = (dxTriMesh*)g1;
const dVector3& TLPosition = *(const dVector3*)dGeomGetPosition(TriMesh);
const dMatrix3& TLRotation = *(const dMatrix3*)dGeomGetRotation(TriMesh);
const unsigned uiTLSKind = TriMesh->getParentSpaceTLSKind();
dIASSERT(uiTLSKind == RayGeom->getParentSpaceTLSKind()); // The colliding spaces must use matching cleanup method
TrimeshCollidersCache *pccColliderCache = GetTrimeshCollidersCache(uiTLSKind);
RayCollider& Collider = pccColliderCache->_RayCollider;
dReal Length = dGeomRayGetLength(RayGeom);
int FirstContact, BackfaceCull;
dGeomRayGetParams(RayGeom, &FirstContact, &BackfaceCull);
int ClosestHit = dGeomRayGetClosestHit(RayGeom);
Collider.SetFirstContact(FirstContact != 0);
Collider.SetClosestHit(ClosestHit != 0);
Collider.SetCulling(BackfaceCull != 0);
Collider.SetMaxDist(Length);
dVector3 Origin, Direction;
dGeomRayGet(RayGeom, Origin, Direction);
/* Make Ray */
Ray WorldRay;
WorldRay.mOrig.x = Origin[0];
WorldRay.mOrig.y = Origin[1];
WorldRay.mOrig.z = Origin[2];
WorldRay.mDir.x = Direction[0];
WorldRay.mDir.y = Direction[1];
WorldRay.mDir.z = Direction[2];
/* Intersect */
Matrix4x4 amatrix;
int TriCount = 0;
if (Collider.Collide(WorldRay, TriMesh->Data->BVTree, &MakeMatrix(TLPosition, TLRotation, amatrix))) {
TriCount = pccColliderCache->Faces.GetNbFaces();
}
if (TriCount == 0) {
return 0;
}
const CollisionFace* Faces = pccColliderCache->Faces.GetFaces();
int OutTriCount = 0;
for (int i = 0; i < TriCount; i++) {
if (TriMesh->RayCallback == null ||
TriMesh->RayCallback(TriMesh, RayGeom, Faces[i].mFaceID,
Faces[i].mU, Faces[i].mV)) {
const int& TriIndex = Faces[i].mFaceID;
if (!Callback(TriMesh, RayGeom, TriIndex)) {
continue;
}
dContactGeom* Contact = SAFECONTACT(Flags, Contacts, OutTriCount, Stride);
dVector3 dv[3];
FetchTriangle(TriMesh, TriIndex, TLPosition, TLRotation, dv);
dVector3 vu;
vu[0] = dv[1][0] - dv[0][0];
vu[1] = dv[1][1] - dv[0][1];
vu[2] = dv[1][2] - dv[0][2];
vu[3] = REAL(0.0);
dVector3 vv;
vv[0] = dv[2][0] - dv[0][0];
vv[1] = dv[2][1] - dv[0][1];
vv[2] = dv[2][2] - dv[0][2];
vv[3] = REAL(0.0);
dCROSS(Contact->normal, =, vv, vu); // Reversed
// Even though all triangles might be initially valid,
// a triangle may degenerate into a segment after applying
// space transformation.
if (dSafeNormalize3(Contact->normal))
{
// No sense to save on single type conversion in algorithm of this size.
// If there would be a custom typedef for distance type it could be used
// instead of dReal. However using float directly is the loss of abstraction
// and possible loss of precision in future.
/*float*/ dReal T = Faces[i].mDistance;
Contact->pos[0] = Origin[0] + (Direction[0] * T);
Contact->pos[1] = Origin[1] + (Direction[1] * T);
Contact->pos[2] = Origin[2] + (Direction[2] * T);
Contact->pos[3] = REAL(0.0);
Contact->depth = T;
Contact->g1 = TriMesh;
Contact->g2 = RayGeom;
Contact->side1 = TriIndex;
Contact->side2 = -1;
OutTriCount++;
// Putting "break" at the end of loop prevents unnecessary checks on first pass and "continue"
if (OutTriCount >= (Flags & NUMC_MASK)) {
break;
}
}
}
}
return OutTriCount;
}
int dCollideRTL(dxGeom* g1, dxGeom* RayGeom, int Flags, dContactGeom* Contacts, int Stride){
dxTriMesh* TriMesh = (dxTriMesh*)g1;
const dVector3& TLPosition = *(const dVector3*)dGeomGetPosition(TriMesh);
const dMatrix3& TLRotation = *(const dMatrix3*)dGeomGetRotation(TriMesh);
RayCollider& Collider = TriMesh->_RayCollider;
dReal Length = dGeomRayGetLength(RayGeom);
int FirstContact, BackfaceCull;
dGeomRayGetParams(RayGeom, &FirstContact, &BackfaceCull);
int ClosestHit = dGeomRayGetClosestHit(RayGeom);
Collider.SetFirstContact(FirstContact != 0);
Collider.SetClosestHit(ClosestHit != 0);
Collider.SetCulling(BackfaceCull != 0);
Collider.SetMaxDist(Length);
dVector3 Origin, Direction;
dGeomRayGet(RayGeom, Origin, Direction);
/* Make Ray */
Ray WorldRay;
WorldRay.mOrig.x = Origin[0];
WorldRay.mOrig.y = Origin[1];
WorldRay.mOrig.z = Origin[2];
WorldRay.mDir.x = Direction[0];
WorldRay.mDir.y = Direction[1];
WorldRay.mDir.z = Direction[2];
/* Intersect */
Matrix4x4 amatrix;
int TriCount = 0;
if (Collider.Collide(WorldRay, TriMesh->Data->BVTree, &MakeMatrix(TLPosition, TLRotation, amatrix))) {
TriCount = TriMesh->Faces.GetNbFaces();
}
if (TriCount == 0) {
return 0;
}
const CollisionFace* Faces = TriMesh->Faces.GetFaces();
int OutTriCount = 0;
for (int i = 0; i < TriCount; i++) {
if (OutTriCount == (Flags & 0xffff)) {
break;
}
if (TriMesh->RayCallback == null ||
TriMesh->RayCallback(TriMesh, RayGeom, Faces[i].mFaceID,
Faces[i].mU, Faces[i].mV)) {
const int& TriIndex = Faces[i].mFaceID;
if (!Callback(TriMesh, RayGeom, TriIndex)) {
continue;
}
dContactGeom* Contact = SAFECONTACT(Flags, Contacts, OutTriCount, Stride);
dVector3 dv[3];
FetchTriangle(TriMesh, TriIndex, TLPosition, TLRotation, dv);
float T = Faces[i].mDistance;
Contact->pos[0] = Origin[0] + (Direction[0] * T);
Contact->pos[1] = Origin[1] + (Direction[1] * T);
Contact->pos[2] = Origin[2] + (Direction[2] * T);
Contact->pos[3] = REAL(0.0);
dVector3 vu;
vu[0] = dv[1][0] - dv[0][0];
vu[1] = dv[1][1] - dv[0][1];
vu[2] = dv[1][2] - dv[0][2];
vu[3] = REAL(0.0);
dVector3 vv;
vv[0] = dv[2][0] - dv[0][0];
vv[1] = dv[2][1] - dv[0][1];
vv[2] = dv[2][2] - dv[0][2];
vv[3] = REAL(0.0);
dCROSS(Contact->normal, =, vv, vu); // Reversed
dNormalize3(Contact->normal);
Contact->depth = T;
Contact->g1 = TriMesh;
Contact->g2 = RayGeom;
OutTriCount++;
}
}
return OutTriCount;
}
int main(void)
{
FILE *fp,*fp2;//,*fp3,*fp4;
int k,m,n;//,p;
double b;
double** Ds;
double** De;
double** Den;
double** Dsn;
int i,j,t=1;
int *B=(int*)malloc(sizeof(int)*N);
double** W;
double** C;
int** H;
int** HT;
double* Cni=(double*)malloc(sizeof(double)*P*P);
char** str; //note
double** Dv;
char** hyperh;//line
//char** node;//all user
double** X;
//char** rank; //relationship 100 biger pehaps
//double** G; //follow relationship
//int *z=(int*)malloc(sizeof(int)*N);
//int *V=(int*)malloc(sizeof(int)*N);
//double *q=(double*)malloc(sizeof(double)*U);//first 1/n
//double *e=(double*)malloc(sizeof(double)*U);//value
if ((B==NULL)||Cni == NULL){
printf("cni error\n");
return 1;
}
printf("txt context:\n"); //print text
str=(char**)malloc(P*sizeof(char *));
for (i=0;i<P;i++){
str[i] = malloc(50* sizeof(char));
}
if (!str){
printf("mem not enough.\n");
return 1;
}
hyperh=(char**)malloc(N*sizeof(char *));
for (i=0;i<N;i++){
hyperh[i] = malloc(100* sizeof(char));
}
if (!hyperh){
printf("mem not enough.\n");
return 1;
}
printf("make str and hyperh\n");
MakeMatrix(str, P, 50);
MakeMatrix(hyperh, N, 100);
fp=fopen("./node1.txt","r"); //open file
fp2=fopen("./line.txt","r+"); //open file
if(fp==NULL)
{
printf("file open error!\n");
exit(0);
}
if(fp2==NULL)
{
printf("file open error!\n");
exit(0);
}
for(i=0;i<P;i++)
{
fgets(str[i],50,fp);
//fgets(*(str+i*100),100,fp); //put it in str[]
}
for(i=0;i<P;i++)
{
for(j=0;j<50;j++)//last word is enter
{
if(str[i][j]=='\n')str[i][j]='\0';//enter delete
if(str[i][j]=='\r')str[i][j]='\0';
}
}
fclose(fp);
for(j=0;j<N;j++)
{
fgets(hyperh[j],100,fp2); //get txt massage
}
fclose(fp2);
H=(int**)malloc(P*sizeof(int *));
for (i=0;i<P;i++){
str[i] = malloc(N* sizeof(int));
}
if (!H){
printf("mem not enough\n");
return 1;
}
MakeMatrixint(H, P, N);
double *a=(double*)malloc(sizeof(double)*P);
if (!a){
printf("mem not enough\n");
return 1;
}
for(i=0;i<P;i++)
a[i]=0;
for(m=0;m<P;m++)//note
{
for(n=0,b=0;n<N;n++)//line
for(i=0;hyperh[n][i]!='\0';i++)//line
{
for(k=i,j=0;str[m][j]!='\0';)//line do not over
if(hyperh[n][k]==str[m][j])//one word pipei
k++,j++;
else
break;
if(str[m][j] == '\0' && j > 0)//note get
{
b++;
//H[m][n];
a[m]=b;
}
}
}
printf("point 1 reached.\n");
Dv=(double**)malloc(P*sizeof(double *));
for (i=0;i<P;i++){
Dv[i] = malloc(P * sizeof(double));
if (!Dv[i]){
printf("mem error dv\n");
return 1;
}
}
if (!Dv){
printf("mem error");
return 1;
}
MakeMatrixdouble(Dv, P, P);
for(i=0;i<P;i++)
{
for(j=0;j<P;j++)
{
Dv[i][j]=0;
}
}
for(i=0;i<P;i++)
{
for(j=0;j<P;j++)
{
if(i==j)
Dv[i][j]=a[i];
}
}
printf("%f\n ",Dv[1][1]);
W=(double**)malloc(N*sizeof(double *));
for (i=0;i<N;i++){
W[i] = malloc(N* sizeof(double));
}
if (!W){
printf("mem error;");
return 1;
}
MakeMatrixdouble(W, N, N);
for(i=0;i<N;i++)
{
for(j=0;j<N;j++)
if(i==j)
W[i][j]=1;
}
HT=(int**)malloc(N*sizeof(double *));
for (i=0;i<N;i++){
HT[i] = malloc(P* sizeof(int));
}
if (!HT){
printf("mem error;");
return 1;
}
MakeMatrixint(HT, N, P);
for(i=0;i<P;i++)
for(n=0;n<N;n++)
{
HT[n][i]=H[i][n];
}
De=(double**)malloc(N*sizeof(double *));
for (i=0;i<N;i++){
De[i] = malloc(N* sizeof(double));
}
if (!De){
printf("mem error;");
return 1;
}
MakeMatrixdouble(De, N, N);
for(i=0;i<N;i++)
{
for(j=0;j<N;j++)
{
De[i][j]=0;
}
}
for(n=0;n<N;n++)//De biaoxian
{
for(k=0,t=1;k<100;k++)
if(hyperh[n][k]==' ')
t++;
B[n]=t;
}
for(i=0;i<N;i++)
{
for(j=0;j<N;j++)
if(i==j)
De[i][j]=B[i];
}
printf("De\n");
printf("point 2 reached.\n");
Ds=(double**)malloc(P*P*sizeof(double *));
for (i=0;i<P;i++){
Ds[i] = malloc(P* sizeof(double));
}
MakeMatrixdouble(Ds, P, P);
for(i=0;i<P;i++)
{
for(j=0;j<P;j++)
Ds[i][j]=0;
}
for(i=0;i<P;i++)
{
for(j=0;j<P;j++)
if(i==j)
Ds[i][j]=sqrt(Dv[i][j]);
}
printf("%f\n",Ds[1][1]);
FreeMatrixdouble(Dv,P,P);
Den=(double**)malloc(N*N*sizeof(double *));
for (i=0;i<N;i++){
Den[i] = malloc(N* sizeof(double));
}
MakeMatrixdouble(Den, N, N);
for(i=0;i<N;i++)
{
for(j=0;j<N;j++)
if(i==j)
{
if(De[i][j]==0)
{
Den[i][j]=1;
}
else
Den[i][j]=1/De[i][j];
}
}
Dsn=(double**)malloc(P*sizeof(double *));
for (i=0;i<P;i++){
Dsn[i] = malloc(P*sizeof(double));
}
MakeMatrixdouble(Dsn, P, P);
for(i=0;i<P;i++)
{
for(j=0;j<P;j++)
if(i==j)
{
if(Ds[i][j]==0)
{
Dsn[i][j]=1;
}
else
Dsn[i][j]=1/Ds[i][j];
}
}
printf("%f ",Dsn[1][1]);
double** DsnH;
DsnH=(double**)malloc(P*sizeof(double *));
for (i=0;i<P;i++){
DsnH[i] = malloc(N* sizeof(double));
}
MakeMatrixdouble(DsnH, P, N);
for(i=0;i<P;i++)
{
for(j=0;j<M;j++)
{
for(k=0;k<P;k++)
DsnH[i][j]+=Dsn[i][k]*H[k][j];
}
}
printf("DsnH\n ");
double** DsnHW;
DsnHW=(double**)malloc(P*sizeof(double *));
for (i=0;i<P;i++){
DsnHW[i] = malloc(N* sizeof(double));
}
MakeMatrixdouble(DsnHW, P, N);
for(i=0;i<P;i++)
{
for(j=0;j<N;j++)
{
for(k=0;k<N;k++)
DsnHW[i][j]+=DsnH[i][k]*W[k][j];
}
}
printf("DsnHW\n ");
double** DsnHWDen;
DsnHWDen=(double**)malloc(P*sizeof(double *));
for (i=0;i<P;i++){
str[i] = malloc(N* sizeof(double));
}
MakeMatrixdouble(DsnHWDen, P, N);
for(i=0;i<P;i++)
{
for(j=0;j<N;j++)
{
for(k=0;k<N;k++)
DsnHWDen[i][j]+=DsnHW[i][k]*Den[k][j];
}
}
printf("DsnHWDn\n ");
double** Az;
Az=(double**)malloc(P*sizeof(double *));
for (i=0;i<P;i++){
Az[i] = malloc(P* sizeof(double));
}
MakeMatrixdouble(Az, P,P);
for(i=0;i<P;i++)
{
for(j=0;j<P;j++)
{
for(k=0;k<N;k++)
Az[i][j]+=DsnHWDen[i][k]*HT[k][j];
}
}
printf("Az\n ");
double** A;
A=(double**)malloc(P*sizeof(double *));
for (i=0;i<P;i++){
A[i] = malloc(P* sizeof(double));
}
MakeMatrixdouble(A, P,P);
for(i=0;i<P;i++)
{
for(j=0;j<P;j++)
{
for(k=0;k<P;k++)
A[i][j]+=Az[i][k]*Dsn[k][j];
}
}
printf("A\n ");
FreeMatrixdouble(Ds,P,P);
FreeMatrixdouble(De,N,N);
FreeMatrixdouble(W,N,N);
FreeMatrixint(H,P,N);
FreeMatrixint(HT,N,P);
FreeMatrixdouble(Dsn,P,P);
FreeMatrixdouble(Den,N,N);
FreeMatrixdouble(DsnH,P,N);
FreeMatrixdouble(DsnHW,P,N);
FreeMatrixdouble(DsnHWDen,P,N);
FreeMatrixdouble(Az,P,P);
printf("\n"); //qiu (I-aA)
double x=0.3;
X=(double**)malloc(P*sizeof(double *));
for (i=0;i<P;i++){
X[i] = malloc(P* sizeof(double));
}
MakeMatrixdouble(X, P,P);
C=(double**)malloc(P*sizeof(double *));
for (i=0;i<P;i++){
C[i] = malloc(P* sizeof(double));
}
MakeMatrixdouble(C, P,P);
for(i=0;i<P;i++)
{
for(j=0;j<P;j++)
X[i][j]=x*A[i][j];
}
for(i=0;i<P;i++)
{
for(j=0;j<P;j++)
if(i==j)
{
C[i][j]=(1-X[i][j]);
}
else
C[i][j]=-X[i][j];
}
printf("c\n ");
double** Cnii;
for(i=0;i<P;i++)
{ //ni juzheng
for(j=0;j<P;j++)
Cni[i*P+j]=C[i][j];
}
Cnii=(double**)malloc(P*sizeof(double *));
for (i=0;i<P;i++){
Cnii[i] = malloc(P* sizeof(double));
}
MakeMatrixdouble(Cnii,P,P);
double *b_data=(double*)malloc(sizeof(double)*P);
for(i=0;i<P;i++)
{
b_data[i]=i;
}
gsl_matrix_view nj
= gsl_matrix_view_array(Cni, P, P);
//gsl_vector_view c = gsl_vector_view_array(b_data, P);
//gsl_vector *xj = gsl_vector_alloc (P);
gsl_matrix *mj= gsl_matrix_alloc (P,P);
int r;
gsl_permutation *pi = gsl_permutation_alloc (12035);
gsl_linalg_LU_decomp (&nj.matrix, pi, &r);
gsl_linalg_LU_invert (&nj.matrix, pi, mj);
printf ("inv Cni=\n");
{
int i;
for (i = 0; i < P; i++)
{
gsl_vector_view mj_i= gsl_matrix_column (mj, i);
for(j=0;j<P;j++)
{
Cni[i*P+j]= gsl_vector_get(&mj_i.vector, j);
}
}
}
for(i=0;i<P;i++)
{
for(j=0;j<P;j++)
Cnii[i][j]=Cni[i*P+j];
}
gsl_permutation_free (pi);
printf("\n");
double *y=(double*)malloc(sizeof(double)*P); // keyword ni yo ru hypergraph no value
char v[10]="���ì";
//printf("please input keyword:\n");
//scanf("%s",v);
//double *r=(double*)malloc(sizeof(double)*P);
int h;
// for(i=0;i<P;i++) //search the keyword
// r[i]=0;
for(i=0;i<P;i++)
{
for(j=0;str[i][j]!='\0';j++)//node
{
for(k=j,m=0;v[m]!='\0';)//line do not over
if(str[i][k]==v[m])//one word pipei
k++,m++;
else
break;
if(v[m]== '\0' && m > 0)//note over win
{
h=i;
}
}
}
printf("%d\n",h);
printf("y=\n"); // made y
for(i=0;i<P;i++)
{
if(i==h)
{
y[i]=1;
}
else
y[i]=A[h][i];
}
printf("\n"); //fina value f
printf("f=\n");
double *f=(double*)malloc(sizeof(double)*P);
for(i=0;i<P;i++){
f[i]=0;
}
for(i=0;i<P;i++)
for(k=0;k<P;k++)
f[i]+=Cnii[i][k]*y[k];
for(i=0;i<U;i++)
{
printf("%f\n",f[i]);
}
//for(i=0;i<U;i++)
//fprintf(fp3,"%f\n",f[i]);
return 0;
}
void GripperSelTransform::OnGripperReleased ( const double* from, const double* to )
{
wxGetApp().DestroyTransformGLList();
wxGetApp().StartHistory();
for ( std::list<HeeksObj *>::iterator It = m_items_marked_at_grab.begin(); It != m_items_marked_at_grab.end(); It++ )
{
HeeksObj* object = *It;
if ( object == m_gripper_parent && m_data.m_type > GripperTypeScale )
{
double shift[3];
if(m_data.m_move_relative){
shift[0] = to[0] - from[0];
shift[1] = to[1] - from[1];
shift[2] = to[2] - from[2];
}
else{
shift[0] = to[0] - m_initial_grip_pos[0];
shift[1] = to[1] - m_initial_grip_pos[1];
shift[2] = to[2] - m_initial_grip_pos[2];
}
{
if(object)wxGetApp().DoUndoable(new StretchTool(object, m_initial_grip_pos, shift, m_data.m_data));
}
m_data.m_x += shift[0];
m_data.m_y += shift[1];
m_data.m_z += shift[2];
}
else
{
gp_Trsf mat;
double object_m[16] = {1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1};
if(m_items_marked_at_grab.size() > 0)m_items_marked_at_grab.front()->GetScaleAboutMatrix(object_m);
MakeMatrix ( from, to, object_m, mat );
double m[16];
extract(mat, m );
wxGetApp().TransformUndoably(object, m);
}
}
m_items_marked_at_grab.clear();
if ( m_data.m_type <= GripperTypeObjectScaleXY )
{
for(std::list<HeeksObj*>::iterator It = wxGetApp().m_hidden_for_drag.begin(); It != wxGetApp().m_hidden_for_drag.end(); It++)
{
HeeksObj* object = *It;
object->m_visible = true;
}
wxGetApp().m_hidden_for_drag.clear();
}
{
std::list<HeeksObj *>::iterator It;
for ( It = m_items_marked_at_grab.begin(); It != m_items_marked_at_grab.end(); It++ )
{
wxGetApp().m_marked_list->set_ignore_onoff ( *It, false );
}
}
wxGetApp().m_marked_list->gripping = false;
wxGetApp().EndHistory();
}
int dCollideSTL(dxGeom* g1, dxGeom* SphereGeom, int Flags, dContactGeom* Contacts, int Stride){
dxTriMesh* TriMesh = (dxTriMesh*)g1;
// Init
const dVector3& TLPosition = *(const dVector3*)dGeomGetPosition(TriMesh);
const dMatrix3& TLRotation = *(const dMatrix3*)dGeomGetRotation(TriMesh);
SphereCollider& Collider = TriMesh->_SphereCollider;
const dVector3& Position = *(const dVector3*)dGeomGetPosition(SphereGeom);
dReal Radius = dGeomSphereGetRadius(SphereGeom);
// Sphere
Sphere Sphere;
Sphere.mCenter.x = Position[0];
Sphere.mCenter.y = Position[1];
Sphere.mCenter.z = Position[2];
Sphere.mRadius = Radius;
Matrix4x4 amatrix;
// TC results
if (TriMesh->doSphereTC) {
dxTriMesh::SphereTC* sphereTC = 0;
for (int i = 0; i < TriMesh->SphereTCCache.size(); i++){
if (TriMesh->SphereTCCache[i].Geom == SphereGeom){
sphereTC = &TriMesh->SphereTCCache[i];
break;
}
}
if (!sphereTC){
TriMesh->SphereTCCache.push(dxTriMesh::SphereTC());
sphereTC = &TriMesh->SphereTCCache[TriMesh->SphereTCCache.size() - 1];
sphereTC->Geom = SphereGeom;
}
// Intersect
Collider.SetTemporalCoherence(true);
Collider.Collide(*sphereTC, Sphere, TriMesh->Data->BVTree, null,
&MakeMatrix(TLPosition, TLRotation, amatrix));
}
else {
Collider.SetTemporalCoherence(false);
Collider.Collide(dxTriMesh::defaultSphereCache, Sphere, TriMesh->Data->BVTree, null,
&MakeMatrix(TLPosition, TLRotation, amatrix));
}
if (!Collider.GetContactStatus()) {
/* no collision occurred */
return 0;
}
// get results
int TriCount = Collider.GetNbTouchedPrimitives();
const int* Triangles = (const int*)Collider.GetTouchedPrimitives();
if (TriCount != 0){
if (TriMesh->ArrayCallback != null){
TriMesh->ArrayCallback(TriMesh, SphereGeom, Triangles, TriCount);
}
int OutTriCount = 0;
for (int i = 0; i < TriCount; i++){
if (OutTriCount == (Flags & 0xffff)){
break;
}
const int& TriIndex = Triangles[i];
dVector3 dv[3];
FetchTriangle(TriMesh, TriIndex, TLPosition, TLRotation, dv);
dVector3& v0 = dv[0];
dVector3& v1 = dv[1];
dVector3& v2 = dv[2];
dVector3 vu;
vu[0] = v1[0] - v0[0];
vu[1] = v1[1] - v0[1];
vu[2] = v1[2] - v0[2];
vu[3] = REAL(0.0);
dVector3 vv;
vv[0] = v2[0] - v0[0];
vv[1] = v2[1] - v0[1];
vv[2] = v2[2] - v0[2];
vv[3] = REAL(0.0);
dReal Depth;
float u, v;
if (!GetContactData(Position, Radius, v0, vu, vv, Depth, u, v)){
continue; // Sphere doesnt hit triangle
}
dReal w = REAL(1.0) - u - v;
if (Depth < REAL(0.0)){
Depth = REAL(0.0);
}
dContactGeom* Contact = SAFECONTACT(Flags, Contacts, OutTriCount, Stride);
Contact->pos[0] = (v0[0] * w) + (v1[0] * u) + (v2[0] * v);
Contact->pos[1] = (v0[1] * w) + (v1[1] * u) + (v2[1] * v);
Contact->pos[2] = (v0[2] * w) + (v1[2] * u) + (v2[2] * v);
Contact->pos[3] = REAL(0.0);
dVector4 Plane;
dCROSS(Plane, =, vv, vu); // Reversed
Plane[3] = dDOT(Plane, v0); // Using normal as plane.
dReal Area = dSqrt(dDOT(Plane, Plane)); // We can use this later
Plane[0] /= Area;
Plane[1] /= Area;
Plane[2] /= Area;
Plane[3] /= Area;
Contact->normal[0] = Plane[0];
Contact->normal[1] = Plane[1];
Contact->normal[2] = Plane[2];
Contact->normal[3] = REAL(0.0);
Contact->depth = Depth;
//Contact->g1 = TriMesh;
//Contact->g2 = SphereGeom;
OutTriCount++;
}
#ifdef MERGECONTACTS // Merge all contacts into 1
if (OutTriCount != 0){
dContactGeom* Contact = SAFECONTACT(Flags, Contacts, 0, Stride);
if (OutTriCount != 1){
Contact->normal[0] *= Contact->depth;
Contact->normal[1] *= Contact->depth;
Contact->normal[2] *= Contact->depth;
Contact->normal[3] *= Contact->depth;
for (int i = 1; i < OutTriCount; i++){
dContactGeom* TempContact = SAFECONTACT(Flags, Contacts, i, Stride);
Contact->pos[0] += TempContact->pos[0];
Contact->pos[1] += TempContact->pos[1];
Contact->pos[2] += TempContact->pos[2];
Contact->pos[3] += TempContact->pos[3];
Contact->normal[0] += TempContact->normal[0] * TempContact->depth;
Contact->normal[1] += TempContact->normal[1] * TempContact->depth;
Contact->normal[2] += TempContact->normal[2] * TempContact->depth;
Contact->normal[3] += TempContact->normal[3] * TempContact->depth;
}
Contact->pos[0] /= OutTriCount;
Contact->pos[1] /= OutTriCount;
Contact->pos[2] /= OutTriCount;
Contact->pos[3] /= OutTriCount;
// Remember to divide in square space.
Contact->depth = dSqrt(dDOT(Contact->normal, Contact->normal) / OutTriCount);
dNormalize3(Contact->normal);
}
Contact->g1 = TriMesh;
Contact->g2 = SphereGeom;
return 1;
}
else return 0;
#elif defined MERGECONTACTNORMALS // Merge all normals, and distribute between all contacts
if (OutTriCount != 0){
if (OutTriCount != 1){
dVector3& Normal = SAFECONTACT(Flags, Contacts, 0, Stride)->normal;
Normal[0] *= SAFECONTACT(Flags, Contacts, 0, Stride)->depth;
Normal[1] *= SAFECONTACT(Flags, Contacts, 0, Stride)->depth;
Normal[2] *= SAFECONTACT(Flags, Contacts, 0, Stride)->depth;
Normal[3] *= SAFECONTACT(Flags, Contacts, 0, Stride)->depth;
for (int i = 1; i < OutTriCount; i++){
dContactGeom* Contact = SAFECONTACT(Flags, Contacts, i, Stride);
Normal[0] += Contact->normal[0] * Contact->depth;
Normal[1] += Contact->normal[1] * Contact->depth;
Normal[2] += Contact->normal[2] * Contact->depth;
Normal[3] += Contact->normal[3] * Contact->depth;
}
dNormalize3(Normal);
for (int i = 1; i < OutTriCount; i++){
dContactGeom* Contact = SAFECONTACT(Flags, Contacts, i, Stride);
Contact->normal[0] = Normal[0];
Contact->normal[1] = Normal[1];
Contact->normal[2] = Normal[2];
Contact->normal[3] = Normal[3];
Contact->g1 = TriMesh;
Contact->g2 = SphereGeom;
}
}
else{
SAFECONTACT(Flags, Contacts, 0, Stride)->g1 = TriMesh;
SAFECONTACT(Flags, Contacts, 0, Stride)->g2 = SphereGeom;
}
return OutTriCount;
}
else return 0;
#else //MERGECONTACTNORMALS // Just gather penetration depths and return
for (int i = 0; i < OutTriCount; i++){
dContactGeom* Contact = SAFECONTACT(Flags, Contacts, i, Stride);
//Contact->depth = dSqrt(dDOT(Contact->normal, Contact->normal));
/*Contact->normal[0] /= Contact->depth;
Contact->normal[1] /= Contact->depth;
Contact->normal[2] /= Contact->depth;
Contact->normal[3] /= Contact->depth;*/
Contact->g1 = TriMesh;
Contact->g2 = SphereGeom;
}
return OutTriCount;
#endif // MERGECONTACTS
}
else return 0;
void GraphicsWindow::Paint(void) {
int i;
havePainted = true;
int w, h;
GetGraphicsWindowSize(&w, &h);
width = w; height = h;
glViewport(0, 0, w, h);
glMatrixMode(GL_PROJECTION);
glLoadIdentity();
glScaled(scale*2.0/w, scale*2.0/h, scale*1.0/30000);
double mat[16];
// Last thing before display is to apply the perspective
double clp = SS.CameraTangent()*scale;
MakeMatrix(mat, 1, 0, 0, 0,
0, 1, 0, 0,
0, 0, 1, 0,
0, 0, clp, 1);
glMultMatrixd(mat);
// Before that, we apply the rotation
Vector n = projUp.Cross(projRight);
MakeMatrix(mat, projRight.x, projRight.y, projRight.z, 0,
projUp.x, projUp.y, projUp.z, 0,
n.x, n.y, n.z, 0,
0, 0, 0, 1);
glMultMatrixd(mat);
// And before that, the translation
MakeMatrix(mat, 1, 0, 0, offset.x,
0, 1, 0, offset.y,
0, 0, 1, offset.z,
0, 0, 0, 1);
glMultMatrixd(mat);
glMatrixMode(GL_MODELVIEW);
glLoadIdentity();
glShadeModel(GL_SMOOTH);
glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
glEnable(GL_BLEND);
glEnable(GL_LINE_SMOOTH);
// don't enable GL_POLYGON_SMOOTH; that looks ugly on some graphics cards,
// drawn with leaks in the mesh
glEnable(GL_POLYGON_OFFSET_LINE);
glEnable(GL_POLYGON_OFFSET_FILL);
glEnable(GL_DEPTH_TEST);
glHint(GL_LINE_SMOOTH_HINT, GL_NICEST);
glEnable(GL_NORMALIZE);
// At the same depth, we want later lines drawn over earlier.
glDepthFunc(GL_LEQUAL);
if(SS.AllGroupsOkay()) {
glClearColor(SS.backgroundColor.redF(),
SS.backgroundColor.greenF(),
SS.backgroundColor.blueF(), 1.0f);
} else {
// Draw a different background whenever we're having solve problems.
RgbColor rgb = Style::Color(Style::DRAW_ERROR);
glClearColor(0.4f*rgb.redF(), 0.4f*rgb.greenF(), 0.4f*rgb.blueF(), 1.0f);
// And show the text window, which has info to debug it
ForceTextWindowShown();
}
glClear(GL_COLOR_BUFFER_BIT);
glClearDepth(1.0);
glClear(GL_DEPTH_BUFFER_BIT);
if(SS.bgImage.fromFile) {
// If a background image is loaded, then we draw it now as a texture.
// This handles the resizing for us nicely.
glBindTexture(GL_TEXTURE_2D, TEXTURE_BACKGROUND_IMG);
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP);
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP);
glTexEnvf(GL_TEXTURE_ENV, GL_TEXTURE_ENV_MODE, GL_DECAL);
glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB,
SS.bgImage.rw, SS.bgImage.rh,
0,
GL_RGB, GL_UNSIGNED_BYTE,
SS.bgImage.fromFile);
double tw = ((double)SS.bgImage.w) / SS.bgImage.rw,
th = ((double)SS.bgImage.h) / SS.bgImage.rh;
double mmw = SS.bgImage.w / SS.bgImage.scale,
mmh = SS.bgImage.h / SS.bgImage.scale;
Vector origin = SS.bgImage.origin;
origin = origin.DotInToCsys(projRight, projUp, n);
// Place the depth of our origin at the point that corresponds to
// w = 1, so that it's unaffected by perspective.
origin.z = (offset.ScaledBy(-1)).Dot(n);
origin = origin.ScaleOutOfCsys(projRight, projUp, n);
// Place the background at the very back of the Z order, though, by
// mucking with the depth range.
glDepthRange(1, 1);
glEnable(GL_TEXTURE_2D);
glBegin(GL_QUADS);
glTexCoord2d(0, 0);
ssglVertex3v(origin);
glTexCoord2d(0, th);
ssglVertex3v(origin.Plus(projUp.ScaledBy(mmh)));
glTexCoord2d(tw, th);
ssglVertex3v(origin.Plus(projRight.ScaledBy(mmw).Plus(
projUp. ScaledBy(mmh))));
glTexCoord2d(tw, 0);
ssglVertex3v(origin.Plus(projRight.ScaledBy(mmw)));
glEnd();
glDisable(GL_TEXTURE_2D);
}
ssglDepthRangeOffset(0);
// Nasty case when we're reloading the imported files; could be that
// we get an error, so a dialog pops up, and a message loop starts, and
// we have to get called to paint ourselves. If the sketch is screwed
// up, then we could trigger an oops trying to draw.
if(!SS.allConsistent) return;
// Let's use two lights, at the user-specified locations
GLfloat f;
glEnable(GL_LIGHT0);
f = (GLfloat)SS.lightIntensity[0];
GLfloat li0[] = { f, f, f, 1.0f };
glLightfv(GL_LIGHT0, GL_DIFFUSE, li0);
glLightfv(GL_LIGHT0, GL_SPECULAR, li0);
glEnable(GL_LIGHT1);
f = (GLfloat)SS.lightIntensity[1];
GLfloat li1[] = { f, f, f, 1.0f };
glLightfv(GL_LIGHT1, GL_DIFFUSE, li1);
glLightfv(GL_LIGHT1, GL_SPECULAR, li1);
Vector ld;
ld = VectorFromProjs(SS.lightDir[0]);
GLfloat ld0[4] = { (GLfloat)ld.x, (GLfloat)ld.y, (GLfloat)ld.z, 0 };
glLightfv(GL_LIGHT0, GL_POSITION, ld0);
ld = VectorFromProjs(SS.lightDir[1]);
GLfloat ld1[4] = { (GLfloat)ld.x, (GLfloat)ld.y, (GLfloat)ld.z, 0 };
glLightfv(GL_LIGHT1, GL_POSITION, ld1);
if(SS.drawBackFaces) {
// For debugging, draw the backs of the triangles in red, so that we
// notice when a shell is open
glLightModelf(GL_LIGHT_MODEL_TWO_SIDE, 1);
} else {
glLightModelf(GL_LIGHT_MODEL_TWO_SIDE, 0);
}
GLfloat ambient[4] = { (float)SS.ambientIntensity,
(float)SS.ambientIntensity,
(float)SS.ambientIntensity, 1 };
glLightModelfv(GL_LIGHT_MODEL_AMBIENT, ambient);
ssglUnlockColor();
if(showSnapGrid && LockedInWorkplane()) {
hEntity he = ActiveWorkplane();
EntityBase *wrkpl = SK.GetEntity(he),
*norm = wrkpl->Normal();
Vector wu, wv, wn, wp;
wp = SK.GetEntity(wrkpl->point[0])->PointGetNum();
wu = norm->NormalU();
wv = norm->NormalV();
wn = norm->NormalN();
double g = SS.gridSpacing;
double umin = VERY_POSITIVE, umax = VERY_NEGATIVE,
vmin = VERY_POSITIVE, vmax = VERY_NEGATIVE;
int a;
for(a = 0; a < 4; a++) {
// Ideally, we would just do +/- half the width and height; but
// allow some extra slop for rounding.
Vector horiz = projRight.ScaledBy((0.6*width)/scale + 2*g),
vert = projUp. ScaledBy((0.6*height)/scale + 2*g);
if(a == 2 || a == 3) horiz = horiz.ScaledBy(-1);
if(a == 1 || a == 3) vert = vert. ScaledBy(-1);
Vector tp = horiz.Plus(vert).Minus(offset);
// Project the point into our grid plane, normal to the screen
// (not to the grid plane). If the plane is on edge then this is
// impossible so don't try to draw the grid.
bool parallel;
Vector tpp = Vector::AtIntersectionOfPlaneAndLine(
wn, wn.Dot(wp),
tp, tp.Plus(n),
¶llel);
if(parallel) goto nogrid;
tpp = tpp.Minus(wp);
double uu = tpp.Dot(wu),
vv = tpp.Dot(wv);
umin = min(uu, umin);
umax = max(uu, umax);
vmin = min(vv, vmin);
vmax = max(vv, vmax);
}
int i, j, i0, i1, j0, j1;
i0 = (int)(umin / g);
i1 = (int)(umax / g);
j0 = (int)(vmin / g);
j1 = (int)(vmax / g);
if(i0 > i1 || i1 - i0 > 400) goto nogrid;
if(j0 > j1 || j1 - j0 > 400) goto nogrid;
glLineWidth(1);
ssglColorRGBa(Style::Color(Style::DATUM), 0.3);
glBegin(GL_LINES);
for(i = i0 + 1; i < i1; i++) {
ssglVertex3v(wp.Plus(wu.ScaledBy(i*g)).Plus(wv.ScaledBy(j0*g)));
ssglVertex3v(wp.Plus(wu.ScaledBy(i*g)).Plus(wv.ScaledBy(j1*g)));
}
for(j = j0 + 1; j < j1; j++) {
ssglVertex3v(wp.Plus(wu.ScaledBy(i0*g)).Plus(wv.ScaledBy(j*g)));
ssglVertex3v(wp.Plus(wu.ScaledBy(i1*g)).Plus(wv.ScaledBy(j*g)));
}
glEnd();
// Clear the depth buffer, so that the grid is at the very back of
// the Z order.
glClear(GL_DEPTH_BUFFER_BIT);
nogrid:;
}
// Draw the active group; this does stuff like the mesh and edges.
(SK.GetGroup(activeGroup))->Draw();
// Now draw the entities
if(showHdnLines) glDisable(GL_DEPTH_TEST);
Entity::DrawAll();
// Draw filled paths in all groups, when those filled paths were requested
// specially by assigning a style with a fill color, or when the filled
// paths are just being filled by default. This should go last, to make
// the transparency work.
Group *g;
for(g = SK.group.First(); g; g = SK.group.NextAfter(g)) {
if(!(g->IsVisible())) continue;
g->DrawFilledPaths();
}
glDisable(GL_DEPTH_TEST);
// Draw the constraints
for(i = 0; i < SK.constraint.n; i++) {
SK.constraint.elem[i].Draw();
}
// Draw the traced path, if one exists
glLineWidth(Style::Width(Style::ANALYZE));
ssglColorRGB(Style::Color(Style::ANALYZE));
SContour *sc = &(SS.traced.path);
glBegin(GL_LINE_STRIP);
for(i = 0; i < sc->l.n; i++) {
ssglVertex3v(sc->l.elem[i].p);
}
glEnd();
// And the naked edges, if the user did Analyze -> Show Naked Edges.
glLineWidth(Style::Width(Style::DRAW_ERROR));
ssglColorRGB(Style::Color(Style::DRAW_ERROR));
ssglDrawEdges(&(SS.nakedEdges), true);
// Then redraw whatever the mouse is hovering over, highlighted.
glDisable(GL_DEPTH_TEST);
ssglLockColorTo(Style::Color(Style::HOVERED));
hover.Draw();
// And finally draw the selection, same mechanism.
ssglLockColorTo(Style::Color(Style::SELECTED));
for(Selection *s = selection.First(); s; s = selection.NextAfter(s)) {
s->Draw();
}
ssglUnlockColor();
// If a marquee selection is in progress, then draw the selection
// rectangle, as an outline and a transparent fill.
if(pending.operation == DRAGGING_MARQUEE) {
Point2d begin = ProjectPoint(orig.marqueePoint);
double xmin = min(orig.mouse.x, begin.x),
xmax = max(orig.mouse.x, begin.x),
ymin = min(orig.mouse.y, begin.y),
ymax = max(orig.mouse.y, begin.y);
Vector tl = UnProjectPoint(Point2d::From(xmin, ymin)),
tr = UnProjectPoint(Point2d::From(xmax, ymin)),
br = UnProjectPoint(Point2d::From(xmax, ymax)),
bl = UnProjectPoint(Point2d::From(xmin, ymax));
glLineWidth((GLfloat)1.3);
ssglColorRGB(Style::Color(Style::HOVERED));
glBegin(GL_LINE_LOOP);
ssglVertex3v(tl);
ssglVertex3v(tr);
ssglVertex3v(br);
ssglVertex3v(bl);
glEnd();
ssglColorRGBa(Style::Color(Style::HOVERED), 0.10);
glBegin(GL_QUADS);
ssglVertex3v(tl);
ssglVertex3v(tr);
ssglVertex3v(br);
ssglVertex3v(bl);
glEnd();
}
// An extra line, used to indicate the origin when rotating within the
// plane of the monitor.
if(SS.extraLine.draw) {
glLineWidth(1);
ssglLockColorTo(Style::Color(Style::DATUM));
glBegin(GL_LINES);
ssglVertex3v(SS.extraLine.ptA);
ssglVertex3v(SS.extraLine.ptB);
glEnd();
}
// A note to indicate the origin in the just-exported file.
if(SS.justExportedInfo.draw) {
ssglColorRGB(Style::Color(Style::DATUM));
Vector p = SS.justExportedInfo.pt,
u = SS.justExportedInfo.u,
v = SS.justExportedInfo.v;
glLineWidth(1.5);
glBegin(GL_LINES);
ssglVertex3v(p.Plus(u.WithMagnitude(-15/scale)));
ssglVertex3v(p.Plus(u.WithMagnitude(30/scale)));
ssglVertex3v(p.Plus(v.WithMagnitude(-15/scale)));
ssglVertex3v(p.Plus(v.WithMagnitude(30/scale)));
glEnd();
ssglWriteText("(x, y) = (0, 0) for file just exported",
DEFAULT_TEXT_HEIGHT,
p.Plus(u.ScaledBy(10/scale)).Plus(v.ScaledBy(10/scale)),
u, v, NULL, NULL);
ssglWriteText("press Esc to clear this message",
DEFAULT_TEXT_HEIGHT,
p.Plus(u.ScaledBy(40/scale)).Plus(
v.ScaledBy(-(DEFAULT_TEXT_HEIGHT)/scale)),
u, v, NULL, NULL);
}
// And finally the toolbar.
if(SS.showToolbar) {
ToolbarDraw();
}
}