Distance between two line segments

Question

EDIT: PROBLEM SOLVED. This code works properly !

I translated this python code Shortest distance between two line segments (answered by Fnord) to Objective-C in order to find the shortest distance between two line segments. However the distances are incorrect. Could you please help me find what is wrong?

Matrix.h:

#import <Cocoa/Cocoa.h>

@interface Matrix : NSObject

@property NSUInteger m;
@property NSUInteger n;
@property NSMutableArray *rows;

- (id)initWithSizeM:(NSUInteger)m N:(NSUInteger)n;
- (CGFloat)determinant3x3;

@end

Matrix.m:

#import <Cocoa/Cocoa.h>
#import "Matrix.h"

@implementation Matrix;

- (id)initWithSizeM:(NSUInteger)m N:(NSUInteger)n {
    self = [super init];
    _m = m;
    _n = n;
    _rows = [NSMutableArray new];
    for (int i = 0; i < m; i ++) {
        [_rows addObject:[NSMutableArray new]];
        for (int j = 0; j < n; j ++) {
            CGFloat value = 0.0;
            [_rows[i] addObject:[NSNumber numberWithFloat:value]];
        }
    }
    return self;
}

- (CGFloat)determinant3x3 {
    CGFloat a = [_rows[0][0] floatValue];
    CGFloat b = [_rows[0][1] floatValue];
    CGFloat c = [_rows[0][2] floatValue];
    CGFloat d = [_rows[1][0] floatValue];
    CGFloat e = [_rows[1][1] floatValue];
    CGFloat f = [_rows[1][2] floatValue];
    CGFloat g = [_rows[2][0] floatValue];
    CGFloat h = [_rows[2][1] floatValue];
    CGFloat i = [_rows[2][2] floatValue];
    return a*e*i + b*f*g + c*d*h - c*e*g - b*d*i - a*f*h;
}


@end

PointCartesian.h:

#import <Cocoa/Cocoa.h>
#import "Vector.h"

@class Vertex;
@interface PointCartesian: NSObject

@property CGFloat x;
@property CGFloat y;
@property CGFloat z;

@property NSString *name;

- (id)initWithCoordinatesX:(CGFloat)x Y:(CGFloat)y Z:(CGFloat)z;
- (Vector*)toVector;
+ (CGFloat)shortestDistanceBetweenTwoSegmentsA0:(PointCartesian*)a0 A1:(PointCartesian*)a1 B0:(PointCartesian*)b0 B1:(PointCartesian*)b1;

@end

PointCartesian.m:

#import <Cocoa/Cocoa.h>
#import "PointCartesian.h"
#import "Vector.h"
#import "Matrix.h"

@implementation PointCartesian;

- (id)initWithCoordinatesX:(CGFloat)x Y:(CGFloat)y Z:(CGFloat)z {
    _x = x;
    _y = y;
    _z = z;
    return self;
}

- (Vector*)toVector {
    return [[Vector alloc] initWithX:_x Y:_y Z:_z];
}

+ (CGFloat)shortestDistanceBetweenTwoSegmentsA0:(PointCartesian*)a0 A1:(PointCartesian*)a1 B0:(PointCartesian*)b0 B1:(PointCartesian*)b1 {
    BOOL clampA0 = YES;
    BOOL clampA1 = YES;
    BOOL clampB0 = YES;
    BOOL clampB1 = YES;
    Vector *A = [Vector vectorFromA:a0 B:a1];
    Vector *B = [Vector vectorFromA:b0 B:b1];
    CGFloat magA = [A length];
    CGFloat magB = [B length];
    Vector *_A = [[Vector alloc] initWithX:A.x/magA Y:A.y/magA Z:A.z/magA];
    Vector *_B = [[Vector alloc] initWithX:B.x/magB Y:B.y/magB Z:B.z/magB];
    Vector *cross = [Vector crossProductA:_A B:_B];
    CGFloat denom = pow([cross length], 2);
    
    if (denom == 0) {
        CGFloat d0 = [Vector dotProductA:_A B:[Vector vectorFromA:a0 B:b0]];
        if (clampA0 || clampA1 || clampB0 || clampB1) {
            CGFloat d1 = [Vector dotProductA:_A B:[Vector vectorFromA:a0 B:b1]];
            if (d0 <= 0 && 0 >= d1) {
                if (clampA0 && clampB1) {
                    if (floor(d0) < floor(d1)) {
                        return [[Vector vectorFromA:b0 B:a0] length];
                    }
                   return [[Vector vectorFromA:b1 B:a0] length];
                }
            }
            if (d0 >= magA && magA <= d1) {
                if (clampA1 && clampB0) {
                    if (floor(d0) < floor(d1)) {
                        return [[Vector vectorFromA:b0 B:a1] length];
                    }
                    return [[Vector vectorFromA:b1 B:a1] length];
                }
            }
        }
        return [[[Vector alloc] initWithX:d0*_A.x + a0.x - b0.x Y:d0*_A.y + a0.y - b0.y Z:d0*_A.z + a0.z - b0.z] length];
    }
    
    Vector *t = [Vector vectorFromA:a0 B:b0];
    
    Matrix *MA = [[Matrix alloc] initWithSizeM:3 N:3];
    MA.rows[0][0] = [NSNumber numberWithFloat:t.x];
    MA.rows[0][1] = [NSNumber numberWithFloat:t.y];
    MA.rows[0][2] = [NSNumber numberWithFloat:t.z];
    MA.rows[1][0] = [NSNumber numberWithFloat:_B.x];
    MA.rows[1][1] = [NSNumber numberWithFloat:_B.y];
    MA.rows[1][2] = [NSNumber numberWithFloat:_B.z];
    MA.rows[2][0] = [NSNumber numberWithFloat:cross.x];
    MA.rows[2][1] = [NSNumber numberWithFloat:cross.y];
    MA.rows[2][2] = [NSNumber numberWithFloat:cross.z];
    
    Matrix *MB = [[Matrix alloc] initWithSizeM:3 N:3];
    MB.rows[0][0] = [NSNumber numberWithFloat:t.x];
    MB.rows[0][1] = [NSNumber numberWithFloat:t.y];
    MB.rows[0][2] = [NSNumber numberWithFloat:t.z];
    MB.rows[1][0] = [NSNumber numberWithFloat:_A.x];
    MB.rows[1][1] = [NSNumber numberWithFloat:_A.y];
    MB.rows[1][2] = [NSNumber numberWithFloat:_A.z];
    MB.rows[2][0] = [NSNumber numberWithFloat:cross.x];
    MB.rows[2][1] = [NSNumber numberWithFloat:cross.y];
    MB.rows[2][2] = [NSNumber numberWithFloat:cross.z];
    
    CGFloat detA = [MA determinant3x3];
    CGFloat detB = [MB determinant3x3];
    
    CGFloat t0 = detA / denom;
    CGFloat t1 = detB / denom;
    
    PointCartesian *pA = [[PointCartesian alloc] initWithCoordinatesX:_A.x*t0 + a0.x Y:_A.y*t0 + a0.y Z:_A.z*t0 + a0.z];
    PointCartesian *pB = [[PointCartesian alloc] initWithCoordinatesX:_B.x*t1 + b0.x Y:_B.y*t1 + b0.y Z:_B.z*t1 + b0.z];

    if (clampA0 || clampA1 || clampB0 || clampB1) {
        if (clampA0 && t0 < 0) {
            pA = a0;
        } else if (clampA1 && t0 > magA) {
            pA = a1;
        }
        if (clampB0 && t1 < 0) {
            pB = b0;
        } else if (clampB1 && t1 > magB) {
            pB = b1;
        }
        if ((clampA0 && t0 < 0) || (clampA1 && t0 > magA)) {
            CGFloat dot = [Vector dotProductA:_B B:[Vector vectorFromA:b0 B:pA]];
            if (clampB0 && dot < 0) {
                dot = 0;
            } else if (clampB1 && dot > magB) {
                dot = magB;
            }
            pB = [[PointCartesian alloc] initWithCoordinatesX:b0.x + _B.x*dot Y:b0.y + _B.y*dot Z:b0.z + _B.z*dot];
        }
        if ((clampB0 && t1 < 0) || (clampB1 && t1 > magB)) {
            CGFloat dot = [Vector dotProductA:_A B:[Vector vectorFromA:a0 B:pB]];
            if (clampA0 && dot < 0) {
                dot = 0;
            } else if (clampA1 && dot > magA) {
                dot = magA;
            }
            pA = [[PointCartesian alloc] initWithCoordinatesX:a0.x + _A.x*dot Y:a0.y + _A.y*dot Z:a0.z + _A.z*dot];
        }
    }
    
    return [[[Vector alloc] initWithX:pA.x-pB.x Y:pA.y-pB.y Z:pA.z - pB.z] length];
}


@end

Vector.h:

#import <Cocoa/Cocoa.h>

@class PointCartesian;
@interface Vector: NSObject

@property CGFloat x;
@property CGFloat y;
@property CGFloat z;

@property NSString *name;

- (id)initWithX:(CGFloat)x Y:(CGFloat)y Z:(CGFloat)z;
- (CGFloat)length;
- (void)normalize;
+ (Vector*)vectorFromA:(PointCartesian*)a B:(PointCartesian*)b;
- (PointCartesian*)toPoint;
+ (CGFloat)dotProductA:(Vector*)a B:(Vector*)b;
+ (Vector*)crossProductA:(Vector*)a B:(Vector*)b;

@end

Vector.m:

#import <Cocoa/Cocoa.h>
#import "Vector.h"
#import "PointCartesian.h"

@implementation Vector

- (id)initWithX:(CGFloat)x Y:(CGFloat)y Z:(CGFloat)z {
    _x = x;
    _y = y;
    _z = z;
    return self;
}
- (CGFloat)length {
    return sqrt(pow(_x,2)+pow(_y,2)+pow(_z,2));
}

- (void)normalize {
    _x = _x / [self length];
    _y = _y / [self length];
    _z = _z / [self length];
}

+ (Vector*)vectorFromA:(PointCartesian*)a B:(PointCartesian*)b {
    Vector *AB = [[Vector alloc] initWithX:b.x-a.x Y:b.y-a.y Z:b.z-a.z];
    return AB;
}

- (PointCartesian*)toPoint {
    return [[PointCartesian alloc] initWithCoordinatesX:_x Y:_y Z:_z];
}

+ (CGFloat)dotProductA:(Vector*)a B:(Vector*)b {
    return a.x*b.x + a.y*b.y + a.z*b.z;
}

+ (Vector*)crossProductA:(Vector*)a B:(Vector*)b {
    return [[Vector alloc] initWithX:a.y*b.z - a.z*b.y Y:a.z*b.x - a.x*b.z Z:a.x*b.y - a.y*b.x];
}




@end

main.m:


#import <Cocoa/Cocoa.h>
#import "PointCartesian.h"


int main(int argc, const char * argv[]) {
    PointCartesian *a0 = [[PointCartesian alloc] initWithCoordinatesX:27.83 Y:31.74 Z:-26.60];
    PointCartesian *a1 = [[PointCartesian alloc] initWithCoordinatesX:13.43 Y:21.77 Z:46.81];
    PointCartesian *b0 = [[PointCartesian alloc] initWithCoordinatesX:77.54 Y:7.53 Z:6.22];
    PointCartesian *b1 = [[PointCartesian alloc] initWithCoordinatesX:26.99 Y:12.39 Z:11.18];
    CGFloat d = [PointCartesian shortestDistanceBetweenTwoSegmentsA0:a0 A1:a1 B0:b0 B1:b1];
    NSLog(@"%f", d);
    
    return NSApplicationMain(argc, argv);
}

output:

2020-06-26 17:57:30.762912+0200 Distance[2235:111429] 15.651394

The distance should be 15.826771412132246 according to the python code in link above.

You have to provide - a) input data for your function, b) expected output, c) definitions of `PointCartesian`, ..., In other words - [MRE](https://stackoverflow.com/help/minimal-reproducible-example) one can copy & paste, run and find the issue. — zrzka, Jun 26 '20 at 08:55
I'm not familiar with Python but translates `if d0 <= 0 >= d1` in Python to `if (d0 <= 0 && d0 >= d1)` or `if (d0 <= 0 && 0 >= d1)`? See [Determine Whether Integer Is Between Two Other Integers?](https://stackoverflow.com/questions/13628791/determine-whether-integer-is-between-two-other-integers). — Willeke, Jun 26 '20 at 09:40
@zrzka It is a huge project with dozens of class. I will make a smaller project with the issue in order to provide a MRE. — pamplemousse, Jun 26 '20 at 15:20
@Willeke Thanks for pointing that out, I corrected the code. — pamplemousse, Jun 26 '20 at 15:20
I rewrote the question to be a minimal reproducible example (MRE) — pamplemousse, Jun 26 '20 at 16:12
OKOK Everything works fine, the distances are correct. I was experiencing a problem with the clamp booleans that I did not set properly. So problem solved. — pamplemousse, Jun 27 '20 at 09:45

Distance between two line segments

0 Answers0